#include <Arduino.h>
// #include <ESP32_PWM.h>
// #include <string>

// using namespace std;


//RF1: SPEED_UP, SPEED_DOWN, STOP_REVERSE, FORWARD;

//RF1: TURN_LEFT, TURN_RIGHT, ZERO_TURN_LEFT, ZERO_TURN_RIGHT;

//NOTE: 0-set speed acceleaction, max speed
#define FORWARD_INPUT  27
#define REVERSE_INPUT 26
#define SPEED_UP_INPUT 25 
#define SLOW_DOWN_INPUT 19 //no good: used for SDA

// #define STOP_INPUT  21
#define START_STOP_INPUT 13
#define ZERO_TURN_INPUT 21//no good: RTC
#define TURN_LEFT_INPUT 32 
#define TURN_RIGHT_INPUT 33

#define ACCELERATION 1

// these values should be less than 100
#define max_speed_forward 200
#define max_speed_reverse 80
#define max_turn_speed 80

#define F_PWM_M1 16
#define R_PWM_M1 18
#define F_PWM_M2 4
#define R_PWM_M2 17

// #define M_ENABLE 17 // no good: I2C bus
#define M_ENABLE 23 //
#define SOFT_START_BYPASS_ENABLE 22
#define SPEED_UP 20
#define DELAY_TIME 150
#define TOGGLE_DELAY 500
// #define MOVEMENT_DELAY 300
#define SOFT_STOP_DELAY 500

#define frequency 25000
#define resolution 8


// #define HW_TIMER_INTERVAL_US      100L
// You can assign any interval for any timer here, in Hz
// float PWM_Freq1   = 100.0f;

// You can assign any interval for any timer here, in microseconds
// uint32_t PWM_Period1 = 1000000 / PWM_Freq1;

// // Init ESP32 timer 1
// ESP32Timer ITimer(1);

// // Init ESP32_ISR_PWM
// ESP32_PWM ISR_PWM;

// bool IRAM_ATTR TimerHandler(void * timerNo)
// {
//   ISR_PWM.run();

//   return true;
// };
// Configure the motor driver.
// Static CytronMD Vehicle(PWM_PWM, F_PWM_M1, R_PWM_M1);   // PWM 1A = Pin 3, PWM 1B = Pin 9.
// Static CytronMD Vehicle(PWM_PWM, F_PWM_M2, R_PWM_M2); // PWM 2A = Pin 10, PWM 2B = Pin 11.

class Controller{
  private: 
   short int F_PWM_OUTPUT_1;
   short int R_PWM_OUTPUT_1;
   short int F_PWM_Channel_1;
   short int R_PWM_Channel_1;

   short int F_PWM_OUTPUT_2;
   short int R_PWM_OUTPUT_2;
   short int F_PWM_Channel_2;
   short int R_PWM_Channel_2;

  public:
    short int ENABLE_OUTPUT_PIN;
    volatile int map ;
    volatile short int current_speed =0;
    volatile bool is_busy = false;
    String current_direction = "stop";
    volatile bool zero_turn = false;

