/*
  Copyright 2025
              Thibaut Ferrand / Ulysse Cura
                                           */

#include <Adafruit_SSD1306.h>
#include <CodeCell.h>
#include <ESP32Servo.h>
#include <cmath>
#include "Ressources.hpp"

#define PIN_TIRETTE 5
#define PIN_BUTTON_COLOR 6
#define PIN_MOTOR1 1
#define PIN_MOTOR2 2
#define PIN_SERVO 7

#define DANCING_ACTION_DELTA_ANGLE 2

#define GAIN_KD 50
#define ANGULAR_SPEED 120 // °/s

using std::abs, std::signbit;

enum class Axes {
  X,
  Y,
  Z
};

class Motor {
 public:
  int init(int pin, int channel)
  {
    m_pin = pin;
    m_channel = channel;

    pinMode(m_pin, OUTPUT);

    ledcAttachChannel(m_pin, 5000, 12, m_channel);
    ledcWriteChannel(m_channel, 0);

    return 0;
  }

  void setSpeed(int speed)
  {
    if(speed > 4095) speed = 4095;

    ledcWriteChannel(m_channel, speed);
  }

 private:
  int m_pin;
  int m_channel;
};

class IO {
 public:
  IO(CodeCell *code_cell) : m_code_cell(code_cell)
  {}

  int init()
  {
    int nb_errors {0};

    pinMode(PIN_TIRETTE, INPUT_PULLUP);
    pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);

    initGyroscope();

    m_initMotors();
    m_initServo();

    nb_errors += m_initScreen();

    if(!nb_errors)
    {
      m_screen.setTextSize(1);
      m_screen.setTextColor(WHITE, BLACK);
      m_screen.setFont(&Res::Fonts::Freshman12pt7b);
    }

    return nb_errors;
  }

  void initGyroscope()
  {
    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
  }

  int update()
  {
    static unsigned long last_color_change_time {0};
    static bool has_color_changed {true};
    static bool is_color_blue {true};

    if(m_is_color_blue != is_color_blue)
    {
      m_screen.clearDisplay();
      m_screen.setCursor(0, 20);

      if(is_color_blue)
      {
        m_screen.print("Blue\nTeam");
      }
      else
      {
        m_screen.print("Yellow\nTeam");
      }

      last_color_change_time = millis();

      has_color_changed = true;
    }

    if(millis() - last_color_change_time > 1000 && has_color_changed)
    {
      m_screen.clearDisplay();
      m_screen.drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
      //m_screen.drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
      //m_screen.drawBitmap(27, 0, Res::Imgs::diable_gaga, 75, 64, WHITE);

      has_color_changed = false;
    }

    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == HIGH);
    m_is_color_blue = is_color_blue;

    is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == LOW);

    if(m_is_motor_control_activated)
    {
      m_updateMotorsControl();
    }

    if(m_is_dancing)
    {
      m_updateDancingAction();
    }

    m_updateScreen();

    return 0;
  }

  bool isTirettePulled()
  {
    return m_is_tirette_pulled;
  }

  bool isSelectedColorBlue()
  {
    return m_is_color_blue;
  }

  void motorControlOn()
  {
    m_is_motor_control_activated = true;
  }

  void motorControlOff()
  {
    m_is_motor_control_activated = false;

    m_motors[0].setSpeed(0);
    m_motors[1].setSpeed(0);
  }

  void setDirWithAngularSpeed(float dir, float angular_speed = ANGULAR_SPEED)
  {
    static unsigned long prev_time {millis()};
    float err_dir = dir - m_dir;
    float angular_displacement = angular_speed * static_cast<float>(millis() - prev_time) / 1000.0f;

    if(abs(err_dir) < abs(angular_displacement))
    {
      m_dir = dir;
    }
    else
    {
      m_dir += angular_displacement * ((!signbit(err_dir)) * 2 - 1);
    }

    prev_time = millis();
  }

  void setDir(float dir)
  {
    m_dir = dir;
  }

  void setSpeed(float speed)
  {
    m_speed = speed;
  }
  
  float getAngle(Axes axis)
  {
    float x, y, z;
    float angle;

    m_code_cell->Motion_RotationRead(x, y, z);

    switch(axis)
    {
     case Axes::X:
      angle = x - m_init_x;
      if(angle < -180) angle += 360;
      if(angle > 180) angle -= 360;
      return angle;

     case Axes::Y:
      angle = y - m_init_y;
      if(angle < -180) angle += 360;
      if(angle > 180) angle -= 360;
      return angle;

     default:
      angle = z - m_init_z;
      if(angle < -180) angle += 360;
      if(angle > 180) angle -= 360;
      return angle;
    }
  }

  void startDancingAction(int dancing_action_delta_angle = DANCING_ACTION_DELTA_ANGLE)
  {
    m_dancing_action_delta_angle = dancing_action_delta_angle;
    m_is_dancing = true;
  }

  Adafruit_SSD1306 *getScreen()
  {
    return &m_screen;
  }

  Motor *getMotor(int motor)
  {
    return &(m_motors[motor]);
  }

 private:
  // Init Motors
  void m_initMotors()
  {
    m_motors[0].init(PIN_MOTOR1, 0);
    m_motors[1].init(PIN_MOTOR2, 1);
  }

  // Init Servo
  void m_initServo()
  {
    m_servo.setPeriodHertz(50);
    m_servo.attach(PIN_SERVO, 500, 2400);

    m_servo.write(87);
  }

  // Init Screen
  int m_initScreen()
  {
    if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
    {
      Serial.println("SSD1306 allocation failed.");
      return -1;
    }

    m_screen.fillScreen(SSD1306_BLACK);

    return 0;
  }

  void m_updateDancingAction()
  {
    static unsigned long prev_time {millis()};
    static unsigned long delta_time {0};
    static int actual_angle {87};
    static int8_t actual_dir {-1};

    delta_time += millis() - prev_time;

    if(delta_time >= 10)
    {
      delta_time = 0;

      actual_angle += actual_dir * m_dancing_action_delta_angle;
    }

    if(actual_angle <= 43)
    {
      actual_dir = 1;
    }
    else if(actual_angle >= 130)
    {
      actual_dir = -1;
    }

    m_servo.write(actual_angle);

    prev_time = millis();
  }

  void m_updateMotorsControl()
  {
    float actual_angle = getAngle(Axes::Z);
    float error = m_dir - actual_angle;

    float correction = error * GAIN_KD;

    int m1_speed = static_cast<int>(m_speed + correction);
    int m2_speed = static_cast<int>(m_speed - correction);

    if(m1_speed < 0)
    {
      m1_speed = 0;
      //m2_speed = static_cast<int>(m_speed - 2 * correction);
    }
    else if(m2_speed < 0)
    {
      m2_speed = 0;
      //m1_speed = static_cast<int>(m_speed + 2 * correction);
    }

    m_motors[0].setSpeed(m1_speed);
    m_motors[1].setSpeed(m2_speed);
  }

  void m_updateScreen()
  {
    m_screen.display();
  }

  bool m_is_motor_control_activated {false};
  bool m_is_tirette_pulled {false};
  bool m_is_color_blue {false};
  bool m_is_dancing {false};

  Motor m_motors[2];
  Servo m_servo;

  Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};

  CodeCell *m_code_cell;

  float m_init_x, m_init_y, m_init_z;
  float m_dir {0};
  float m_speed {0};
  int m_dancing_action_delta_angle {DANCING_ACTION_DELTA_ANGLE};
};