#pragma once

#include "autoopt/parameters.hpp"
#include "autoopt/quadric.hpp"

namespace autoopt {

struct parabola_new : curve_base {
  // T focal_length;
  parabola_new() : curve_base() {
    initial = Eigen::VectorXd::Zero(3);
    delta = Eigen::VectorXd::Zero(3);
    param_names =
        std::vector<const char*>{"focus", "entrance angle", "rotation"};
  }

  template <typename T>
  quadric<T> grapher_from_params(const Eigen::VectorX<T>& params) const {
    T exit_arm = params(0);
    T entrance_angle = params(1);
    T alpha = params(2);

    T x = exit_arm * std::sin(T{2.0} * entrance_angle);
    T f = T{0.5} * (T{1.0} - std::cos(T{2.0} * entrance_angle)) * exit_arm;
    T a = T{1.0} / (T{4.0} * f);
    T y = a * x * x;
    quadric<T> q = quadric(a, T{0}, T{0}, T{0}, T{-1.0}, T{0});
    return q.translated_by(-x, -y).rotated_by(entrance_angle - T{M_PI_2} +
                                              alpha);
  }
};

}  // namespace autoopt