#include <Arduino.h>
#include <Stepper.h>
#include <FastLED.h>
#include <RH_ASK.h>
#include <SPI.h> // Not actually used but needed to compile

RH_ASK driver;

#define LED_PIN     13
#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 = A4;

// 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 connections
int enA = 4;
int in1 = 3;
int in2 = 2;
int speed = 100; //0-255


//states
int sqdState = 0; //1 is atatched, 2 is retracted
int chopstickOpen = 0; //1 is open, 2 is closed
int chopsticksLeft = 0; //1 is left, 2 is center



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

    FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
    FastLED.setBrightness(BRIGHTNESS);
    memset(coloredPositions, 0, sizeof(coloredPositions)); // Initialize all positions to false
    // Clear the strip
    fill_solid(leds, NUM_LEDS, CRGB::Black);
    // Update the LED strip
    FastLED.show();

    pinMode(A6, OUTPUT);
    pinMode(12, OUTPUT);
    pinMode(10, OUTPUT);
    pinMode(9, OUTPUT);

    delay(500); // Wait for servo to reach start position.

    sqdState = 1; //0 is unknown, 1 is atatched, 2 is retracted
    chopstickOpen = 1; //0 is unknown, 1 is open, 2 is closed
    chopsticksLeft = 1; //0 is unknown, 1 is left, 2 is center

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

    pinMode(shipSensor, INPUT);

    // set the speed at 60 rpm:
    myStepper.setSpeed(60);

    // Set the motor control pins to outputs
    pinMode(enA, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);

    // Turn off motor - Initial state
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(enA, HIGH);


//home motors
    //home booster
    if(digitalRead(boosterBottom) == HIGH){ //bottom is pressed
      digitalWrite(in1, HIGH);//motor up
      digitalWrite(in2, LOW);
      analogWrite(enA, speed);

      delay(100);

      digitalWrite(in1, LOW);//motor stop
      digitalWrite(in2, LOW);
    }

    while(digitalRead(boosterBottom) == LOW){
      digitalWrite(in1, LOW);//motor down
      digitalWrite(in2, HIGH);
      analogWrite(enA, speed);
    }

    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW); 

    //home chopsticks


}

void loop() {
    uint8_t buf[10];
    uint8_t buflen = sizeof(buf);
    if (driver.recv(buf, &buflen)) // Non-blocking
    {
        int i;
        // Message with a good checksum received, dump it.
        Serial.println((char*)buf);         
    }

    if (strcmp((char*)buf, "stack") == 0)
      {
          Serial.println("stack");
          stack();
      }
    if (strcmp((char*)buf, "launch") == 0)
      {
          Serial.println("launch");
          launch();
      }
    if (strcmp((char*)buf, "down") == 0)
      {
          Serial.println("down");
          moveStepperTo(currentPos - 20);
      }
    if (strcmp((char*)buf, "up") == 0)
      {
          Serial.println("up");
          moveStepperTo(currentPos + 20);
      }
    if (strcmp((char*)buf, "openClose") == 0)
      {
          Serial.println("openClose");
          chopsticksOpenClose();
      }
    if (strcmp((char*)buf, "sideToSide") == 0)
      {
          Serial.println("sideToSide");
          sideToSide();
      }
    if (strcmp((char*)buf, "shipQD") == 0)
      {
          Serial.println("sqd");
          sqd();
      }
}

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){
    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){
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    analogWrite(enA, speed);
    }
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 

}

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

  }

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

}

void sqd(){
  if(sqdState == 1){
    qdRetract();
  }

  if(sqdState == 2){
    qdAttach();
  }
  
}

void qdAttach() {
    easeServo(12, 10, 1000); // Move to position with easing over 1000 ms
    easeServo(11, 145, 1000); // Move to position with easing over 1000 ms
}

void qdRetract() {
    easeServo(12, 70, 1000); // Move to position with easing over 1000 ms
    easeServo(11, 45, 1000); // Move to position with easing over 1000 ms
}

void chopsticksOpenClose(){
  if(chopstickOpen == 1 && chopsticksLeft == 1){ //left open
      moveChopsticksLeft();
    }
  if(chopstickOpen == 1 && chopsticksLeft == 2){ //center open
      chopsticksClose();
    }
  if(chopstickOpen == 2 && chopsticksLeft == 1){ //left closed
      chopsticksLeftOpen();
    }
  if(chopstickOpen == 2 && chopsticksLeft == 2){ //center closed
      chopsticksOpen();
    }
  if(chopsticksOpen == 0 || chopsticksLeft == 0){
    error();
  }
}

void sideToSide(){
  if(chopstickOpen == 1 && chopsticksLeft == 1){ //left open
      chopsticksOpen();                          //center open
    }
  if(chopstickOpen == 1 && chopsticksLeft == 2){ //center open
      chopsticksLeftOpen();                      //left open
    }
  if(chopstickOpen == 2 && chopsticksLeft == 1){ //left closed
      chopsticksClose();                         //center closed
    }
  if(chopstickOpen == 2 && chopsticksLeft == 2){ //center closed
      moveChopsticksLeft();                      //left closed
    }
  if(chopsticksOpen == 0 || chopsticksLeft == 0){
    error();
  }
}