    void changeState( bool acc, String new_direction, short int new_speed = 0){
        // current_speed = current_speed == 0 ? SPEED_UP*2 : current_speed;
        is_busy = true;
        short int initial_speed = 0;
        if(new_speed && new_speed > current_speed){
          initial_speed = current_speed;
          current_speed = new_speed;
        } else if (new_speed && new_speed < current_speed) {
          initial_speed = new_speed;
        };
        static bool is_stopped = false;
        if (new_direction == "stop")
        {
          new_direction = current_direction;
          acc = 0;
          is_stopped = true;
        } else is_stopped = false;

        if (new_direction == "speed_up" || new_direction == "slow_down")
        {
          new_direction = current_direction;
        }

        Serial.println("Direction :: Speed ::"
                        +new_direction+" :: "
                        +String(initial_speed)+" :: "
                        +String(current_speed));
        for(
            int i = acc ? initial_speed : current_speed;
                    acc ? i<current_speed : i> initial_speed;
                    acc ? i+=ACCELERATION : i-=ACCELERATION
            ){
          Serial.println("motor speed::"+String(i));
           short int F_M1_Speed;
           short int R_M1_Speed;
           short int F_M2_Speed;
           short int R_M2_Speed;
          if(i == ACCELERATION && is_stopped) {
            // digitalWrite(M_ENABLE, LOW);
            current_speed = 0;
            i = 0;
            break;
          };

          new_direction == "forward" || new_direction == "turn_right" ? writePWM(F_PWM_OUTPUT_1, i) : writePWM(F_PWM_OUTPUT_1, 0);
          new_direction == "reverse" || new_direction == "turn_left"  ? writePWM(R_PWM_OUTPUT_1, i) : writePWM(R_PWM_OUTPUT_1, 0);
          new_direction == "forward" || new_direction == "turn_left" ? writePWM(F_PWM_OUTPUT_2, i) : writePWM(F_PWM_OUTPUT_2, 0);
          new_direction == "reverse" || new_direction == "turn_right" ? writePWM(R_PWM_OUTPUT_2, i) : writePWM(R_PWM_OUTPUT_2, 0);
        };
        is_busy = false;
        if (new_direction
            && new_direction != "stop"
            && new_direction != current_direction
           ) current_direction = new_direction;
        current_speed = new_speed;
        softStartBypass();
      return;
    };

    void softStop(){
      if(current_speed > 0){
        is_busy = true;
            changeState(0, "stop");
        is_busy = false;
        digitalWrite(SOFT_START_BYPASS_ENABLE, LOW);
      return;
      };
    };

    void forward(){

      if(!is_busy ){
          is_busy = true;
          changeState(1, "forward", current_speed == 0 ? SPEED_UP*5 : current_speed);
          is_busy = false;
      return;
      };
      
    };

    void reverse(){
      if(!is_busy ){
          is_busy = true;
          changeState(1, "reverse", max_speed_reverse);
          is_busy = false;  
      return;
        }
    };

    void speedUp(){
      uint8_t new_speed = current_speed+SPEED_UP <= max_speed_forward ? current_speed+SPEED_UP : current_speed;
      if(new_speed - current_speed > 0 && !is_busy) changeState(1, "speed_up", new_speed);
      return;
        
    };

    void slowDown(){
      uint8_t new_speed = current_speed-SPEED_UP > 0 ? current_speed-SPEED_UP : current_speed;
      if(current_speed - new_speed > 0 && !is_busy) changeState(0, "slow_down", new_speed);
      return;
        
    };

  void soft_turn_pwm (short int pwm) {

  }

  void soft_turn (String turn_direction){
    const short int intial_speed = current_speed;
    if(current_direction == "forward" || current_direction == "reverse" && intial_speed - SPEED_UP >0) {
      short int fast_turn_speed = intial_speed+(SPEED_UP*3) < 220 ? intial_speed+(SPEED_UP*3) : intial_speed;
      short int slow_turn_speed = intial_speed-(SPEED_UP) > 0 ? intial_speed-(SPEED_UP) : intial_speed;
      while(
        digitalRead(TURN_LEFT_INPUT) || digitalRead(TURN_RIGHT_INPUT) 
      )
      {
          if( current_direction == "forward" ){
            writePWM(F_PWM_M1, turn_direction == "left" ? slow_turn_speed : fast_turn_speed);
            writePWM(R_PWM_M1, 0);
            writePWM(F_PWM_M2, turn_direction == "left" ? fast_turn_speed : slow_turn_speed);
            writePWM(R_PWM_M2, 0);
          } else if(current_direction == "reverse") {

            writePWM(F_PWM_M1, 0);
            writePWM(R_PWM_M1, turn_direction == "left" ? slow_turn_speed : fast_turn_speed);
            writePWM(F_PWM_M2, 0);
            writePWM(R_PWM_M2, turn_direction == "left" ? fast_turn_speed : slow_turn_speed);

          }
    };
      if(current_direction == "forward"){

              writePWM(F_PWM_M1, intial_speed);
              writePWM(R_PWM_M1, 0);
              writePWM(F_PWM_M2, intial_speed);
              writePWM(R_PWM_M2, 0);
      } else if(current_direction == "reverse") {
              writePWM(F_PWM_M1, 0);
              writePWM(R_PWM_M1, intial_speed);
              writePWM(F_PWM_M2, 0);
              writePWM(R_PWM_M2, intial_speed);
      };
    };
  };  

