#include <WiFi.h>
#include <ArduinoOTA.h>


const char* ssid = "CIA";
const char* password = "hi123456";

#define motorAttach  ledcAttach     // Get more appropriate names
#define motorSpeed   ledcWrite      // same "

// Motor FR (Front Right)
  #define FR_IN1 27
  #define FR_IN2 14
  #define FR_PWM 0

  // Motor FL (Front Left)
  #define FL_IN1 12
  #define FL_IN2 13
  #define FL_PWM 2

// Motor BR (Back Right)
  #define BR_IN1 33
  #define BR_IN2 32
  #define BR_PWM 23

  // Motor BL (Back Left)
  #define BL_IN1 26
  #define BL_IN2 25
  #define BL_PWM 22


  // PWM settings
  #define PWM_FREQ 1000
  #define PWM_RESOLUTION 8  // 0-255

  #define SERIAL_RX 21

  // Sensor data received from micro sensors
  int sens_FL = -1;
  int sens_FR = -1;
  int sens_BL = -1;
  int sens_BR = -1;
  int sens_LF = -1;
  int sens_LB = -1;
  int sens_RF = -1;
  int sens_RB = -1;

  // Trim values to calibrate wheel speeds
  int trimFL = 0;
  int trimFR = 0;
  int trimBL = 0;
  int trimBR = 0;
    #define BUFFER_SIZE 7
    int buffer_FL[BUFFER_SIZE];
    int buffer_FR[BUFFER_SIZE];
    int buffer_BL[BUFFER_SIZE];
    int buffer_BR[BUFFER_SIZE];
    int buffer_LF[BUFFER_SIZE];
    int buffer_LB[BUFFER_SIZE];
    int buffer_RF[BUFFER_SIZE];
    int buffer_RB[BUFFER_SIZE];

    int bufferIndex = 0;
    bool bufferFilled = false;
    bool ph1 = false;           // Phase 1 complete flag
    bool ph2 = false;           //phase 2 complete flag
    bool ph3 = false;           //phase 3 complete flag
    bool p2_stop = false;       // Flag when its passed the top of the hill

// Telnet server to watch serial
WiFiServer telnetServer(23);
WiFiClient telnetClient;

// Function declaritons
void otaTask(void *parameter);              // Get micro ready for OTA
void logPrint(const char* msg);             // Print function for serial and telnet, strings
void logPrint(int val);                     // Print function for serial and telnet, integers
void logPrint(float val);                   // Print function for serial and telnet, floats
void logPrintln(const char* msg);           // Print line function for serial and telnet
void logPrintln(int val);                   // Print line function for serial and telnet, integers
void logPrintln(float val);                 // Print line function for serial and telnet, floats
void telnetTask(void *parameter);           // Configure telnet serial
void motorFR(int speed);            // Front right motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorFL(int speed);            // Front left motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorBR(int speed);            // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorBL(int speed);            // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward
void motorsStop();                  // Stop all motors
void motorsGo(int speed);           // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward
void Go45right();
void Go45left();
void Go90right();
void Go90left();
void slideRight();
void slideLeft();
void receiveSensorData();           // Receive the sensors data from micro sensors
int rollingAverage(int buffer[]);   // rollingAverage function call
void sensorsTest();
void motorsTest();
void phaseThree();
void align(int dist);

void setup() {
  Serial.begin(115200);

  // Configure WIFI
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);

  int wifiAttempts = 0;
  while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) {
      Serial.println("WiFi connecting...");
      wifiAttempts++;
      delay(500);
  }

  if (WiFi.status() == WL_CONNECTED) {
      Serial.print("IP Address: ");
      Serial.println(WiFi.localIP());
  } else {
      Serial.println("WiFi not available - running without network");
  }


