#include <spdlog/spdlog.h>

#include "include/GoToController.hpp"
#include "include/Robot.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/OdometryController.hpp"
#include "include/mathUtils.hpp"

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

// Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities
// Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code
void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) {
  auto vr_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
  auto vl_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;

  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s);
  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
}

// +dist, +vms -> forward
// -dist, +vms -> backward
// +dist, -vms -> backward
// -dist, -vms -> backward
void GoToController::drive_distance(double distance_m, double v_m_s) {
  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ);

  if (distance_m < 0 || v_m_s < 0) {
    distance_m = -fabs(distance_m);
    v_m_s = -fabs(v_m_s);
  }

  auto was_enabled = OdometryController::getInstance().is_enabled();
  OdometryController::getInstance().enable();
  auto start_x = OdometryController::getInstance().get().get_x_position();
  auto end_x = start_x + distance_m;

  auto distance_to_target = distance_m;
  while (fabs(distance_to_target) > ROBOT_GO_TO_DISTANCE_END_M) {
    std::this_thread::sleep_for(sleep_duration);
    auto current_pos = OdometryController::getInstance().get();
    distance_to_target = end_x - current_pos.get_x_position();
    auto alpha = -mathUtils::wrap_angle_to_pi(current_pos.get_angular_orientation());

    auto vel_mult = mathUtils::limit_max(distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P, -1.0, 1.0);
    auto adjusted_speed = mathUtils::limit_min(fabs(v_m_s) * vel_mult, ROBOT_GO_TO_DISTANCE_MIN_VEL);
    diff_drive_inverse_kinematics(adjusted_speed, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P);

    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
                  current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
                  distance_to_target,
                  alpha, vel_mult, adjusted_speed
    );
  }

  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);

  std::this_thread::sleep_for(std::chrono::milliseconds(50));

  ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
  ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);

  if (!was_enabled) {
    OdometryController::getInstance().disable();
  }
}

// +ang, +rad -> cw
// -ang, +rad -> ccw
// +ang, -rad -> ccw
// -ang, -rad -> ccw
void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
  auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ);

  if (angle_rad < 0 || v_rad_s < 0) {
    angle_rad = -fabs(angle_rad);
    v_rad_s = -fabs(v_rad_s);
  }

  auto was_enabled = OdometryController::getInstance().is_enabled();
  OdometryController::getInstance().enable();

  auto start_angle = OdometryController::getInstance().get().get_angular_orientation();
  auto end_angle = mathUtils::wrap_angle_to_pi(start_angle + angle_rad);
  auto angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - start_angle);
  while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) {
    std::this_thread::sleep_for(sleep_duration);
    auto current_angle = OdometryController::getInstance().get().get_angular_orientation();
    angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - current_angle);

    if (fabs(angle_to_target) > M_PI_2) {
      diff_drive_inverse_kinematics(0.0, v_rad_s);
      spdlog::debug("TURN: {:03.4f}; {:03.4f};", current_angle, angle_to_target);
    } else {
      auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0);
      auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL);
      diff_drive_inverse_kinematics(0.0, adjusted_speed);
      spdlog::debug("TURN: {:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed);
    }
  }

  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);

  std::this_thread::sleep_for(std::chrono::milliseconds(50));

  ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
  ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);

  if (!was_enabled) {
    OdometryController::getInstance().disable();
  }
}