  void zero_turn_left(){
    if(!is_busy ){
      // current_speed = max_turn_speed;
        is_busy = true;
        changeState(1, "turn_left", max_turn_speed);
        is_busy = false;  
      return;
      }
    };

    void zero_turn_right(){
      if(!is_busy ){
        // current_speed = max_turn_speed;
          is_busy = true;
          changeState(1, "turn_right", max_turn_speed);
          is_busy = false;  
      return;
        }
    };


    void enable() {
      digitalWrite(ENABLE_OUTPUT_PIN, HIGH);
      return;
    };
    
    void disable(){
      digitalWrite(ENABLE_OUTPUT_PIN, LOW);
      return;
    };

    void toggle_zero_turn () {
      zero_turn ^=true;
    }

    void writePWM(short int PWM_Pin, short int pwm){
      short int PWM_Channel;
      if(PWM_Pin == F_PWM_OUTPUT_1) PWM_Channel = F_PWM_Channel_1;
      if(PWM_Pin == F_PWM_OUTPUT_2) PWM_Channel = F_PWM_Channel_2;
      if(PWM_Pin == R_PWM_OUTPUT_1) PWM_Channel = R_PWM_Channel_1;
      if(PWM_Pin == R_PWM_OUTPUT_2) PWM_Channel = R_PWM_Channel_2;

      // bool changePWM = ISR_PWM.modifyPWMChannel(PWM_Channel, PWM_Pin, frequency, pwm);
      ledcWrite(PWM_Channel, pwm);
      // Serial.println("modify PWM channel"+String(PWM_Channel));
      return;
    };

    void softStartBypass(){
      Serial.println(" softStartBypass started"+String(millis()));
      volatile long currentTime  = millis();
      bool wait = true;
      while(true){
        if(millis() - currentTime >= 1000 ) {
          digitalWrite(SOFT_START_BYPASS_ENABLE, HIGH);
          currentTime = millis();
          Serial.println(" softStartBypass after"+String(millis()));
          wait =false;
          break;
        };
      };
      Serial.println(" softStartBypass ended"+String(millis()));
      return;
    };

  Controller(){
    
    ENABLE_OUTPUT_PIN = M_ENABLE;
    F_PWM_OUTPUT_1 = F_PWM_M1;
    R_PWM_OUTPUT_1 = R_PWM_M1;

    F_PWM_OUTPUT_2 = F_PWM_M2;
    R_PWM_OUTPUT_2 = R_PWM_M2;

    short int PINS [10] = {
        ENABLE_OUTPUT_PIN,
        // ENABLE_OUTPUT_PIN_2,
        F_PWM_OUTPUT_1,
        R_PWM_OUTPUT_1,
        F_PWM_OUTPUT_2,
        R_PWM_OUTPUT_2,
        SOFT_START_BYPASS_ENABLE
    };
    for (int pin: PINS) pinMode(pin, OUTPUT);

    F_PWM_Channel_1 = 0;
    // ISR_PWM.setPWM(F_PWM_OUTPUT_1, frequency, 0);
    R_PWM_Channel_1 = 1;
    // ISR_PWM.setPWM(R_PWM_OUTPUT_1, frequency, 0);


    F_PWM_Channel_2 = 2;
    // ISR_PWM.setPWM(F_PWM_OUTPUT_2, frequency, 0);
    R_PWM_Channel_2 = 3;
    // ISR_PWM.setPWM(R_PWM_OUTPUT_2, frequency, 0);
    short channels [4] = {F_PWM_Channel_1, R_PWM_Channel_1, F_PWM_Channel_2, R_PWM_Channel_2};
    for( short int channel: channels) {
      short int pin;
      if(channel == F_PWM_Channel_1) pin = F_PWM_OUTPUT_1;
      if(channel == R_PWM_Channel_1) pin = R_PWM_OUTPUT_1;
      if(channel == F_PWM_Channel_2) pin = F_PWM_OUTPUT_2;
      if(channel == R_PWM_Channel_2) pin = R_PWM_OUTPUT_2;
      
      ledcSetup(channel, frequency, resolution);
      ledcAttachPin(pin, channel);
    };
    digitalWrite(SOFT_START_BYPASS_ENABLE, HIGH);

  };
};

