#include <gtest/gtest.h>

#include <autoopt/parabola_new.hpp>
#include <print>
#include "autoopt/util.hpp"


using namespace autoopt;


TEST(New, Parabola) {
  parabola_new parab;
  parab.flip = false;
  parab.initial(0) = 50.0;
  parab.initial(1) = autoopt::deg2rad(1.0);
  parab.initial(2) = autoopt::deg2rad(0.05);

  parab.delta(0) = 10.0;
  parab.delta(1) = autoopt::deg2rad(0.1);
  parab.delta(2) = autoopt::deg2rad(0.1);

  test_data td;
  td._xs = Eigen::VectorXd(21);
  td._ys = Eigen::VectorXd(21);
  td._center = 10.0;

  for (size_t i = 0; i < 21; i++) {
    td._xs(i) = static_cast<double>(i);
  }

  auto res = parab.plot(td);

  for (auto y : res) {
    std::println("y: {}", y);
  }

  td._ys = res;

  parab.initial(0) = 54.0;
  parab.initial(1) = autoopt::deg2rad(1.05);

  auto [fit, rms] = parab.fit(td);

  for (auto y : fit) {
    std::println("fit: {}", y);
  }

  std::println("RMS: {}", rms);
}