#include <NeoPixelConnect.h>
#define MAXIMUM_NUM_NEOPIXELS 7
#include <RP2040_PWM.h>

const int numPins = 3;
const int pwmPins[] = {A1, A2, A3};
const float minDutyCycle[numPins] = {5.98, 6.03, 6.03};
const float maxDutyCycle[numPins] = {12.08, 12.02, 11.94};
const float deadzone = 0.50;
volatile float mappedDutyCycles[numPins] = {0.0, 0.0, 0.0};

enum devMode{
  automatic = 0,
  safe = 1,
  manual = 2
};

volatile devMode mode = safe;

// WS2812B LEDs
const int numLEDs = 7;
const int ledPin = 15;
NeoPixelConnect p(ledPin, numLEDs, pio1, 0);

// Servo
const int servoPins[] = {10, 6};
const int numServos = 2;
const int servoFreq = 60;
RP2040_PWM* servo1;
RP2040_PWM* servo2;
float servo1Perc = 0.0;
float servo2Perc = 0.0;
//system

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void runDirectMode();

void setup() {
  Serial.begin(9600);
  delay(5000);

  for (int i = 0; i < numPins; i++) {
    pinMode(pwmPins[i], INPUT);
  }

  // Set up WS2812B LEDs
  //strip.begin();
  //strip.show();
  //setLEDDefaultColors();

  // Set up servo PWM
  for (int i = 0; i < numServos; i++) {
    pinMode(servoPins[i], OUTPUT);
  }
  servo1 = new RP2040_PWM(servoPins[0], servoFreq, 0);
  servo2 = new RP2040_PWM(servoPins[1], servoFreq, 0);

  servo1->setPWM();
  servo2->setPWM();
}

void runLEDs(){
  switch((int)mode){
    case 0:
      p.neoPixelFill(0, 0, 255, true);
    break;
    case 1:
      p.neoPixelFill(255, 0, 0, true);
    break;
    case 2:
      setLEDDefaultColors();
    break;  
    default:
      p.neoPixelFill(0, 255, 0, true);
      Serial.println(mode);
    break;  
  }
}

void loop() {
  for (int i = 0; i < numPins; i++) {
    unsigned long highDuration = pulseIn(pwmPins[i], HIGH);
    unsigned long lowDuration = pulseIn(pwmPins[i], LOW);
    unsigned long period = highDuration + lowDuration;
    float dutyCycle = (float)highDuration / (float)period * 100.0;

    // Map the duty cycle value to the range of -1.0 to 1.0
    mappedDutyCycles[i] = mapFloat(dutyCycle, minDutyCycle[i], maxDutyCycle[i], -1.0, 1.0);
    Serial.print(i);
    Serial.print(" ");
    Serial.println(period);
    // Apply deadzone
    if (mappedDutyCycles[i] > -deadzone && mappedDutyCycles[i] < deadzone) {
      mappedDutyCycles[i] = 0.0;
    }
  }

  delay(1);
}

// Running on core1
void setup1() {
  delay(5000);
}

void loop1() {
  modeCheck();
  runLEDs();
  switch(mode){
    case automatic://do nothing, values prodivded by host computer
    break;
    case safe: //stop car
      servo1Perc = 0.0f;
      servo2Perc = 0.0f;      
    break;
    case manual:
      servo1Perc = ((mappedDutyCycles[1] + 1.0) * 3);
      servo2Perc = ((mappedDutyCycles[2] + 1.0) * 3);
      //Serial.println(servo1Perc);
      //Serial.println(servo1Perc);
    break;
  }
  updateServos();
  handleSerialCommands();  
  // Print the pin name and the mapped duty cycle in a format suitable for the Serial Plotter
  /*for (int i = 0; i < numPins; i++) {
    Serial.print(pwmPins[i]);
    Serial.print(":");
    Serial.print(mappedDutyCycles[i]);

    if (i < numPins - 1) {
      Serial.print(",");
    } else {
      Serial.println();
    }
  }*/

  delay(1); // Wait for 1ms before the next transmission
}

void setLEDDefaultColors() {
  for (int i = 0; i < 2; i++) {
    //strip.setPixelColor(i, strip.Color(255, 0, 0));
    p.neoPixelSetValue(i, 255,0,0, true);
  }
  for (int i = 2; i < 6; i++) {
    //strip.setPixelColor(i, strip.Color(255, 255, 255));
     p.neoPixelSetValue(i, 255,255,255, true);
  }
  //strip.show();
}

void cycleLEDColors() {/*
  static uint32_t color = strip.Color(255, 0, 0);
  static uint8_t colorState = 0;

  for (int i = 0; i < numLEDs; i++) {
    strip.setPixelColor(i, color);
  }
  strip.show();

  if (colorState == 0) {
    if (color == strip.Color(255, 0, 0)) {
      color = strip.Color(0, 255, 0);
    } else if (color == strip.Color(0, 255, 0)) {
      color = strip.Color(0, 0, 255);
    } else {
      color = strip.Color(255, 0, 0);
    }
  }*/
  p.neoPixelFill(255, 0, 0, true);  
}

void handleSerialCommands() {
  if (Serial.available() > 0) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    if (command[0] = '$') {
      Serial.println("$b29b3875-2e31-4b13-991f-08b7f62c141b");
    }
    if (command.startsWith("l")) {
      if (command == "l1") {
        //setLEDDefaultColors();
      } else if (command == "l0") {
        //cycleLEDColors();
      } else if (command.startsWith("l")) {
        int ledNum;
        int r, g, b;
        if (sscanf(command.c_str(), "l%d %d,%d,%d", &ledNum, &r, &g, &b) == 4) {
          if (ledNum >= 0 && ledNum < numLEDs) {
            //strip.setPixelColor(ledNum, strip.Color(r, g, b));
            //strip.show();
          }
        }
      }
    } else if (command.startsWith("s")) {
      int servoNum;
      int dutyCycle;
      if (sscanf(command.c_str(), "s%d %d", &servoNum, &dutyCycle) == 2) {
        if (servoNum >= 0 && servoNum < numServos && dutyCycle >= 0 && dutyCycle <= 255) {
          analogWrite(servoPins[servoNum], dutyCycle);
          (servoNum == 0 ? servo1 : servo2)->setPWM(servoPins[servoNum], 60, dutyCycle);
        }
      }
    }
  }
}

void modeCheck(){
  for (int i = 0; i < 3; i++) {
      if (mappedDutyCycles[i] < -1.3 || mappedDutyCycles[i] > 1.3) {
          mode = safe;  //out of bounds/disconnected
          Serial.println("#OUT_OF_BOUNDS");
          return;
      }
  }

  if(mappedDutyCycles[0] > 0.5){ //interrupteur sur le côté de la télécommande
    mode = manual;
  }else if(mappedDutyCycles[0] > -0.25 && mappedDutyCycles[0] < 0.25){
    mode = automatic;
  }
  else if(mappedDutyCycles[0] < -0.5){
    mode = safe;
  }
}

void updateServos(){
  servo1->setPWM(servoPins[0],60,servo1Perc+5.5f);  //accelerateur
  servo2->setPWM(servoPins[1],60,servo2Perc);  //dir
}