Controller Vehicle;
void set_pin_modes(){
  const int INPUTS[8] = {
        FORWARD_INPUT, 
        REVERSE_INPUT,
        SLOW_DOWN_INPUT,
        SPEED_UP_INPUT,
        START_STOP_INPUT,
        TURN_LEFT_INPUT, 
        TURN_RIGHT_INPUT,
        ZERO_TURN_INPUT
    };

    for (int pin: INPUTS) {
      pinMode(pin, INPUT_PULLDOWN);
    };
};

void setup() {
  Serial.begin(115200);
  Serial.println("Serial monitor started");
  set_pin_modes();
  // Interval in microsecs
//  ITimer.attachInterruptInterval(HW_TIMER_INTERVAL_US, TimerHandler);
  
};

void loop() {
  static short int current_pin = START_STOP_INPUT;
  static bool is_on  =  true;
  static unsigned long zero_turn_toggle_timer, start_stop_toggle_timer  = millis();
  if (digitalRead(START_STOP_INPUT) && millis() - start_stop_toggle_timer > TOGGLE_DELAY) {
        Serial.println("stop:" +String(is_on));
        Vehicle.softStop();
        is_on ^= true;
        if(is_on) {
          Vehicle.enable();
        } else {
          Vehicle.disable();
        };
    Vehicle.current_speed = 0;
    current_pin = START_STOP_INPUT;

    digitalWrite(SOFT_START_BYPASS_ENABLE, LOW);

    start_stop_toggle_timer = millis();
  };

  if (digitalRead(ZERO_TURN_INPUT) && millis() - zero_turn_toggle_timer > TOGGLE_DELAY) {
        Serial.println(" zero turn:" +String(Vehicle.zero_turn));

        Vehicle.softStop();
          delay(SOFT_STOP_DELAY);
          Vehicle.enable();
          delay(15);
        Vehicle.zero_turn_right();
        current_pin = ZERO_TURN_INPUT;
  };

  if (digitalRead(FORWARD_INPUT)  && current_pin != FORWARD_INPUT && is_on) {
      Serial.println("forward:"+String(current_pin)+String(FORWARD_INPUT));
      Vehicle.softStop();
      delay(SOFT_STOP_DELAY);
      Vehicle.enable();
      delay(15);
      Vehicle.forward();
      current_pin = FORWARD_INPUT;
      // set_speed=max_speed_forward;
  };
  if (digitalRead(REVERSE_INPUT) && current_pin != REVERSE_INPUT && is_on ) {
        Serial.println("reverse:"+String(current_pin)+String(REVERSE_INPUT));
        Vehicle.softStop();
        delay(SOFT_STOP_DELAY);
        Vehicle.enable();
        delay(15);
        Vehicle.reverse();
        current_pin = REVERSE_INPUT;
  };
  if (digitalRead(SPEED_UP_INPUT) ){
         Vehicle.speedUp();
        Serial.println("speedUp: after"+String(Vehicle.current_speed)+ "::"+String(Vehicle.current_speed));

  };
  if (digitalRead(SLOW_DOWN_INPUT)){
        Vehicle.slowDown();
        Serial.println("slowDown: after"+String(Vehicle.current_speed)+ "::"+String(Vehicle.current_speed));
  };
  if (digitalRead(TURN_LEFT_INPUT) && current_pin != TURN_LEFT_INPUT && is_on ) {
        Serial.println("turn_left:"+String(Vehicle.zero_turn));
        Vehicle.soft_turn("left");
        
  };
  if (digitalRead(TURN_RIGHT_INPUT) &&  current_pin != TURN_RIGHT_INPUT && is_on ) {
        Serial.println("turn_right:"+String(Vehicle.zero_turn));
        Vehicle.soft_turn("right");
        
  };


  delay(200);
};