#include <iostream>
#include <CompLib.pb.h>
#include <sys/un.h>
#include <sys/socket.h>
#include <zconf.h>
#include "include/mathUtils.hpp"
#include "include/messageBuilder.hpp"
#include "include/errorMessages.hpp"
#include <spdlog/spdlog.h>
#include <chrono>
#include <algorithm>

#include "include/spi.hpp"
#include "include/reset.hpp"
#include "include/encoder.hpp"
#include "include/motor.hpp"
#include "include/robot.hpp"

#define SOCKET_PATH "/tmp/compLib"
#define BUFFER_SIZE 64

google::protobuf::Message *processMessage(const std::string &serializedMessage) {
  CompLib::GenericRequest message;
  message.ParseFromString(serializedMessage);
  auto messageTypeName = message.header().message_type();

  if (messageTypeName == CompLib::ReadSensorsRequest::GetDescriptor()->full_name()) {
    CompLib::ReadSensorsRequest readSensorsRequest;
    readSensorsRequest.ParseFromString(serializedMessage);
  } else if (messageTypeName == CompLib::ReadSensorsResponse::GetDescriptor()->full_name()) {
    CompLib::ReadSensorsResponse readSensorsResponse;
    readSensorsResponse.ParseFromString(serializedMessage);
    std::cout << readSensorsResponse.ir_1() << std::endl;
    std::cout << readSensorsResponse.ir_2() << std::endl;
    std::cout << readSensorsResponse.ir_3() << std::endl;
    std::cout << readSensorsResponse.ir_4() << std::endl;
    std::cout << readSensorsResponse.ir_5() << std::endl;
  } else {
    std::cout << messageTypeName << " not found!" << std::endl;
  }

  google::protobuf::Message *returnMessage = MessageBuilder::genericResponse(false, ERROR_MESSAGE_UNKNOWN);
  return returnMessage;
}

[[noreturn]] void socketServer() {
  struct sockaddr_un socketAddress;
  int socketFileDescriptor = socket(AF_UNIX, SOCK_STREAM, 0);
  remove(SOCKET_PATH);

  memset(&socketAddress, 0, sizeof(struct sockaddr_un));
  socketAddress.sun_family = AF_UNIX;
  strncpy(socketAddress.sun_path, SOCKET_PATH, sizeof(socketAddress.sun_path) - 1);

  if (bind(socketFileDescriptor, (struct sockaddr *) &socketAddress, sizeof(struct sockaddr_un)) == -1) {
    exit(-1);
  }

  if (listen(socketFileDescriptor, 1) == -1) {
    exit(-2);
  }

  char readBuffer[BUFFER_SIZE];
  char writeBuffer[BUFFER_SIZE];
  for (;;) {
    int clientFileDescriptor = accept(socketFileDescriptor, NULL, NULL);
    auto numRead = read(clientFileDescriptor, readBuffer, 1);
    std::cout << numRead << std::endl;

    uint8_t messageSize = readBuffer[0];
    std::cout << std::to_string(messageSize) << std::endl;

    numRead = read(clientFileDescriptor, readBuffer, readBuffer[0]);
    std::cout << numRead << std::endl;

    std::string stringMessage;
    for (int i{}; i < messageSize; i++) {
      stringMessage += readBuffer[i];
    }

    auto response = processMessage(stringMessage);
    uint8_t responseSize = response->ByteSizeLong();
    writeBuffer[0] = responseSize;

    std::cout << std::to_string(responseSize) << std::endl;
    write(clientFileDescriptor, writeBuffer, 1);

    response->SerializeToArray(writeBuffer, BUFFER_SIZE);
    write(clientFileDescriptor, writeBuffer, responseSize);

    close(clientFileDescriptor);
  }
}

int main() {
  // socketServer();
  Reset::reset_robot();
  spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
  // Spi &spi = Spi::getInstance();
  // int version = spi.read(1, 1);

  // double count = 20000;
  // double start_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
  // for (int i{0}; i < count; i++){
  //     spi.read(1, 1);
  // }
  // double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

  // double kp = 0.5;
  // double ki = 5.0;
  // double kd = 0.025;
  double setpoint = 75;
  // double errror_sum = 0;
  // double last_error = 0;
  // int port = 0;
  // long double accumulator = 0;
  // long double alpha = 0.2;
  // auto last_time = std::chrono::high_resolution_clock::now();
  // auto start_time = last_time;
  // int i = 0;
  while (1 == 1) {
    // setpoint += 0.5;
    // auto current_time = std::chrono::high_resolution_clock::now();
    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
    // long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count() / 1000000.0;

    // last_time = current_time;

    // auto speeds = Motor::get_instance().get_speed();
    // double e = setpoint - speeds.at(0);
    // errror_sum += e * delta_seconds;
    // double error_diff = (e - last_error) / delta_seconds;
    // last_error = e;
    // double u = kp * e + errror_sum * ki + error_diff * kd;

    // Motor::power(0, std::min(std::max(-100.0, u), 100.0));

    // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));

    // usleep(1000.0 * (1000.0 / 10.0));
    // Motor::get_instance().set_speed(0, -setpoint);
    // Motor::get_instance().set_speed(3, setpoint);
    // i += 1;
    // if (i % 1000 == 0) {
    //   setpoint *= -1;
    // }
    // auto speeds = Motor::get_instance().get_speed();
    // spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint);

    // auto current_time = std::chrono::high_resolution_clock::now();
    // long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
    // last_time = current_time;

    // uint8_t result_bytes[2] = {0};
    // Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, 2, result_bytes);
    // int16_t tps = mathUtils::int_from_bytes(result_bytes, 2);
    // std::vector<int32_t> current_ticks = Encoder::read_all();

    // long double delta_ticks = current_ticks.at(0) - last_ticks;

    // spdlog::info("{} {}", tps, delta_ticks / delta_seconds);

    // last_ticks = current_ticks.at(0);
  }
  return 0;
}