if (WiFi.status() == WL_CONNECTED) {
      ArduinoOTA.setHostname("Motors");
      ArduinoOTA.onStart([]() {
          Serial.println("OTA Update starting...");
      });
      ArduinoOTA.onEnd([]() {
          Serial.println("\nOTA Update complete!");
      });
      ArduinoOTA.onError([](ota_error_t error) {
          Serial.printf("OTA Error [%u]\n", error);
      });
      ArduinoOTA.begin();
      telnetServer.begin();
  }

  Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1);

  // Motor direction pins
  pinMode(FR_IN1, OUTPUT);
  pinMode(FR_IN2, OUTPUT);
  pinMode(FL_IN1, OUTPUT);
  pinMode(FL_IN2, OUTPUT);
  pinMode(BR_IN1, OUTPUT);
  pinMode(BR_IN2, OUTPUT);
  pinMode(BL_IN1, OUTPUT);
  pinMode(BL_IN2, OUTPUT);

  // PWM setup for ESP32
  motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION);
  motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION);
  motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION);
  motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION);

  // Start with motors stopped
  digitalWrite(FR_IN1, LOW);
  digitalWrite(FR_IN2, LOW);
  digitalWrite(FL_IN1, LOW);
  digitalWrite(FL_IN2, LOW);
  motorSpeed(FR_PWM, 0);
  motorSpeed(FL_PWM, 0);
  digitalWrite(BR_IN1, LOW);
  digitalWrite(BR_IN2, LOW);
  digitalWrite(BL_IN1, LOW);
  digitalWrite(BL_IN2, LOW);
  motorSpeed(BR_PWM, 0);
  motorSpeed(BL_PWM, 0);

    // FreeRTOS tasks
    if (WiFi.status() == WL_CONNECTED) {
      xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL);
      xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL);
  }


}

void loop() {
    ph1 = true;
//    ph2 = true;
    ph3 = true;


    if (!ph1) {
        phaseOne();
        ph1 = true;
    }

    if (!ph2) {
        phaseTwo();
        ph2 = true;
    }

    if (!ph3) {
        phaseThree();
        ph3 = true;
    }

//    motorsTest();
    sensorsTest();

}

void phaseOne() {
    // Wait for valid sensor readings
    while (sens_FL < 20 || sens_FR < 20 || sens_FL > 8000 || sens_FR > 8000) {
        receiveSensorData();    // read sensors
        delay(10);
    }

    int distance;
    int speed;

    receiveSensorData();
    while ((distance = (sens_FL + sens_FR) / 2) > 225) {
        // Slow down as we approach target
        if (distance > 500) {
            speed = 120;
        } else if (distance > 450) {
            speed = 80;
        } else {
            speed = 40;         // Slow down even more
        }
        motorsGo(speed);
        delay(5);               // Wait for new sensor data to arrive
        receiveSensorData();    // Get fresh reading each iteration
    }

    motorsStop();

    align(245);

    logPrint("Stopped at: "); logPrintln((sens_FL + sens_FR) / 2);
    delay(1000);
    Go90right();
    delay(2000);
    motorsGo(80);
    delay(1250);        // Go into the pit 1800 miliseconds,
    motorsStop();
    delay(3000);		//how long to pick balls? dunno
    motorsGo(-150);
    delay(1200);        // Go out of the pit 1800 miliseconds
    motorsStop();
    delay(500);
    slideLeft();
    delay(1450);
    motorsStop();
    delay(2000);
}

void phaseTwo() {
    // Wait for valid sensor readings - ENABLE IF YOU ARE TESTING ONLY THIS PHASE
    while (sens_FL < 0 || sens_FR < 0) {
        receiveSensorData();    // read sensors
        delay(10);
    }

//    align(150);


    receiveSensorData();

/*
        while ((sens_FL + sens_FR) / 2 > 7000) {
            motorsGo(-230);
            delay(5);               // Wait for new sensor data to arrive
            receiveSensorData();    // Get fresh reading each iteration
        }
        while ((sens_FL + sens_FR) / 2 < 300) {     // Used to be 680
            motorsGo(-250);
            delay(5);               // Wait for new sensor data to arrive
            receiveSensorData();    // Get fresh reading each iteration
        }
        while ((sens_FL + sens_FR) / 2 > 500) {
            motorsGo(-120);
            delay(5);               // Wait for new sensor data to arrive
            receiveSensorData();    // Get fresh reading each iteration
        }
*/

    delay(1000);
    motorsGo(-255);

    delay(3000);


    
        while ((sens_FL + sens_FR) / 2 > 300) {
            motorsGo(-100);
            delay(5);               // Wait for new sensor data to arrive
            receiveSensorData();    // Get fresh reading each iteration
        }
        while ((sens_FL + sens_FR) / 2 > 170) {
            motorsGo(-80);
            delay(5);               // Wait for new sensor data to arrive
            receiveSensorData();    // Get fresh reading each iteration
        }

        motorsStop();



}

void phaseThree() {


}

void otaTask(void *parameter) {
      for (;;) {
          ArduinoOTA.handle();
          vTaskDelay(10 / portTICK_PERIOD_MS);
      }
  }

