#include <Adafruit_NeoPixel.h>
#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.1;
volatile float mappedDutyCycles[numPins] = {0.0, 0.0, 0.0};

// WS2812B LEDs
const int numLEDs = 6;
const int ledPin = 10;
Adafruit_NeoPixel strip(numLEDs, ledPin, NEO_GRB + NEO_KHZ800);

// Servo
const int servoPins[] = {11, 12};
const int numServos = 2;
const int servoFreq = 60;
RP2040_PWM* servo1;
RP2040_PWM* servo2;

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

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

    // Apply deadzone
    if (mappedDutyCycles[i] > -deadzone && mappedDutyCycles[i] < deadzone) {
      mappedDutyCycles[i] = 0.0;
    }
  }

  delay(1);
}

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

void loop1() {
  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("Pin ");
    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 < 4; i++) {
    strip.setPixelColor(i, strip.Color(255, 255, 255));
  }
  for (int i = 4; i < 6; i++) {
    strip.setPixelColor(i, strip.Color(255, 0, 0));
  }
  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);
    }
  }
}

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