/*
  Copyright 2025
              Thibaut Ferrand / Ulysse Cura
                                           */

#include "esp32-hal.h"
#include <CodeCell.h>
#include <ESP32Servo.h>
#include <Adafruit_SSD1306.h>
#include <cmath>

#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 100
#define ANGULAR_SPEED 10 // °/s

using std::abs;

enum class Axes {
  X,
  Y,
  Z
};

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

    pinMode(m_pin, OUTPUT);

    analogWriteFrequency(m_pin, 5000);
    analogWriteResolution(m_pin, 12);

    analogWrite(m_pin, 0);

    return 0;
  }

  void setSpeed(int speed)
  {
    analogWrite(m_pin, 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);

    m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);

    m_initMotors();
    m_initServo();

    nb_errors += m_initScreen();

    return nb_errors;
  }

  int update()
  {
    m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
    m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);

    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() / 1000 - prev_time / 1000);

    Serial.printf("Angular Speed : %d\nStatic Cast : %d\nprev_time : %d\n millis : %d\n", angular_speed, static_cast<float>(millis() / 1000 - prev_time / 1000), prev_time, millis());

    if(abs(err_dir) < angular_displacement)
    {
      m_dir = dir;
    }
    else
    {
      m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
    }

    Serial.printf("Angle : %d\nAngular Displacement : %d\n", m_dir, angular_displacement);

    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;

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

    switch(axis)
    {
     case Axes::X:
      return x - m_init_x;

     case Axes::Y:
      return y - m_init_y;

     default:
      return z - m_init_z;
    }
  }

  void startDancingAction()
  {
    m_is_dancing = true;
  }

  Adafruit_SSD1306 *getScreen()
  {
    return &m_screen;
  }

  Motor m_motors[2];

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

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

    m_servo.write(87);
  }

  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 * 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};

  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};
};