//
// Tyrone Crawler & Engine Start Code
//
// This sketch contains a combination of the crawler and engine start code in a single sketch
//

// Notes from crawler code ...
//
// Some of this code needs to be eliminated or integrated into the other codes where the chute comes into play.
// ENB,IN3 and IN4 are not gonna be used. I installed another BTS7960 to carry the load for the chute actuator.
//
// Chute with directional memory: max 3.5s CW or CCW from center, total 7s range
// Actuator lift (CH7 only)
// Big Ben Crawler + Blower integration with persistent lift in reverse and persistent lower in forward
//

// Defines to control serial debug output statements (0 = disable, 1 = enable)
#define DEBUG_PRINT_FLYSKY_INPUTS                 1
#define DEBUG_PRINT_ENGINE_STATE_INFO             0
#define DEBUG_PRINT_CLUTCH_STATE_INFO             0
#define DEBUG_PRINT_CRAWLER_MOVEMENT_INFO         1
#define DEBUG_PRINT_BLOWER_UP_DOWN_INFO           0
#define DEBUG_PRINT_CHUTE_ROTATION_INFO           0

// Define to activate Wokwi.com simulation code (0 = disable, 1 = enable)
// See https://wokwi.com/projects/433580030820482049
#define WOKWI_SIMULATION                          1

// Include needed libraries
#include <SoftwareSerial.h>
#include "SabertoothSimplified.h"

// Sabertooth S1 Motor Driver
#define S1_PIN 5                       // Sabertooth S1

// Flysky FS-i6X 10 Channel RC Transmitter and FS-iA6B Receiver 
#define INPUT_CH1_PIN 2                // Flysky Right Gimbal (forward-reverse & left-right control)
#define INPUT_CH2_PIN 3                // Flysky Right Gimbal (forward-reverse & left-right control)
// CH3 is connected directly to engine throttle (Flysky Left Gimbal) 
// CH4 is connected directly to engine choke (Flysky Left Gimbal) 
#define INPUT_CH5_PIN 35               // Flysky Switch 5 (Engine start/stop)
#define INPUT_CH6_PIN 36               // Flysky Switch 6 (Clutch control)
#define INPUT_CH7_PIN A6               // Flysky Var7 knob (Chute deflection angle control)
#define INPUT_CH8_PIN A7               // Flysky Var8 knob (Chute rotation control)

#define FLYSKY_TIMEOUT_VALUE 35000     // Timeout is in microseconds   

// 100 amp alternator tells the Arduino engine is running and kicks the starter off. 
#define CURRENT_SENSOR_PIN A5   // ACS758 current sensor
#define STARTER_RELAY_PIN 22    // Starter relay pin
#define KILL_RELAY_PIN 23       // Kill relay pin
#define CLUTCH_RELAY_PIN 24     // Clutch relay pin

// L298N pins
#define ENA 13
#define IN1 A0
#define IN2 A1
#define ENB 7
#define IN3 A2
#define IN4 A3

// BTS7960 blower actuator
#define R_PWM 6
#define L_PWM 9
#define R_EN 11
#define L_EN 12

// Create the Sabertooth object ST
SoftwareSerial SWSerial(NOT_A_PIN, S1_PIN);
SabertoothSimplified ST(SWSerial);

//
// Global variable declarations and initialization
//

// Engine variables
bool starterActive = false;
bool killActive = false;
bool engineRunning = false;
bool engineWasRunning = false;

unsigned long startTime = 0;
unsigned long killTime = 0;

bool lastEngineRunStopSwitch = false;

// Chute variables
#define CHUTE_MAX_TIME 3500 // 3500 is 3.5 seconds
int initialCh8 = 1500;
int chutePosition = 0;
unsigned long chuteLastUpdate = 0;
unsigned long lastCh8Pulse = 0;
bool knobMoved = false;
bool blowerLiftMode = false;
bool blowerLowerMode = false;

