#include "include/communication/MessageBuilder.hpp"

using namespace CompLib;

CompLib::Header *MessageBuilder::header(const std::string &message_type) {
  auto header = new CompLib::Header();
  header->set_message_type(message_type);
  return header;
}

CompLib::Status *MessageBuilder::status(bool successful, const std::string &error_message) {
  auto status = new CompLib::Status();
  status->set_successful(successful);
  status->set_error_message(error_message);
  return status;
}

CompLib::GenericResponse *MessageBuilder::generic_response(bool successful, const std::string &error_message) {
  auto genericResponse = new CompLib::GenericResponse();
  genericResponse->set_allocated_header(header(CompLib::GenericResponse::descriptor()->full_name()));
  genericResponse->set_allocated_status(status(successful, error_message));
  return genericResponse;
}

CompLib::Status *MessageBuilder::default_successful_status() {
  return status(true, "");
}


CompLib::GenericResponse *MessageBuilder::default_successful_generic_response() {
  return generic_response(true, "");
}

CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_response(std::array<int32_t, ROBOT_MOTOR_COUNT> positions) {
  auto response = new EncoderReadPositionsResponse();
  response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name()));
  response->set_allocated_status(default_successful_status());
  for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
    response->set_positions(i, positions.at(i));
  }
  return response;
}

CompLib::EncoderReadVelocitiesResponse *MessageBuilder::encoder_read_velocities_response(std::array<double, ROBOT_MOTOR_COUNT> velocities) {
  auto response = new EncoderReadVelocitiesResponse();
  response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name()));
  response->set_allocated_status(default_successful_status());
  for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
    response->set_velocities(i, velocities.at(i));
  }
  return response;
}

CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response(std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> data) {
  auto response = new IRSensorsReadAllResponse();
  response->set_allocated_header(header(IRSensorsReadAllResponse::descriptor()->full_name()));
  response->set_allocated_status(default_successful_status());
  for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) {
    response->set_data(i, data.at(i));
  }
  return response;
}