void telnetTask(void *parameter) {
      for (;;) {
          if (telnetServer.hasClient()) {
              if (telnetClient && telnetClient.connected()) {
                  telnetClient.stop();
              }
              telnetClient = telnetServer.available();
              telnetClient.println("Connected to Motors");
          }
          vTaskDelay(100 / portTICK_PERIOD_MS);
      }
  }

  void logPrint(const char* msg) {
      Serial.print(msg);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.print(msg);
      }
  }

  void logPrint(int val) {
      Serial.print(val);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.print(val);
      }
  }

  void logPrint(float val) {
      Serial.print(val);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.print(val);
      }
  }

  void logPrintln(const char* msg) {
      Serial.println(msg);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.println(msg);
      }
  }

  void logPrintln(int val) {
      Serial.println(val);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.println(val);
      }
  }

  void logPrintln(float val) {
      Serial.println(val);
      if (telnetClient && telnetClient.connected()) {
          telnetClient.println(val);
      }
  }

void motorFR(int speed) {
      if (speed > 0) {
          digitalWrite(FR_IN1, HIGH);
          digitalWrite(FR_IN2, LOW);
          motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255));
      } else if (speed < 0) {
          digitalWrite(FR_IN1, LOW);
          digitalWrite(FR_IN2, HIGH);
          motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255));
      } else {
          digitalWrite(FR_IN1, LOW);
          digitalWrite(FR_IN2, LOW);
          motorSpeed(FR_PWM, 0);
      }
  }

  void motorFL(int speed) {
      if (speed > 0) {
          digitalWrite(FL_IN1, HIGH);
          digitalWrite(FL_IN2, LOW);
          motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255));
      } else if (speed < 0) {
          digitalWrite(FL_IN1, LOW);
          digitalWrite(FL_IN2, HIGH);
          motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255));
      } else {
          digitalWrite(FL_IN1, LOW);
          digitalWrite(FL_IN2, LOW);
          motorSpeed(FL_PWM, 0);
      }
  }

void motorBR(int speed) {
      if (speed > 0) {
          digitalWrite(BR_IN1, HIGH);
          digitalWrite(BR_IN2, LOW);
          motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255));
      } else if (speed < 0) {
          digitalWrite(BR_IN1, LOW);
          digitalWrite(BR_IN2, HIGH);
          motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255));
      } else {
          digitalWrite(BR_IN1, LOW);
          digitalWrite(BR_IN2, LOW);
          motorSpeed(BR_PWM, 0);
      }
  }

  void motorBL(int speed) {
      if (speed > 0) {
          digitalWrite(BL_IN1, HIGH);
          digitalWrite(BL_IN2, LOW);
          motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255));
      } else if (speed < 0) {
          digitalWrite(BL_IN1, LOW);
          digitalWrite(BL_IN2, HIGH);
          motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255));
      } else {
          digitalWrite(BL_IN1, LOW);
          digitalWrite(BL_IN2, LOW);
          motorSpeed(BL_PWM, 0);
      }
  }

  void motorsStop() {
      motorFR(0);
      motorFL(0);
      motorBR(0);
      motorBL(0);
  }

  int rollingAverage(int buffer[]) {
    int sum = 0;
    int count = bufferFilled ? BUFFER_SIZE : (bufferIndex + 1);

    for (int i = 0; i < count; i++) {
        sum += buffer[i];
    }

    return sum / count;
}

void receiveSensorData() {

    static String data = "";                                                                                                   
                                                                                                                                 
      while (Serial2.available()) {                                                                                              
          char c = Serial2.read();                                                                                               
          if (c == '\n') {                                                                                                       
              // Process complete line                                                                                           
              int values[8];                                                                                                     
              int idx = 0;                                                                                                       
              int start = 0;                                                                                                     
                                                                                                                                 
              for (int i = 0; i <= data.length() && idx < 8; i++) {                                                              
                  if (i == data.length() || data[i] == ',') {                                                                    
                      values[idx++] = data.substring(start, i).toInt();                                                          
                      start = i + 1;                                                                                             
                  }                                                                                                              
              }                                                                                                                  
                                                                                                                                 
              if (idx == 8) {                                                                                                    
                  sens_FL = values[0];                                                                                           
                  sens_FR = values[1];                                                                                           
                  sens_BL = values[2];                                                                                           
                  sens_BR = values[3];                                                                                           
                  sens_LF = values[4];                                                                                           
                  sens_LB = values[5];                                                                                           
                  sens_RF = values[6];                                                                                           
                  sens_RB = values[7];                                                                                           
              }                                                                                                                  
              data = "";                                                                                                         
          } else if (c != '\r') {                                                                                                
              data += c;                                                                                                         
          }                                                                                                                      
      }  

/*    
      String data;

      // Drain buffer - keep only the latest complete line
      while (Serial2.available()) {
          data = Serial2.readStringUntil('\n');
      }

      if (data.length() == 0) return;

      int values[8];
      int idx = 0;
      int start = 0;

      for (int i = 0; i <= data.length() && idx < 8; i++) {
          if (i == data.length() || data[i] == ',') {
              values[idx++] = data.substring(start, i).toInt();
              start = i + 1;
          }
      }

      if (idx == 8) {
          sens_FL = values[0];
          sens_FR = values[1];
          sens_BL = values[2];
          sens_BR = values[3];
          sens_LF = values[4];
          sens_LB = values[5];
          sens_RF = values[6];
          sens_RB = values[7];
      }
*/
  }