//
// Setup function
//

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

  // Engine pin assignment & setup
  pinMode(STARTER_RELAY_PIN, OUTPUT);
  pinMode(KILL_RELAY_PIN, OUTPUT);
  pinMode(CLUTCH_RELAY_PIN, OUTPUT);

  digitalWrite(STARTER_RELAY_PIN, LOW);
  digitalWrite(KILL_RELAY_PIN, LOW);
  digitalWrite(CLUTCH_RELAY_PIN, LOW);  // Clutch disengaged

  // FS-iA6B Receiver pin assignments
  pinMode(INPUT_CH1_PIN, INPUT);
  pinMode(INPUT_CH2_PIN, INPUT);
  pinMode(INPUT_CH5_PIN, INPUT);
  pinMode(INPUT_CH6_PIN, INPUT);
  pinMode(INPUT_CH7_PIN, INPUT);
  pinMode(INPUT_CH8_PIN, INPUT);

  // L298N pin assignment
  // IN1, IN2 and ENA control the chute motor
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  // IN3, IN4 and ENB control the chute rotation motor
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Disable the chute motor on powerup
  digitalWrite(ENA, LOW);  

  // BTS7960 blower actuator pin assignment
  pinMode(R_PWM, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(L_EN, OUTPUT);

  // Enable the blower motor actuator on powerup
  digitalWrite(R_EN, HIGH);
  digitalWrite(L_EN, HIGH);

  chuteLastUpdate = millis();
  lastCh8Pulse = chuteLastUpdate;

  delay(500);
  initialCh8 = pulseIn(INPUT_CH8_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  if (initialCh8 == 0) initialCh8 = 1500;

  Serial.println("Setup complete - System Ready.");
}


//
// Main Loop function
//

void loop() {

  // Read inputs from Flysky receiver
  long ch1RawValue = pulseIn(INPUT_CH1_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch2RawValue = pulseIn(INPUT_CH2_PIN, HIGH, FLYSKY_TIMEOUT_VALUE); 
  long ch5Value = pulseIn(INPUT_CH5_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch6Value = pulseIn(INPUT_CH6_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch7Value = pulseIn(INPUT_CH7_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch8Value = pulseIn(INPUT_CH8_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);

  // Wokwi simulated Flysky receiver channel inputs
  #if WOKWI_SIMULATION
    int inputA14 = analogRead(A14); 
    ch1RawValue = map(inputA14, 0, 1023, 900, 2100); // This range allows for out of range testing
    int inputA15 = analogRead(A15); 
    ch2RawValue = map(inputA15,0 , 1023, 900, 2100); // This range allows for out of range testing   
  #endif

  // Change Ch1 and Ch2 to a failsafe value of 1500 if raw value is outside expected range.
  // This is done because a value of 1500 for both results in a "stop" movement for the clawler.  
  // While the raw value should be in the range of 1000 to 2000, real-world observation found
  // that values are sometime just outside those limits.  So extend the lower and upper limits
  // by 50 when checking for out of range inputs.
  long ch1Value = setToDefaultIfOutOfRange(ch1RawValue, 950, 2050, 1500);
  long ch2Value = setToDefaultIfOutOfRange(ch2RawValue, 950, 2050, 1500);

  // Constrain the value of all channel inputs to 1000 to 2000.  If the transmitter or receiver 
  // are off, a value of 0 is see by the Arduino with the pulseIn function. This action will change
  // a 0 or any out of range value to 1000, which is a "safe" value for Ch5, Ch6, Ch7 and Ch8.
  ch1Value = constrain(ch1Value, 1000, 2000);
  ch2Value = constrain(ch2Value, 1000, 2000);
  ch5Value = constrain(ch5Value, 1000, 2000);
  ch6Value = constrain(ch6Value, 1000, 2000);
  ch7Value = constrain(ch7Value, 1000, 2000);
  ch8Value = constrain(ch8Value, 1000, 2000);

  // Debug code to print Flysky channel inputs on serial monitor
  #if DEBUG_PRINT_FLYSKY_INPUTS
    Serial.print("Flysky Receiver:  Ch1="); Serial.print(ch1Value);
    Serial.print(" | Ch1Raw="); Serial.print(ch1RawValue);
    Serial.print(" | Ch2="); Serial.print(ch2Value); 
    Serial.print(" | Ch2Raw="); Serial.print(ch2RawValue);
    Serial.print(" | Ch5="); Serial.print(ch5Value);
    Serial.print(" | Ch6="); Serial.print(ch6Value);
    Serial.print(" | Ch7="); Serial.print(ch7Value);
    Serial.print(" | Ch8="); Serial.print(ch8Value);
    Serial.println();
  #endif

  // Engine and clutch control
  bool engineRunStopSwitch = (ch5Value > 1700);       // Channel 5 is the engine run/stop switch
  bool engageClutchSwitch = (ch6Value > 1700);        // Channel 6 is the clutch engage switch

  controlEngine(engineRunStopSwitch);
  controlClutch(engageClutchSwitch);

  // Crawler movement control
  controlCrawlerMovement(ch1Value, ch2Value);

  // Crawler chute deflection angle control
  // controlChuteDeflection(ch7Value);
  
  // Crawler chute rotation and lifting/lowering of blower
  controlChuteRotation(ch8Value);
  controlBlowerLiftingAndLowering(ch1Value, ch2Value);

  // The individual test sketches used a couple different delays.  The engine code had delay(100) and 
  // crawler code was delay(10).  For intial testing, we will go with the longer delay of 100 milliseconds. 
  delay(100); 
}

//
// Support functions
//

long setToDefaultIfOutOfRange(long val, long minVal, long maxVal, long defaultVal) {
  return (val < minVal || val > maxVal) ? defaultVal : val;
}


//
// Control functions
//

void controlEngine(bool engineRunStopSwitch) {

  // The engineRunStopSwitch is true when this switch is in the on position.  When the engine is not
  // running, moving this switch to the "on" position to engage the starter to crank the engine to start
  // it.  After the engine is running, the operator will move this switch back to the "off" position.
  // When the engine is running, moving this switch to the "on" position will stop the engine.  After
  // the engine has stopped, the operator will move this switch back to the "off" position.

  #define ENGINE_START_DURATION 3000         // 3 seconds
  #define ENGINE_KILL_DURATION 2000          // 2 seconds
  #define ENGINE_RUNNING_THRESHOLD 3.0       // 3 Amps - adjust if needed
  
  // === Read current sensor to determine is engine is running ===
  int rawValue = analogRead(CURRENT_SENSOR_PIN);
  float voltage = (rawValue / 1023.0) * 5.0;
  float currentAmps = (voltage - 2.5) / 0.066;
  if (abs(currentAmps) < 1.0) currentAmps = 0.0;

  engineRunning = currentAmps >= ENGINE_RUNNING_THRESHOLD;
  if (engineRunning) engineWasRunning = true;

  // === Engine START logic ===
  if (engineRunStopSwitch && !lastEngineRunStopSwitch && !engineWasRunning) {
    digitalWrite(STARTER_RELAY_PIN, HIGH);
    startTime = millis();
    starterActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO 
      Serial.println("Cranking engine...");
    #endif
  }

  if (starterActive && (millis() - startTime >= ENGINE_START_DURATION || engineRunning)) {
    digitalWrite(STARTER_RELAY_PIN, LOW);
    starterActive = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO 
      Serial.println("Starter OFF");
    #endif    
  }

  // === Engine STOP logic ===
  if (!engineRunStopSwitch && lastEngineRunStopSwitch && engineWasRunning) {
    digitalWrite(KILL_RELAY_PIN, HIGH);
    killTime = millis();
    killActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO 
      Serial.println("Killing engine...");
    #endif
  }

  if (killActive && millis() - killTime >= ENGINE_KILL_DURATION) {
    digitalWrite(KILL_RELAY_PIN, LOW);
    killActive = false;
    engineWasRunning = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO 
      Serial.println("Kill relay OFF - engine reset.");
    #endif
  }

  #if DEBUG_PRINT_ENGINE_STATE_INFO    
    Serial.print("Switch 5 (Start): "); Serial.print(engineRunStopSwitch ? "ON" : "OFF");
    Serial.print(" | Amps: "); Serial.print(currentAmps, 2);
    Serial.print(" | Engine: "); Serial.println(engineWasRunning ? "ON" : "OFF");
  #endif
    
  lastEngineRunStopSwitch = engineRunStopSwitch;
}


void controlClutch(bool engageClutch) {

  // === Clutch control ===
  digitalWrite(CLUTCH_RELAY_PIN, engageClutch ? HIGH : LOW);
  #if DEBUG_PRINT_CLUTCH_STATE_INFO    
    Serial.print("Clutch: ");
    Serial.println(engageClutch ? "ENGAGED" : "DISENGAGED");
  #endif  
}


void disabled_controlChuteDeflection(long ch7) {
  if (ch7 > 1600) {
    digitalWrite(ENA, HIGH);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (ch7 < 1400) {
    digitalWrite(ENA, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(ENA, LOW);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}


void controlChuteRotation(long ch8) {
  unsigned long now = millis();
  float delta = now - chuteLastUpdate;
  chuteLastUpdate = now;
  if (delta > 1000) delta = 1000;

  if (ch8 > 950 && ch8 < 2050) {
    lastCh8Pulse = now;
  } else if (now - lastCh8Pulse > 500) {
    ch8 = 1500;
  }

  // After power-on, wait for the knob to move a bit before 
  // actaully using it's value to rotate the chute 
  if (!knobMoved && abs(ch8 - initialCh8) > 30) {
    knobMoved = true;
  }

  if (knobMoved) {
    if (ch8 > 1600) {
      if (chutePosition < CHUTE_MAX_TIME) {
        digitalWrite(ENB, HIGH);
        digitalWrite(IN3, HIGH);
        digitalWrite(IN4, LOW);
        chutePosition += delta;
        if (chutePosition > CHUTE_MAX_TIME) chutePosition = CHUTE_MAX_TIME;
      } else {
        stopChute();
      }
    } else if (ch8 < 1400) {
      if (chutePosition > -CHUTE_MAX_TIME) {
        digitalWrite(ENB, HIGH);
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, HIGH);
        chutePosition -= delta;
        if (chutePosition < -CHUTE_MAX_TIME) chutePosition = -CHUTE_MAX_TIME;
      } else {
        stopChute();
      }
    } else {
      stopChute();
    }
  } else {
    stopChute();
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print("Chute Position: ");
    Serial.println(chutePosition);
  #endif
}

void stopChute() {
  digitalWrite(ENB, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}


void controlBlowerLiftingAndLowering(long ch1, long ch2) {
  static bool wasReverse = false;
  static bool wasForward = false;

  if (ch1 < 1460 && ch2 < 1460) {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 255);
    wasReverse = true;
    wasForward = false;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO     
      Serial.println("Lifting Blower (triggered by reverse)");
    #endif
  } else if (wasReverse) {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 255);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO 
      Serial.println("Lifting Blower (persisting reverse)");    
    #endif
    if (ch1 > 1530 && ch2 > 1530) {
      wasReverse = false;
    }
  } else if (ch1 > 1530 && ch2 > 1530) {
    analogWrite(R_PWM, 255);
    analogWrite(L_PWM, 0);
    wasForward = true;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO     
      Serial.println("Lowering Blower (triggered by forward)");
    #endif
  } else if (wasForward) {
    analogWrite(R_PWM, 255);
    analogWrite(L_PWM, 0);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO     
      Serial.println("Lowering Blower (persisting forward)");
    #endif
    if (ch1 < 1460 && ch2 < 1460) {
      wasForward = false;
    }
  } else {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 0);
  }
}


void controlCrawlerMovement(long ch1, long ch2) {
  // controlCrawler has been updated to turn the correct direction
  // instead of the opposite direction as it originally did 

  if (ch1 > 1530 && ch2 > 1530) {
    // Move straight forward
    int speedRight = map(ch1, 1530, 2000, 0, 127);
    int speedLeft = map(ch2, 1530, 2000, 0, 127);
    ST.motor(1, min(speedRight, speedLeft));
    ST.motor(2, min(speedRight, speedLeft));
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 
      Serial.print("Crawler Movement: Move straight forward  | Speed="); Serial.println(min(speedRight, speedLeft));
    #endif
  } else if (ch1 > 1530 && ch2 < 1460) {
    // Turn right
    // int speedRight = map(ch1, 1530, 2000, 0, 127);  -- Turns left
    // int speedLeft = map(ch2, 1000, 1460, -127, 0);  -- Turns left
    int speedRight = map(ch1, 1530, 2000, 0, -127);
    int speedLeft = map(ch2, 1000, 1460, 127, 0);
    ST.motor(1, speedRight);
    ST.motor(2, speedLeft);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 
      Serial.print("Crawler Movement: Turning right  | Speed: Left="); Serial.print(speedLeft); Serial.print(" Right=");Serial.println(speedRight);
    #endif
  } else if (ch1 < 1460 && ch2 > 1530) {
    // Turn left
    // int speedRight = map(ch1, 1000, 1460, -127, 0); -- Turns right
    // int speedLeft = map(ch2, 1530, 2000, 0, 127);   -- Turns right
    int speedRight = map(ch1, 1000, 1460, 127, 0);
    int speedLeft = map(ch2, 1530, 2000, 0, -127);
    ST.motor(1, speedRight);
    ST.motor(2, speedLeft);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 
      Serial.print("Crawler Movement: Turning left  | Speed: Left="); Serial.print(speedLeft); Serial.print(" Right=");Serial.println(speedRight);
    #endif
  } else if (ch1 < 1460 && ch2 < 1460) {
    // Move straight back
    int speedRight = map(ch1, 1000, 1460, -127, 0);
    int speedLeft = map(ch2, 1000, 1460, -127, 0);
    ST.motor(1, max(speedRight, speedLeft));
    ST.motor(2, max(speedRight, speedLeft));
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 
      Serial.print("Crawler Movement: Move straight backward  | Speed="); Serial.println(max(speedRight, speedLeft));
    #endif
  } else {
    // Stop
    ST.motor(1, 0);
    ST.motor(2, 0);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 
      Serial.println("Crawler Movement: Stop  |  Speed=0");
    #endif
  }
}