void chopsticksOpen(){
  easeServos(9, 90, 10, 90, 1000);
}

void chopsticksClose(){ // acts as close and center
  easeServos(9, 45, 10, 45, 1000);
}

void chopsticksLeftOpen(){
  easeServos(9, 44, 10, 135, 1000);
}

void moveChopsticksLeft(){// Left closed
  easeServos(9, 0, 10, 90, 1000);
}

void moveStepperTo(float targetPos) {
    // Calculate how far to move the motor
    int stepsToMove = (targetPos - currentPos) * gearReduction;

    // Check if the motor will pass between positions 200 and 300
    if ((currentPos < 200 && targetPos > 200) || (currentPos < 300 && targetPos > 300) ||
        (currentPos > 300 && targetPos < 300) || (currentPos > 200 && targetPos < 200)) {
        // Move the servo on pin 12 to 70 degrees
        qdRetract;
    }

    // Move the motor in small steps and check limit switches frequently
    int stepSize = (stepsToMove > 0) ? 1 : -1;
    for (int i = 0; i < abs(stepsToMove); i++) {
        // Check the limit switches
        if (digitalRead(chopstickTop) == HIGH || digitalRead(chopstickBottom) == HIGH) {
            break; // Stop the motor if a limit switch is pressed
        }
        myStepper.step(stepSize);
        currentPos += stepSize;
    }
}

void easeServo(int pin, int targetPos, int duration) {
    int startPos = pulseWidthToDegrees(analogRead(pin));
    int delta = targetPos - startPos;
    unsigned long startTime = millis();
    
    while (millis() - startTime < duration) {
        float t = float(millis() - startTime) / duration;
        float easedT = t * t * t; // Cubic easing in
        int currentPos = startPos + easedT * delta;
        analogWrite(pin, degreesToPulseWidth(currentPos));
        delay(15); // Update rate (you can adjust this)
    }
    analogWrite(pin, degreesToPulseWidth(targetPos)); // Ensure we end exactly at the target position
}

void easeServos(int pin1, int targetPos1, int pin2, int targetPos2, int duration) {
    int startPos1 = pulseWidthToDegrees(analogRead(pin1));
    int delta1 = targetPos1 - startPos1;
    int startPos2 = pulseWidthToDegrees(analogRead(pin2));
    int delta2 = targetPos2 - startPos2;
    unsigned long startTime = millis();
    
    while (millis() - startTime < duration) {
        float t = float(millis() - startTime) / duration;
        float easedT = t * t * t; // Cubic easing in
        int currentPos1 = startPos1 + easedT * delta1;
        int currentPos2 = startPos2 + easedT * delta2;
        analogWrite(pin1, degreesToPulseWidth(currentPos1));
        analogWrite(pin2, degreesToPulseWidth(currentPos2));
        delay(15); // Update rate (you can adjust this)
    }
    analogWrite(pin1, degreesToPulseWidth(targetPos1)); // Ensure we end exactly at the target position
    analogWrite(pin2, degreesToPulseWidth(targetPos2)); // Ensure we end exactly at the target position
}

int pulseWidthToDegrees(int pulseWidth) {
    return map(pulseWidth, 544, 2400, 0, 180); // Adjust if your servos have a different range
}

int degreesToPulseWidth(int degrees) {
    return map(degrees, 0, 180, 544, 2400); // Adjust if your servos have a different range
}

void led() {
    static uint8_t offset = 0;  // Offset for the moving red pattern
    static unsigned long lastUpdate = 0;
    unsigned long now = millis();

    // Update the LED pattern every 10 milliseconds
    if (now - lastUpdate >= 10) {
        lastUpdate = now;

        // Clear the strip
        fill_solid(leds, NUM_LEDS, CRGB::Black);

        // Set every other LED to a dimming and color-changing red with an offset
        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; // Increase green to make the color yellower as it travels down
                leds[i] = CRGB(red, green, 0);
                leds[i].fadeLightBy(255 - brightness);
            }
        }

        // Move the colored LEDs down the strip with fading
        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; // Reset the first position

        // Frequently add a new random colored runner
        if (random(0, 4) > 2) { // Increased frequency here
            coloredPositions[0] = true;
            // Generate a random reddish color with less yellow
            uint8_t red = random(128, 256); // Red component between 128 and 255
            uint8_t green = random(0, 64); // Green component between 0 and 63 for less yellow
            coloredColors[0] = CRGB(red, green, 0);
        }

        // Show the LEDs
        FastLED.show();

        // Increment the offset to create the moving effect
        offset++;

        // Wrap around the offset
        if (offset >= 2) {
            offset = 0;
        }
    }
}

void error(){

}