// simulation flags
bool simflag1;

// Libraries
#include <AccelStepper.h>
#include <FastLED.h>
#include <Wire.h>

// LED No. and Pins
#define NUM_LEDS_A 23 // RH Inlet
#define DATA_PIN_A 8 //
// #define DATA_PIN_A 1 //
#define NUM_LEDS_B 6 // RH Cylinder
#define DATA_PIN_B 3
#define NUM_LEDS_C 4 // RH Exhaust
#define DATA_PIN_C 5
#define NUM_LEDS_D NUM_LEDS_A  // LH Inlet
#define DATA_PIN_D 2
#define NUM_LEDS_E NUM_LEDS_B  // LH Cylinder
#define DATA_PIN_E 4
#define NUM_LEDS_F NUM_LEDS_C  // LH Exhaust ADDED "F" LH EXHAUST
#define DATA_PIN_F 6 // ADDED "F" LH EXHAUST

// Arrays for LED colors
CRGB ledsA[NUM_LEDS_A];
CRGB ledsB[NUM_LEDS_B];
CRGB ledsC[NUM_LEDS_C];
CRGB ledsD[NUM_LEDS_D];
CRGB ledsE[NUM_LEDS_E];
CRGB ledsF[NUM_LEDS_F]; // ADDED "F" LH EXHAUST

// Solenoid Variables
const int solenoidPin = 7; // Solenoid connected to digital pin 6
unsigned long previousMillis_VALVE = 0;


// Motor interface type for a driver
AccelStepper stepper(AccelStepper::DRIVER, 10, 9); // Step pin 10, direction pin 9

// Switch and Button Pins
const int homeSwitchPin = 25;
const int startButtonPin = 24; // Start button connected to pin 24

// Motor Variables
unsigned long previousMillis_MOTOR = 0; // Stores the last time the motor direction was changed
bool direction = true; // Direction of rotation

const int motorMaxspeedhome = 1000; // DO NOT CHANGE
const int motorAccelerationhome = 1000; // DO NOT CHANGE

float motorMaxspeed = 1900; // Motor max speed - CAN BE ADJUSTED FROM - TO -

float motorAcceleration = 30000; // Motor acceleration - DO NOT CHANGE
float motorSteps = 5000; // Motor Steps per 1/2 cycle - DO NOT CHANGE

// LED Interval Constants
float Ta = motorMaxspeed / motorAcceleration;
float Sa = Ta * motorMaxspeed / 2;
float Sc = motorSteps - (Sa * 2);
float Tc = Sc / motorMaxspeed;

float motorHalfcycle = (((motorSteps - (((motorMaxspeed / motorAcceleration) * motorMaxspeed / 2) * 2)) / motorMaxspeed) + (2 * (motorMaxspeed / motorAcceleration)) + 0.025) * 1000;
float motorFullcycle = motorHalfcycle * 2;

const int combustionInterval = motorHalfcycle;
int combustionPreviousMillis = -combustionInterval;
const int combustionFlashInterval = motorHalfcycle / 12;
int combustionFlashPreviousMillis = 0;

unsigned long previousMillis = 0;  // Stores the last time the LED was updated
const long interval = 100;         // Interval at which to update the LEDs (milliseconds)
int currentLED = 0;                // Index of the current LED to light up or turn off
bool stripBInitialized = false;    // Flag to check if strip B is initialized
bool lightingUp = true;            // Direction of the sequence: true for lighting up, false for turning off
int currentIndex = 0;

unsigned long fadePreviousMillis = 0;
const long fadeInterval = 33;
int fadeIndex = 0;
int totalUpdates = 100; // Total fade updates
int ledOffInterval = 27; // Compression Timing
int combustionFlashCount = 0;
int combustionLEDIndex = NUM_LEDS_B - 1;

// Motor Booleans
bool motorStarted = false;
bool homingComplete = false; // Flag to ensure homing happens only once
bool additionalMoveComplete = false; // Flag to ensure the additional move is done only once
bool motorCycle = false;
bool startLEDCycle = false;
bool Test = false;
bool combustionCycleB = false;
bool Initialize = false;
bool motorCompletedThisCycle = false;

// LED Variables
unsigned long currentMillis = 0; // last time cycle began for RH side LEDS
unsigned long previousmotorMillis = 0; // last time cycle began for LH side LEDS

