#pragma once

#include "autoopt/quadric.hpp"

namespace autoopt
{
  
template <typename T>
struct parabola {
  // T focal_length;
  T exit_arm;
  T entrance_angle;

  quadric<T> to_quadric() const {
    // T a = T{1} / (T{4} * focal_length);
    // T x = T{2} * focal_length / std::tan(entrance_angle);
    // T y = a * x * x;
    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});
  }
};

} // namespace autoopt