#include <cmath>

#include "include/OdometryController.hpp"
#include "include/Robot.hpp"
#include "include/Encoders.hpp"
#include <spdlog/spdlog.h>
#include "include/mathUtils.hpp"

#define MS_IN_S 1000
#define US_IN_S (1000 * MS_IN_S)

bool OdometryController::is_enabled() const {
  return enabled;
}

void OdometryController::enable() {
  last_run = clock::now();
  enabled = true;
}

void OdometryController::disable() {
  spdlog::debug("OdometryController::disable()");
  enabled = false;
  OdometryController::reset();
}

void OdometryController::reset() {
  std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
  current_odometry = Odometry();
  last_run = clock::now();
}

Odometry OdometryController::get() {
  return current_odometry;
}


OdometryController::OdometryController() {
  odometry_thread = std::thread(&OdometryController::odometry_loop, this);
  odometry_thread.detach();
}

[[noreturn]] void OdometryController::odometry_loop() {
  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ);
  while (true) {
    std::this_thread::sleep_for(sleep_duration);
    std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
    if (enabled) {
      last_run = clock::now();
      auto encoder_positions = Encoders::getInstance().get_positions();

      auto current_position_left =
          encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
      auto current_position_right =
          encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
      auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
      auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER;
      last_position_left = current_position_left;
      last_position_right = current_position_right;

      // The section below implements differential drive kinematics.
      // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin
      //    or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart
      //    and Illah R. Nourbakhsh
      auto v = (distance_right + distance_left) / 2.0;
      auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M;

      auto curr_x = current_odometry.get_x_position();
      auto curr_y = current_odometry.get_y_position();
      auto curr_theta = current_odometry.get_angular_orientation();

      auto x = v * cos(w);
      auto y = v * sin(w);

      auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y);
      auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y);
      auto new_theta = curr_theta + w;

      new_theta = mathUtils::wrap_angle_to_pi(new_theta);
      current_odometry = Odometry(new_x, new_y, new_theta);


      spdlog::debug("ODOM: {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
                    current_position_left, current_position_right,
                    distance_left, distance_right,
                    v, w);
    }
  }
}