// LED States
enum class State {
  CombustionRH,
  CompressionLH,
  InletRH,
  ScavengeRH,
  CombustionLH,
  CompressionRH,
  InletLH,
  ScavengeLH,
  ExhaustLH,
  Initialize,
  StripA_Blue,
  StripB_ToBlue,
  StripC_Red,
  TurnOff_AC,
  FadeStripB,
  CombustionSequence,
  Done
};
State currentState = State::Initialize;

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

  pinMode(homeSwitchPin, INPUT_PULLUP);
  pinMode(startButtonPin, INPUT_PULLUP);
  pinMode(solenoidPin, OUTPUT);

  FastLED.addLeds<WS2812B, DATA_PIN_A, GRB>(ledsA, NUM_LEDS_A);
  FastLED.addLeds<WS2812B, DATA_PIN_B, GRB>(ledsB, NUM_LEDS_B);
  FastLED.addLeds<WS2812B, DATA_PIN_C, GRB>(ledsC, NUM_LEDS_C);
  FastLED.addLeds<WS2812B, DATA_PIN_D, GRB>(ledsD, NUM_LEDS_D);
  FastLED.addLeds<WS2812B, DATA_PIN_E, GRB>(ledsE, NUM_LEDS_E);
  FastLED.addLeds<WS2812B, DATA_PIN_F, GRB>(ledsF, NUM_LEDS_F); // ADDED "F" LH EXHAUST
  FastLED.clear();

  fill_solid(ledsA, NUM_LEDS_A, CRGB::Blue); // Strip A Blue
  fill_solid(ledsB, NUM_LEDS_B, CRGB::Red); // Strip B Red
  fill_solid(ledsC, NUM_LEDS_C, CRGB::Red); // Strip C Red
  fill_solid(ledsD, NUM_LEDS_D, CRGB::Red); // Strip D Red
  fill_solid(ledsE, NUM_LEDS_E, CRGB::Blue); // Strip E Red
  fill_solid(ledsF, NUM_LEDS_F, CRGB::Blue); // Strip F Blue // ADDED "F" LH EXHAUST

  FastLED.show();
}

void loop() {

  // Check if start button is pressed and motor hasn't started yet
  // if (digitalRead(startButtonPin) == HIGH && !motorStarted) {
  if (digitalRead(startButtonPin) == LOW && !motorStarted) {
    delay(500); // Debounce delay
    motorStarted = true; // Prevent re-entry
  }

  // Perform homing if it hasn't been done yet and the motor has started
  if (motorStarted && !homingComplete) {
    // Set speed and acceleration for homing
    stepper.setMaxSpeed(motorMaxspeedhome); // max speed for faster homing
    stepper.setAcceleration(motorAccelerationhome); // acceleration for faster homing

    stepper.setSpeed(-motorMaxspeedhome); // speed for homing
    // Homing procedure: move continuously until the home switch is triggered
    while (digitalRead(homeSwitchPin) == HIGH) {
      stepper.runSpeed(); // Move continuously
    }

    stepper.stop(); // Stop the motor once the home switch is triggered
    stepper.setCurrentPosition(0); // Set current position as 0

    // Reset speed and acceleration to normal operation values
    stepper.setMaxSpeed(motorMaxspeed); // Reset max speed
    stepper.setAcceleration(motorAcceleration); // Reset acceleration

    homingComplete = true; // Set homing complete flag
  }

  // After homing, move an additional 370 steps
  if (homingComplete && !additionalMoveComplete) {
    stepper.move(370); // Move 370 steps away from the switch
    while (stepper.distanceToGo() != 0) {
      stepper.run(); // Continuously run until the motor reaches the target position
    }
    additionalMoveComplete = true; // Mark the additional move as complete
  }

  // After homing, control the motor based on logic
  if (homingComplete) {
    unsigned long currentmotorMillis;
    if (currentmotorMillis - previousmotorMillis >= 1) {
      previousmotorMillis = currentmotorMillis;
      if (stepper.distanceToGo() == 0) {
        direction = !direction;
        long stepsToMove = direction ? -5000 : 5000;
        stepper.move(stepsToMove);
      }
      stepper.run();
    }
  }
  unsigned long currentMillis = millis();
  switch (currentState) {
    case State::Initialize:
      currentIndex = 0; // Reset index for the first operation
      if (motorStarted && !startLEDCycle) {
        currentState = State::CombustionRH;
        startLEDCycle = true;
      }
      break;

    case State::CombustionRH: //COMBUSTION
      if (currentMillis - previousMillis >= combustionFlashInterval) {
        previousMillis = currentMillis;
        // Serial.println("CombustionRH");
        combustionRH();
      }
      break;

    case State::InletRH:
      if (currentMillis - previousMillis >= interval && currentIndex < NUM_LEDS_A) {
        previousMillis = currentMillis;
        // Serial.println("InletRH");
        inletRH();
      } else if (currentIndex >= NUM_LEDS_A && currentIndex >= NUM_LEDS_C) {
        currentIndex = 0; // Reset for the next sequence
        currentState = State::ScavengeLH;
      }
      break;

    case State::ScavengeLH: // NEW AIR PUSHING OLD AIR OUT
      if (currentMillis - previousMillis >= interval && currentIndex < NUM_LEDS_B) {
        previousMillis = currentMillis;
        // Serial.println("ScavengeLH");
        scavengeLH();
      } else if (currentIndex >= NUM_LEDS_B) {
        currentIndex = 0; // Reset for the next sequence
        currentState = State::ExhaustLH;
      }
      break;

    case State::ExhaustLH: //EXHUAST
      if (currentMillis - previousMillis >= interval && currentIndex < NUM_LEDS_C) {
        previousMillis = currentMillis;
        ledsC[currentIndex] = CRGB::Red;
        FastLED.show();
        currentIndex++;
      } else if (currentIndex >= NUM_LEDS_C) {
        fill_solid(ledsA, NUM_LEDS_A, CRGB::Black); // Turn off strip A
        fill_solid(ledsC, NUM_LEDS_C, CRGB::Black); // Turn off strip C
        FastLED.show();
        currentState = State::CompressionRH;
        totalUpdates = ledOffInterval * NUM_LEDS_B;
        fadeIndex = 0;
      }
      break;

    case State::CompressionRH:
      if (currentMillis - fadePreviousMillis >= fadeInterval) {
        fadePreviousMillis = currentMillis;
        // Serial.println("CompressionRH");
        compressionRH();
      }
      if (!motorCompletedThisCycle && stepper.distanceToGo() == 0) {
        motorCompletedThisCycle = true;
      }
      break;

    case State::Done: //LOOP AGAIN
      currentState = State::Initialize;
      motorStarted = false;
      motorCompletedThisCycle = false;
      break;
  }

  if (motorStarted && !motorCompletedThisCycle) {
    stepper.run();
  }
}

