#include <Arduino.h>
#include <Stepper.h>
#include <FastLED.h>
#include <PWMServo.h> // Include the PWMServo library

// --- REMOVED FOR WIRED COMMUNICATION ---
// #include <SPI.h>
// #include <RF24.h>
// RF24 radio(9, 10);
// const byte address[6] = "00001";
// char receivedMessage[32];
// --- END OF REMOVED CODE ---


#define LED_PIN     A4
#define NUM_LEDS    60 // Adjust to the number of LEDs in your strip
#define BRIGHTNESS  200
#define LED_TYPE    WS2812
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];
bool coloredPositions[NUM_LEDS]; // Array to track the positions of colored LEDs
CRGB coloredColors[NUM_LEDS]; // Array to store the random colors for colored LEDs

const int chopstickBottom = A0;
const int chopstickTop = A1;

const int boosterTop = A2;
const int boosterBottom = A3;

const int shipSensor = A7;

// Number of steps per output rotation
const int stepsPerRevolution = 200;
// make this one if you don't have gears
int gearReduction = 1;
// Current position of the motor
int currentPos = 0;
// Create Instance of Stepper library
Stepper myStepper(stepsPerRevolution, 5, 6, 7, 8);

// DC Motor A connections
int enA = 4;
int in1 = 3;
int in2 = 2;
int speed = 0;

// PWMServo objects
PWMServo servo1;
PWMServo servo0;
PWMServo servoA5;
PWMServo servoA6;

// Define a default interval for servo easing
const unsigned long SERVO_EASING_INTERVAL = 10; // milliseconds

void setup() {
    // We will use the main hardware Serial for communication
    Serial.begin(9600);
    // The while loop is now just for debugging with the Serial Monitor if needed before connecting wires
    while (!Serial && millis() < 5000);
    Serial.println("Wired Receiver Starting...");

    // --- REMOVED NRF24 INITIALIZATION ---

    servo1.attach(10);
    servo0.attach(11);
    servoA5.attach(A5);
    servoA6.attach(A6);

    easeServo(servo1, 146, 1000, SERVO_EASING_INTERVAL);
    easeServo(servo0, 150, 1000, SERVO_EASING_INTERVAL);

    FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
    FastLED.setBrightness(BRIGHTNESS);
    memset(coloredPositions, 0, sizeof(coloredPositions));
    fill_solid(leds, NUM_LEDS, CRGB::Black);
    FastLED.show();

    delay(500);

    pinMode(chopstickTop, INPUT);
    pinMode(chopstickBottom, INPUT);
    pinMode(boosterTop, INPUT);
    pinMode(boosterBottom, INPUT);
    pinMode(shipSensor, INPUT);

    myStepper.setSpeed(60);

    pinMode(enA, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);

    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(enA, HIGH);
}

void loop() {
    // --- MODIFIED TO USE WIRED SERIAL ---
    // Check if there is data available to read from the serial port.
    if (Serial.available() > 0) {
        // Read the incoming command string until a newline character is received.
        String message = Serial.readStringUntil('\n');
        message.trim(); // Clean up any whitespace

        // --- The rest of the logic is the same ---
        Serial.print("Message received: ");
        Serial.println(message);

        if (message == "arm") {
            Serial.println("arm activated");
            arm();
        } else if (message == "launch") {
            Serial.println("launch activated");
            launch();
        } else if (message == "stack") {
            Serial.println("stack activated");
            stack();
        } else if (message == "shipQD") {
            Serial.println("shipQD activated");
            shipQD();
        } else if (message == "openClose") {
            Serial.println("openClose activated");
            openClose();
        } else if (message == "sideToSide") {
            Serial.println("sideToSide activated");
            sideToSide();
        } else if (message == "up") {
            Serial.println("up activated");
            up();
        } else if (message == "down") {
            Serial.println("down activated");
            down();
        } else if (message.length() > 0) { // Only print if the message isn't empty
            Serial.print("Unknown command: ");
            Serial.println(message);
        }
    }
}

// --- ALL HELPER FUNCTIONS BELOW REMAIN UNCHANGED ---
// arm(), launch(), stack(), etc. are all the same as before.

void arm(){
    if(digitalRead(shipSensor) == HIGH){
      stack();
    }
    moveStepperTo(500); // Move the stepper to position
    qdAttach();
}

void launch() {
  delay(3000); //wait for the countdown
  qdRetract(); //this will make the timing inacurate needs to be fixed*************

  //start engines at T-2 seconds
  unsigned long ledTimer = millis() + 2000;
  while(millis() < ledTimer){
    led();
  }

  //move booster up
  while(digitalRead(boosterTop) == LOW){
    speed = 100; //0-255
    led();
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    analogWrite(enA, speed);
    }
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);

  //wait for 3 seconds
  ledTimer = millis() + 3000;
  while(millis() < ledTimer){
    led();
  }

  //turn off leds
  fill_solid(leds, NUM_LEDS, CRGB::Black);
  // Update the LED strip
  FastLED.show();

  //move booster back down
  while(digitalRead(boosterBottom) == LOW){
    speed = 60; //0-255
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    analogWrite(enA, speed);
    }
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
}

void stack() {    //**removed**
  /*if(digitalRead(shipSensor) == HIGH){//stack 
  qdRetract();
  moveStepperTo(500); // Move the stepper above booster
  chopsticksLeftOpen();
  moveStepperTo(100);
  chopsticksLeft();
  moveStepperTo(520);
  chopsticksClose();
  moveStepperTo(500);
  chopsticksOpen();
  moveStepperTo(400);
  }

  if(digitalRead(shipSensor) == LOW){//destack
  qdRetract();
  chopsticksOpen();
  moveStepperTo(500);
  chopsticksClose();
  moveStepperTo(520);
  chopsticksLeft();
  moveStepperTo(100);
  chopsticksLeftOpen();
  moveStepperTo(400);
  chopsticksOpen();
  }*/
}

