#include <gtest/gtest.h>

#include <autoopt/ellipse.hpp>
#include <autoopt/optimization_problem.hpp>
#include <autoopt/util.hpp>
#include <autoopt/btls.hpp>
#include <fstream>
#include <unordered_map>
#include <autoopt/interface.hpp>
#include <iomanip>


using namespace autoopt;

std::vector<std::pair<double, double>> read_data(const std::string& filename) {
  std::fstream file(filename, std::ios::in);

  std::unordered_map<double, size_t> index_map;
  std::vector<double> x_values;
  std::vector<std::vector<double>> data_points;

  std::string line;

  size_t index = 0;

  double x_avg = 0.0;

  for(;std::getline(file, line);) {
    if (line.empty() || line[0] == '#') {
      continue;
    }
    std::istringstream iss(line);
    double x, y;
    iss >> x;
    for (size_t i = 1; i < 5; ++i) {
      iss >> y;  // skip unused columns
    }

    y = arcsec2rad(-y);

    if (index_map.find(x) == index_map.end()) {
      index_map[x] = index++;
      x_values.push_back(x);
      data_points.emplace_back();
      data_points.back().push_back(y);
      x_avg += x;
      continue;
    }
    data_points[index_map[x]].push_back(y);
  }
  x_avg /= x_values.size();
  std::vector<std::pair<double, double>> result;
  for (size_t i = 40; i < x_values.size() - 40; ++i) {
    double x = x_values[i];
    double y_avg = 0.0;
    for (double y : data_points[i]) {
      y_avg += y;
    }
    y_avg /= data_points[i].size();
    result.emplace_back(x - x_avg, y_avg);
  }
  return result;
}

TEST(Fit, EllipseFit) {
  auto data_points = read_data("tests/input/ellipse.dat");

  std::cout << "Read " << data_points.size() << " data points." << std::endl;

  auto mid_point = data_points[data_points.size() / 2];

  std::cout << "Mid point: (" << mid_point.first << ", " << mid_point.second << ")" << std::endl;

  Eigen::VectorX<double> initial_params(4);
  initial_params << 6900.0, 500.0, deg2rad(2.0), mid_point.second;
  
  Eigen::VectorX<double> delta(4);
  delta << 10.0, 10.0, deg2rad(0.1), deg2rad(0.1);

  auto res = fit_ellipse(data_points, initial_params, delta);
  std::cout << res.transpose() << std::endl;
}