#include <spdlog/spdlog.h>
#include <unistd.h>

#include "include/reset.hpp"
#include "include/OdometryController.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/communication/UnixSocketServer.hpp"
#include "include/communication/TCPSocketServer.hpp"
#include "include/GoToController.hpp"

using namespace std;

int main(int argc, char *argv[]) {
  Reset::reset_robot();
  spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
  spdlog::set_level(spdlog::level::debug);

  UnixSocketServer unixSocketServer;
  TCPSocketServer tcpSocketServer;

//  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT);
//  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT);
//  OdometryController::getInstance().enable();

//  for (int i = 0; i < 10000; ++i) {
//    auto odom = OdometryController::getInstance().get();
//    spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
//    usleep(1000);
//  }

//  OdometryController::getInstance().enable();
//  GoToGoalController::diff_drive_inverse_kinematics(0.2, -M_PI_4);
//  for (int i = 0; i < 800; ++i) {
//    std::this_thread::sleep_for(std::chrono::milliseconds(10));
//    auto odom = OdometryController::getInstance().get();
//    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
//  }

//  GoToGoalController::go_to_point(2, 0, 0.2);
//  GoToController::drive_distance(-4, 0.2);
//  GoToController::turn_degrees(-90, M_PI_2);

//  GoToController::drive_distance(2, 0.2);
//  GoToController::turn_degrees(-90, M_PI_2);
//
//  GoToController::drive_distance(2, 0.2);
//  GoToController::turn_degrees(-90, M_PI_2);
//
//  GoToController::drive_distance(2, 0.2);
//  GoToController::turn_degrees(-90, M_PI_2);
//
//  GoToController::drive_distance(2, 0.2);
//  GoToController::turn_degrees(-90, M_PI_2);

  ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1]));

//  ClosedLoopMotorController::getInstance().generate_step_response_data();
//  ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
//  std::this_thread::sleep_for(std::chrono::hours(12));
  return 0;
}