#include <spdlog/spdlog.h>

#include "include/communication/MessageProcessor.hpp"
#include "include/communication/MessageBuilder.hpp"

#include "include/Encoders.hpp"
#include "include/IRSensors.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/GoToController.hpp"

using namespace CompLib;

google::protobuf::Message *MessageProcessor::process_message(const std::string &serializedMessage) {
  CompLib::GenericRequest message;
  message.ParseFromString(serializedMessage);
  auto messageTypeName = message.header().message_type();
  spdlog::debug("Got request with type {}", messageTypeName);

  try {
    // Encoder
    if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) {
      return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions());
    } else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) {
      return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rad_s());
    }

    // IR Sensors
    if (messageTypeName == IRSensorsEnableRequest::GetDescriptor()->full_name()) {
      IRSensors::enable();
      return MessageBuilder::default_successful_generic_response();
    } else if (messageTypeName == IRSensorsDisableRequest::GetDescriptor()->full_name()) {
      IRSensors::disable();
      return MessageBuilder::default_successful_generic_response();
    } else if (messageTypeName == IRSensorsReadAllRequest::GetDescriptor()->full_name()) {
      return MessageBuilder::ir_sensors_read_all_response(IRSensors::getInstance().read());
    }

    // Motors
    if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) {
      MotorsSetPowerRequest request;
      request.ParseFromString(serializedMessage);
      ClosedLoopMotorController::getInstance().set_power(request.port(), request.power());
      return MessageBuilder::default_successful_generic_response();
    } else if (messageTypeName == MotorsSetSpeedRequest::GetDescriptor()->full_name()) {
      MotorsSetSpeedRequest request;
      request.ParseFromString(serializedMessage);
      ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed());
      return MessageBuilder::default_successful_generic_response();
    }

    // Drive
    if (messageTypeName == DriveDistanceRequest::GetDescriptor()->full_name()) {
      DriveDistanceRequest request;
      request.ParseFromString(serializedMessage);
      GoToController::drive_distance(request.distance_m(), request.velocity_m_s());
      return MessageBuilder::default_successful_generic_response();
    } else if (messageTypeName == TurnDegreesRequest::GetDescriptor()->full_name()) {
      TurnDegreesRequest request;
      request.ParseFromString(serializedMessage);
      GoToController::turn_degrees(request.angle_degrees(), request.velocity_rad_s());
      return MessageBuilder::default_successful_generic_response();
    } else if (messageTypeName == DriveRequest::GetDescriptor()->full_name()) {
      DriveRequest request;
      request.ParseFromString(serializedMessage);
      GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s());
      return MessageBuilder::default_successful_generic_response();
    }
  } catch (const std::exception &ex) {
    spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what());
    google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what());
    return returnMessage;
  }

  spdlog::error("Message {} unknown!", messageTypeName);
  google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ERROR_MESSAGE_HEADER_UNKNOWN);
  return returnMessage;
}