void startLED() {
  startLEDCycle = true;
  currentState = State::CombustionRH;
}

void combustionRH() {
  if (combustionFlashCount < 2 * NUM_LEDS_B) {
    int ledToFlash = combustionLEDIndex - (combustionFlashCount / 2);
    if (ledToFlash >= 0 && ledToFlash < NUM_LEDS_B) {
      for (int j = ledToFlash; j < NUM_LEDS_B; j++) {
        ledsB[j] = (combustionFlashCount % 2 == 0) ? CRGB::Red : CRGB::Black;
      }
    }
    FastLED.show();
    combustionFlashCount++;
    if (combustionFlashCount % 2 == 0 && ledToFlash >= 0) {
      ledsB[ledToFlash] = CRGB::Red; // Keep the LED on red after flashing
    }
  } else {
    currentState = State::InletRH  ; // All LEDs have flashed
  }
}

void inletRH() {
  if (currentIndex < NUM_LEDS_A) {
    ledsA[currentIndex] = CRGB::Blue;
  }

  if (currentIndex == 0) {
    digitalWrite(solenoidPin, HIGH); // Open the solenoid
  }

  // Simultaneously start the sequence on Strip C
  if (currentIndex < NUM_LEDS_C) { // Check to ensure we don't overflow Strip C
    ledsC[currentIndex] = CRGB::Red; // Start Strip C sequence alongside Strip A
  }

  if (currentIndex == 0) {
    fill_solid(ledsB, NUM_LEDS_B, CRGB::Red); // Initialize strip B
  }

  FastLED.show();
  currentIndex++;
}

void scavengeLH () {
  if (currentIndex >= 3) {
    ledsC[currentIndex - 3] = CRGB::Black;
  }

  if (currentIndex < NUM_LEDS_C) { // Check to ensure we don't overflow Strip C
    ledsC[currentIndex] = CRGB::Red; // Start Strip C sequence alongside Strip A
  }
  ledsB[currentIndex] = CRGB::Blue;
  FastLED.show();
  currentIndex++;
}

void compressionRH() {
  if (fadeIndex <= totalUpdates) {
    for (int i = 0; i < NUM_LEDS_B; i++) {
      // Fading logic FOR COMPRESSION
      if (fadeIndex >= i * ledOffInterval && i != NUM_LEDS_B - 1) {
        ledsB[i] = CRGB::Black;
      } else {
        ledsB[i].r = map(fadeIndex, 0, totalUpdates, 0, 80);
        ledsB[i].g = 0;
        ledsB[i].b = map(fadeIndex, 0, totalUpdates, 255, 80);
      }
    }
    FastLED.show();
    fadeIndex++;
  } else {
    // Proceed to the combustion sequence
    currentState = State::CombustionRH;
    combustionLEDIndex = NUM_LEDS_B - 1;
    combustionFlashCount = 0;
  }
}