void shipQD(){
    // Placeholder
}

void qdAttach() {
    easeServo(servoA5, 10, 1000, SERVO_EASING_INTERVAL);
    easeServo(servoA6, 145, 1000, SERVO_EASING_INTERVAL);
}

void qdRetract() {
    easeServo(servoA5, 70, 1000, SERVO_EASING_INTERVAL);
    easeServo(servoA6, 45, 1000, SERVO_EASING_INTERVAL);
}

void openClose(){
    // Placeholder
}

void chopsticksOpen(){
  easeServos(servo1, 179, servo0, 179, 1000, SERVO_EASING_INTERVAL);
}

void chopsticksClose(){// acts as close and right
  easeServos(servo1, 146, servo0, 150, 1000, SERVO_EASING_INTERVAL);
}

void sideToSide(){
    // Placeholder
}

void chopsticksLeftOpen(){
  easeServos(servo1, 44, servo0, 135, 1000, SERVO_EASING_INTERVAL);
}

void chopsticksLeft(){// Left closed
  easeServos(servo1, 0, servo0, 90, 1000, SERVO_EASING_INTERVAL);
}

void up(){
  moveStepperTo(currentPos + 100);
}

void down(){
  moveStepperTo(currentPos - 100);
}

void moveStepperTo(float targetPos) {
    int stepsToMove = (targetPos - currentPos) * gearReduction;

    if ((currentPos < 200 && targetPos > 200) || (currentPos < 300 && targetPos > 300) ||
        (currentPos > 300 && targetPos < 300) || (currentPos > 200 && targetPos < 200)) {
        qdRetract();
    }

    int stepSize = (stepsToMove > 0) ? 1 : -1;
    for (int i = 0; i < abs(stepsToMove); i++) {
        if (digitalRead(chopstickTop) == HIGH || digitalRead(chopstickBottom) == HIGH) {
            break;
        }
        myStepper.step(stepSize);
        currentPos += stepSize; // Update currentPos based on actual steps moved
    }
    // Turn off stepper coils to save power and reduce heat
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
    digitalWrite(7, LOW);
    digitalWrite(8, LOW);
}

void easeServo(PWMServo& servo, int targetPos, int duration, unsigned long interval) {
    int startPos = servo.read();
    float delta = targetPos - startPos;
    unsigned long startTime = millis();
    unsigned long nextUpdateTime = startTime;

    if (duration <= 0) { // Handle cases with non-positive duration
        servo.write(targetPos);
        return;
    }

    while (millis() - startTime < duration) {
        if (millis() >= nextUpdateTime) {
            float t = float(millis() - startTime) / duration;
            float easedT = t * t * t; // Cubic easing
            float currentPosFloat = startPos + easedT * delta;
            servo.write(static_cast<int>(currentPosFloat));
            nextUpdateTime += interval;
        }
        delay(1); // Very small delay to allow other processes
    }
    servo.write(targetPos);
}

void easeServos(PWMServo& servo1, int targetPos1, PWMServo& servo2, int targetPos2, int duration, unsigned long interval) {
    int startPos1 = servo1.read();
    float delta1 = targetPos1 - startPos1;
    int startPos2 = servo2.read();
    float delta2 = targetPos2 - startPos2;
    unsigned long startTime = millis();
    unsigned long nextUpdateTime = startTime;

    if (duration <= 0) { // Handle cases with non-positive duration
        servo1.write(targetPos1);
        servo2.write(targetPos2);
        return;
    }

    while (millis() - startTime < duration) {
        if (millis() >= nextUpdateTime) {
            float t = float(millis() - startTime) / duration;
            float easedT = t * t * t; // Cubic easing in
            int currentPos1 = startPos1 + easedT * delta1;
            int currentPos2 = startPos2 + easedT * delta2;
            servo1.write(currentPos1);
            servo2.write(currentPos2);
            nextUpdateTime += interval;
        }
        delay(1); // Very small delay to allow other processes
    }
    servo1.write(targetPos1); // Ensure we end exactly at the target position
    servo2.write(targetPos2); // Ensure we end exactly at the target position
}


void led() {
    static uint8_t offset = 0;
    static unsigned long lastUpdate = 0;
    unsigned long now = millis();

    if (now - lastUpdate >= 10) {
        lastUpdate = now;
        fill_solid(leds, NUM_LEDS, CRGB::Black);

        for (int i = 0; i < NUM_LEDS; i++) {
            if ((i + offset) % 2 == 0) {
                uint8_t brightness = BRIGHTNESS * (NUM_LEDS - i) / NUM_LEDS;
                uint8_t red = 255;
                uint8_t green = 255 * i / NUM_LEDS;
                leds[i] = CRGB(red, green, 0);
                leds[i].fadeLightBy(255 - brightness);
            }
        }

        for (int i = NUM_LEDS - 1; i > 0; i--) {
            coloredPositions[i] = coloredPositions[i - 1];
            coloredColors[i] = coloredColors[i - 1];
            if (coloredPositions[i]) {
                uint8_t brightness = BRIGHTNESS * (NUM_LEDS - i) / NUM_LEDS;
                leds[i] = coloredColors[i];
                leds[i].fadeLightBy(255 - brightness);
            }
        }
        coloredPositions[0] = false;

        if (random(0, 4) > 2) {
            coloredPositions[0] = true;
            uint8_t red = random(128, 256);
            uint8_t green = random(0, 64);
            coloredColors[0] = CRGB(red, green, 0);
        }

        FastLED.show();
        offset++;
        if (offset >= 2) {
            offset = 0;
        }
    }
}