#pragma once

#include <autoopt/derivative.hpp>
#include <cmath>

namespace autoopt {

template <typename T>
struct quadric {
  T _A;  // x² coefficient
  T _B;  // xy coefficient
  T _C;  // y² coefficient
  T _D;  // x coefficient
  T _E;  // y coefficient
  T _F;  // constant term

  constexpr quadric(T A = 0, T B = 0, T C = 0, T D = 0, T E = 0, T F = 0)
      : _A(A), _B(B), _C(C), _D(D), _E(E), _F(F) {}

  constexpr quadric rotated_by(T angle_rad) const {
    T co = std::cos(angle_rad);
    T si = std::sin(angle_rad);
    T co_2 = co * co;
    T si_2 = si * si;
    T co_si = co * si;

    T A_new = _A * co_2 - _B * co_si + _C * si_2;
    T B_new = T{2} * (_A - _C) * co_si + _B * (co_2 - si_2);
    T C_new = _A * si_2 + _B * co_si + _C * co_2;
    T D_new = _D * co - _E * si;
    T E_new = _D * si + _E * co;
    T F_new = _F;

    return quadric(A_new, B_new, C_new, D_new, E_new, F_new);
  }

  constexpr quadric translated_by(T x_o, T y_o) const {
    T D_new = _D - T{2} * _A * x_o - _B * y_o;
    T E_new = _E - _B * x_o - T{2} * _C * y_o;
    T F_new = _F + _A * x_o * x_o + _B * x_o * y_o + _C * y_o * y_o - _D * x_o -
              _E * y_o;
    return quadric(_A, _B, _C, D_new, E_new, F_new);
  }

  constexpr T at(T x) const {
    T sign = T{-1};
    T Bx_E = _B * x + _E;
    T Ax2_Dx_F = _A * x * x + _D * x + _F;

    if (_C == T{0}) {
      return -Ax2_Dx_F / Bx_E;
    }

    T discriminant = Bx_E * Bx_E - T{4} * _C * Ax2_Dx_F;
    T num = sign * std::sqrt(discriminant) - Bx_E;
    T denom = T{2} * _C;
    return num / denom;
  }

  constexpr T slope_at(T x) const {
    return derivative(
        [&]<typename U>(U x_val) {
          quadric<U> q{U{_A}, U{_B}, U{_C}, U{_D}, U{_E}, U{_F}};
          return q.at(x_val);
        },
        x);
  }
};

}  // namespace autoopt