void sensorsTest() {

    receiveSensorData();

      logPrint("FL:"); logPrint(sens_FL);
      logPrint(" FR:"); logPrint(sens_FR);
      logPrint(" BL:"); logPrint(sens_BL);
      logPrint(" BR:"); logPrint(sens_BR);
      logPrint(" LF:"); logPrint(sens_LF);
      logPrint(" LB:"); logPrint(sens_LB);
      logPrint(" RF:"); logPrint(sens_RF);
      logPrint(" RB:"); logPrintln(sens_RB);

      delay(100);
}

void motorsTest() {

      logPrintln("Right forward...");
      motorBR(80);
      motorFR(80);
      delay(2000);

      logPrintln("Right stop...");
      motorBR(0);
      motorFR(0);
      delay(1000);

      logPrintln("Right forward...");
      motorBR(250);
      motorFR(250);
      delay(2000);

      logPrintln("Right stop...");
      motorBR(0);
      motorFR(0);
      delay(1000);

      logPrintln("Left forward...");
      motorBL(80);
      motorFL(80);
      delay(2000);

      logPrintln("Left stop...");
      motorsStop();
      delay(1000);

      logPrintln("Left forward...");
      motorBL(250);
      motorFL(250);
      delay(2000);

      logPrintln("Left stop...");
      motorsStop();
      delay(1000);

      logPrintln("Front forward...");
      motorFR(80);
      motorFL(80);
      delay(2000);

      logPrintln("Front stop...");
      motorsStop();
      delay(1000);

      logPrintln("Front forward...");
      motorFR(250);
      motorFL(250);
      delay(2000);

      logPrintln("Front stop...");
      motorsStop();
      delay(1000);

      logPrintln("Back forward...");
      motorBR(80);
      motorBL(80);
      delay(2000);

      logPrintln("Back stop...");
      motorsStop();
      delay(1000);

      logPrintln("Back forward...");
      motorBR(250);
      motorBL(250);
      delay(2000);

      logPrintln("Back stop...");
      motorsStop();
      delay(1000);
}

void motorsGo(int speed) {
      motorFR(speed);
      motorFL(speed);
      motorBR(speed);
      motorBL(speed);
}

void Go45right() {
    motorFL(150);
    motorFR(-150);
    motorBL(150);
    motorBR(-150);
    delay(875);
    motorsStop();
}

void Go45left() {
    motorFL(-150);
    motorFR(150);
    motorBL(-150);
    motorBR(150);
    delay(875);
    motorsStop();
}

void Go90right() {
    motorFL(150);
    motorFR(-150);
    motorBL(150);
    motorBR(-150);
    delay(1100);
    motorsStop();
}

void Go90left() {
    motorFL(-150);
    motorFR(150);
    motorBL(-150);
    motorBR(150);
    delay(1100);
    motorsStop();
}

void slideRight() {
    motorFL(-150);
    motorFR(150);
    motorBL(150);
    motorBR(-150);
}

void slideLeft() {
    motorFL(150);
    motorFR(-150);
    motorBL(-150);
    motorBR(150);
}

void align(int dist) {                                                                                                         
      receiveSensorData();                                                                                                       
      while ((sens_FL + sens_FR) / 2 > dist) {                                                                                   
          if (sens_FL > sens_FR) {                                                                                               
              motorFL(50);                                                                                                       
          } else {                                                                                                               
              motorFR(50);                                                                                                       
          }                                                                                                                      
          delay(10);                                                                                                             
          motorsStop();                                                                                                          
          receiveSensorData();                                                                                                   
      }                                                                                                                          
  }