#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"
#include "include/Encoders.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::info);

  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);
//  this_thread::sleep_for(std::chrono::milliseconds(100));
//  GoToController::turn_degrees(-90, M_PI);


//  while (true) {
//    this_thread::sleep_for(std::chrono::milliseconds(10));
//    ClosedLoopMotorController::getInstance().set_power(0, 100);
//    spdlog::debug("{}", Encoders::getInstance().get_velocities_rad_s().at(0));
//  }

//  GoToController::drive_distance(2, 0.3);
//  std::this_thread::sleep_for(std::chrono::seconds(2));
//  GoToController::drive_distance(-2, 0.3);

//  while (true) {
//    GoToController::drive_distance(0.2, 0.4);
//    GoToController::turn_degrees(90, M_PI_2);
//
//    GoToController::drive_distance(0.2, 0.4);
//    GoToController::turn_degrees(90, M_PI_2);
//
//    GoToController::drive_distance(0.2, 0.4);
//    GoToController::turn_degrees(90, M_PI_2);
//
//    GoToController::drive_distance(0.2, 0.4);
//    GoToController::turn_degrees(90, M_PI_2);
//  }


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

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