/* *************************************************************************************************

 Tyrone Control Software -- PPM Input and Serial Monitor at 1000000 baud rate

 This sketch is an integrated version of the crawler, engine and blower control code in a
 single sketch.

 Change Log:

   v12 - Changed from PWM inputs to PPM inputs and added code to control throttle and choke
         Added code to iterate main loop at a fix iteration rate

   v13 - Changed processing rate to 10 Hz
         Added filtering to address servo dithering on throttle and choke
         Added code to set default value of 1500 for channels 3 and 4
         Added debug output for throttle and choke servo data

   v14 - Changed from SoftwareSerial to Serial1 for connection to Sabertooth S1 Motor Driver
         Changed servo dithering threshold from 2 degrees to 5 degrees

   v15 - Added auto choke functionality to automatically control choke during engine startup

   v16 - Clarified comments & debug output relate to how engine start/stop code really works, Ch5
           input is like a light switch which is either in the engine on or off position
         Simplified the logic associated with the engineRunning variable and eliminated the
           engineWasRunning variable as it seemed redundent and confusing
         In engine start/stop code, improved readability of ACS758 sensor code and modified code
           so that a negative currentAmps can never occur since it makes no sense
         Added 1 second debounce to engine run/stop and clutch engage input switches to prevent
           a glitch from accidential activation
         Changed auto choke duration after engine start from 5 seconds to 0.5 seconds
         Enhanced auto choke functionality to explicitly disable auto choke if the engine is not
           running
         Flipped engine throttle input so that "full gas" setting is at 1750 value

   v17 - Added auto throttle functionality to automatically control throttle when clutch is engaged
         Added capability to selectively enable/disable auto choke and auto throttle functionality
         Enhanced engine debug information output

   v18 - Changed ENGINE_RUNNING_THRESHOLD from 3.0 Amps to 2.0 Amps based on observation during
         engine run on 8/1/2025 where current dropped to 2.93 Amps while engine was running.

   v19 - Updated mapping values for engine throttle servo angles based on 7/18/25 testing and
          scales to 1250-1750 input range
         Enhanced debug output to provide reason for turning starter off

   v20 - Revised the order of control functions processing in main loop to start with engine control
           function and then engine-related functions
         Modified logic that determine engine run/stop status to use a moving average of the generator
           current to smooth out the fluctations of individual current readings
         Modified engine kill logic in engine control function to insure it will always kill the
           engine when switch is moved to "Stop" and insured that the starter will not engage and if
           already engaged, will disengage, if the kill relay is active
         Rearrange order of control functions defintions
         Removed Wokwi simulation conditional complication code

   v40 - Replaced the original Crawler control function with a completely new implementation
         generated by ChatGPT.  This new movement control logic that uses three shapers: a center
         deadband (kills stick jitter), soft zero-cross “braking” (prevents instant reversals
         that make tracks grab), and a slew limiter (ramps starts/turns/stops smoothly).  It can
         be tuned using SLEW_STEP, BRAKE_SOFTEN, and RC_DEADBAND for feel.

************************************************************************************************* */

// Defines to control serial debug output statements (0 = disable, 1 = enable)
#define DEBUG_PRINT_FLYSKY_RAW_PPM_INPUTS         0
#define DEBUG_PRINT_INPUT_VALUES                  0
#define DEBUG_PRINT_FRAME_DURATION_INFO           0
#define DEBUG_PRINT_ENGINE_THROTTLE_INFO          0
#define DEBUG_PRINT_ENGINE_CHOKE_INFO             0
#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 DEBUG_PRINT_CHUTE_DEFLECTOR_INFO          0

// Defines to enable Auto Choke and Auto Throttle functions (0 = disable, 1 = enable)
#define ENABLE_AUTO_CHOKE_FUNCTION                1
#define ENABLE_AUTO_THROTTLE_FUNCTION             1

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

// Sabertooth S1 Motor Driver (Serial1 TX Pin is pre-assigned to pin 18)
#define Serial1_TX_PIN 18                       // Sabertooth S1

// Define the PPM Reader for FS-iA6B Receiver
const byte PPM_PIN = 2;
const byte NUM_CHANNELS = 8;
#define PPM_DEFAULT_VALUE 0

PPMReader ppm(PPM_PIN, NUM_CHANNELS);

// Define the RC channel value array for all 8 RC channels.  Since RC channel numbers
// start with 1, the [0] array element will never be used.
int chValue[9];

// Channel assignments - Flysky FS-i6X 10 Channel RC Transmitter and FS-iA6B Receiver
// INPUT CH1 = Flysky Right Gimbal (forward-reverse & left-right control)
// INPUT CH2 = Flysky Right Gimbal (forward-reverse & left-right control)
// INPUT CH3 = Flysky Left Gimbal (engine throttle control - TX configured range 1250 to 1750)
// INPUT CH4 = Flysky Left Gimbal (engine choke control - TX configured range 1000 to 1500)
// INPUT CH5 = Flysky Switch 5 (Engine run/stop)
// INPUT CH6 = Flysky Switch 6 (Clutch control)
// INPUT CH7 = Flysky Var7 knob (Chute deflection angle control)
// INPUT CH8 = Flysky Var8 knob (Chute rotation control)

// Define RC Servo 35kg Servos (270 Degree rotation) for engine throttle and choke control
#define ENGINE_THROTTLE_PIN 48
#define ENGINE_CHOKE_PIN 50

Servo engineThrottleServo, engineChokeServo;

// 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

// BTS7960 Blower Actuator (Raises and lowers the blower assembly)
#define BLOWER_RPWM 6
#define BLOWER_LPWM 9
#define BLOWER_REN 11
#define BLOWER_LEN 12

// BTS7960 Chute Rotation Motor (Rotates blower chute left and right)
#define CHUTE_RPWM 30
#define CHUTE_LPWM 31
#define CHUTE_REN 26
#define CHUTE_LEN 27

// Chute Rotation Motor RS/LS current sense
#define CHUTE_RS_PIN A2   // Right Sense
#define CHUTE_LS_PIN A3   // Left Sense

// BTS7960 Chute Deflector Actuator (Adjusts chute deflector angle)
#define DEFLECTOR_RPWM 44
#define DEFLECTOR_LPWM 45
#define DEFLECTOR_REN 28
#define DEFLECTOR_LEN 29

// Create the Sabertooth object ST connected to Serial1
SabertoothSimplified ST(Serial1);   // Serial1 TX Pin which is pre-assigned to pin 18

//
// Global variable declarations and initialization
//

// Cyclic processing variables
#define FRAME_ITERATION_RATE 10                                 // Hertz
#define FRAME_DURATION_IN_MS (1000/FRAME_ITERATION_RATE)        // Milliseconds
unsigned long currentTime = 0;
unsigned long previousCurrentTime = 0;
unsigned long processingEndTime;
unsigned long frameEndTime = 0;

// Throttle variables
int currentThrottleServoAngle = 0;
#define AUTO_THROTTLE_SETTING 1750      // 1750 is the max value for the Ch3 throttle input

// Choke variables
int currentChokeServoAngle = 0;
bool autoChokeEnabled = false;
long autoChokePWMSetting = 1500;
unsigned long autoChokeEngineStartTime = 0;

// Engine variables
bool starterActive = false;
bool starterWasActive = false;
bool killActive = false;
bool engineRunning = false;
unsigned long startTime = 0;
unsigned long killTime = 0;
bool engineRunStopSwitch = false;
bool lastEngineRunStopSwitch = false;

// Clutch variables
bool engageClutchSwitch = false;
bool clutchEngaged = false;

// Chute variables
int initialCh8 = 1500;
int chutePosition = 0;
unsigned long chuteLastUpdate = 0;
unsigned long lastCh8Pulse = 0;
bool knobMoved = false;
bool chutePositionRiggedStarted = false;
bool chutePositionRigged = false;
bool blowerLiftMode = false;
bool blowerLowerMode = false;

// Structure and states for engine run/stop and engage clutch switch debouncing
struct DebounceState {
  bool lastStableValue;
  unsigned long lastChangeTime;
};

DebounceState engineRunStopSwitchState = {false, 0};
DebounceState engageClutchSwitchState = {false, 0};

//
// Setup function
//

void setup() {
  Serial.begin(1000000);
  Serial.println("Starting initialization ...");

  Serial1.begin(9600);

  // Attach engine throttle and choke servos to pins
  engineThrottleServo.attach(ENGINE_THROTTLE_PIN);
  engineChokeServo.attach(ENGINE_CHOKE_PIN);

  // Engine starter and clutch 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

  // BTS7960 blower actuator pin assignment
  pinMode(BLOWER_RPWM, OUTPUT);
  pinMode(BLOWER_LPWM, OUTPUT);
  pinMode(BLOWER_REN, OUTPUT);
  pinMode(BLOWER_LEN, OUTPUT);

  // Enable the blower actuator on powerup
  digitalWrite(BLOWER_REN, HIGH);
  digitalWrite(BLOWER_LEN, HIGH);

  // BTS7960 chute rotation motor pin assignment
  pinMode(CHUTE_RPWM, OUTPUT);
  pinMode(CHUTE_LPWM, OUTPUT);
  pinMode(CHUTE_REN, OUTPUT);
  pinMode(CHUTE_LEN, OUTPUT);

  // Enable the chute rotation motor on powerup
  digitalWrite(CHUTE_REN, HIGH);
  digitalWrite(CHUTE_LEN, HIGH);

  // Chute rotation current sensor pin assignment
  pinMode(CHUTE_RS_PIN, INPUT);
  pinMode(CHUTE_LS_PIN, INPUT);

  // BTS7960 chute deflection actuator
  pinMode(DEFLECTOR_RPWM, OUTPUT);
  pinMode(DEFLECTOR_LPWM, OUTPUT);
  pinMode(DEFLECTOR_REN, OUTPUT);
  pinMode(DEFLECTOR_LEN, OUTPUT);

  digitalWrite(DEFLECTOR_REN, HIGH);
  digitalWrite(DEFLECTOR_LEN, HIGH);

  // Initialize various "last" chute rotation values
  delay(1000);
  chuteLastUpdate = millis();
  lastCh8Pulse = chuteLastUpdate;
  initialCh8 = ppm.latestValidChannelValue(8, PPM_DEFAULT_VALUE);  // Channels are numbered 1 to 8
  initialCh8 = constrain(initialCh8, 1000, 2000);
  Serial.print("initialCh8="); Serial.println(initialCh8);

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


//
// Main Loop function
//

void loop() {

  // Read the channel PPM inputs from the Flysky receiver
  for (byte ch = 1; ch <= NUM_CHANNELS; ++ch) {
    chValue[ch] = ppm.latestValidChannelValue(ch, PPM_DEFAULT_VALUE);  // Channels are numbered 1 to 8
  }

  // Debug code to print Flysky raw PPM channel inputs on serial monitor
  #if DEBUG_PRINT_FLYSKY_RAW_PPM_INPUTS
    Serial.print("Flysky Raw PPM Inputs: Ch1="); Serial.print(chValue[1]);
    Serial.print(" | Ch2="); Serial.print(chValue[2]);
    Serial.print(" | Ch3="); Serial.print(chValue[3]);
    Serial.print(" | Ch4="); Serial.print(chValue[4]);
    Serial.print(" | Ch5="); Serial.print(chValue[5]);
    Serial.print(" | Ch6="); Serial.print(chValue[6]);
    Serial.print(" | Ch7="); Serial.print(chValue[7]);
    Serial.print(" | Ch8="); Serial.print(chValue[8]);
    Serial.println();
  #endif

  // To simplify integration of PPM input with existing code, define individual
  // input variables using the Flysky PPM receiver input values
  long ch1RawValue = chValue[1];
  long ch2RawValue = chValue[2];
  long ch3RawValue = chValue[3];
  long ch4RawValue = chValue[4];
  long ch5Value = chValue[5];
  long ch6Value = chValue[6];
  long ch7RawValue = chValue[7];
  long ch8Value = chValue[8];

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

  // Adjust Ch3 and Ch4 to a failsafe value of 1500 if raw value is outside expected range.
  // This is done because a value of 1500 for both is the safe, default value.  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.

  // Note - While the full range of Ch3 and Ch4 is 1000 to 2000, the Flysky FS-i6X was configured to
  // limit the ranges when these inputs were directly connected to the throttle and choke servos.
  // When we switched to PPM inputs, we decided to simply leave these limits in place.  So in theory,
  // Ch3 should have always be in the range of 1250 to 1750 and Ch4 should be in the range 1000 to 1500.
  long ch3Value = setToDefaultIfOutOfRange(ch3RawValue, 950, 2050, 1500);
  long ch4Value = setToDefaultIfOutOfRange(ch4RawValue, 950, 2050, 1500);

  // Adjust Ch7 to a failsafe value of 1500 if raw value is outside expected range.
  // This is done because a value of 1500 stops the movement of chute deflector actuator.
  long ch7Value = setToDefaultIfOutOfRange(ch7RawValue, 950, 2050, 1500);

  // ***********************************************************************
  // Consider adding code to condition Ch8 to insure full rotation range.
  // ***********************************************************************

  // Constrain the value of each channel input. If the transmitter or receiver are off, a value of 0
  // is see by the Arduino. 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);
  ch3Value = constrain(ch3Value, 1250, 1750);
  ch4Value = constrain(ch4Value, 1000, 1500);
  ch5Value = constrain(ch5Value, 1000, 2000);
  ch6Value = constrain(ch6Value, 1000, 2000);
  ch7Value = constrain(ch7Value, 1000, 2000);
  ch8Value = constrain(ch8Value, 1000, 2000);

  // Debug code to print modified/conditioned channel inputs on serial monitor
  #if DEBUG_PRINT_INPUT_VALUES
    Serial.print("Modified Input Values: Ch1="); Serial.print(ch1Value);
//    Serial.print(" | Ch1Raw="); Serial.print(ch1RawValue);
    Serial.print(" | Ch2="); Serial.print(ch2Value);
//    Serial.print(" | Ch2Raw="); Serial.print(ch2RawValue);
    Serial.print(" | Ch3="); Serial.print(ch3Value);
    Serial.print(" | Ch4="); Serial.print(ch4Value);
    Serial.print(" | Ch5="); Serial.print(ch5Value);
    Serial.print(" | Ch6="); Serial.print(ch6Value);
    Serial.print(" | Ch7="); Serial.print(ch7Value);
//    Serial.print(" | Ch7Raw="); Serial.print(ch7RawValue);
    Serial.print(" | Ch8="); Serial.print(ch8Value);
    Serial.println();
  #endif

  // Engine control - Channel 5 is the engine run/stop switch
  bool rawEngineRunStopSwitch = (ch5Value > 1700);
  engineRunStopSwitch = debounceBoolean(rawEngineRunStopSwitch, engineRunStopSwitchState);
  controlEngine(engineRunStopSwitch);

 // Engine throttle control including auto throttle function
  #if ENABLE_AUTO_THROTTLE_FUNCTION
    if (clutchEngaged) {
      controlEngineThrottle(AUTO_THROTTLE_SETTING);
    } else {
      controlEngineThrottle(ch3Value);
    }
  #else
    controlEngineThrottle(ch3Value);
  #endif

  // Engine choke control including auto choke function
  #if ENABLE_AUTO_CHOKE_FUNCTION
    controlAutoChokeOnEngineStart();
    if (autoChokeEnabled) {
      controlEngineChoke(autoChokePWMSetting);
    } else {
      controlEngineChoke(ch4Value);
    }
  #else
    controlEngineChoke(ch4Value);
  #endif

  // Clutch control - Channel 6 is the clutch engage switch
  bool rawEngageClutchSwitch = (ch6Value > 1700);
  engageClutchSwitch = debounceBoolean(rawEngageClutchSwitch, engageClutchSwitchState);
  controlClutch(engageClutchSwitch);

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

  // Crawler chute rotation - do this as soon as possible after reading RC channel inputs
  controlChuteRotation(ch8Value);

  // Crawler chute deflection angle control
  controlChuteDeflection(ch7Value);

  // Crawler blower assembly lifting/lowering
  controlBlowerLiftingAndLowering(ch1Value, ch2Value);

  // Pause processing so that we iterate at a specified rate (FRAME_ITERATION_RATE) and collect some
  // statistics relative to overall processing utilization.

  processingEndTime = millis();                       // Save millis time when processing is done

  while (millis() < frameEndTime);                    // Pause processing for specific iteration rate

  currentTime = millis();                             // Calculate end of next processsing frame
  frameEndTime = currentTime + FRAME_DURATION_IN_MS;

  #if DEBUG_PRINT_FRAME_DURATION_INFO
    unsigned long frameDurationInMS = currentTime - previousCurrentTime;
    unsigned long frameUtilizationPercentage = (100 * (processingEndTime - previousCurrentTime)) / FRAME_DURATION_IN_MS;
    Serial.print("Actual frame duration = "); Serial.print(frameDurationInMS);
    Serial.print(" mS | Frame Utilization = "); Serial.print(frameUtilizationPercentage);
    Serial.print("% | Current millis() = "); Serial.println(currentTime);
    previousCurrentTime = currentTime;
  #endif

}

//
// Support functions
//

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


bool debounceBoolean(bool rawValue, DebounceState &state) {

  #define DEBOUNCE_TIME_IN_MS 1000   // 1 second
  unsigned long now = millis();

  if (rawValue != state.lastStableValue) {
    // If the value has changed, reset the timer
    if ((now - state.lastChangeTime) >= DEBOUNCE_TIME_IN_MS) {
      state.lastStableValue = rawValue;
      state.lastChangeTime = now;
    }
  } else {
    // If value matches stable state, just reset timer
    state.lastChangeTime = now;
  }

  return state.lastStableValue;
}


float calculateCurrentMovingAverage(float newSample) {

  #define WINDOW 5      // Moving average of 5 samples;

  // Define static so these varialbe retain their values across calls
  static float buffer[WINDOW] = {0};
  static int index = 0;
  static float sum = 0;

  // Subtract oldest sample from sum
  sum -= buffer[index];

  // Store the new sample in the buffer
  buffer[index] = newSample;

  // Add new sample into sum
  sum += newSample;

  // Advance and wrap the index
  index = (index + 1) % WINDOW;

  // Return the current moving average
  return (sum / WINDOW);
}


//
// Control functions - Engine
//

void controlEngine(bool engineRunStopSwitch) {

  // The engineRunStopSwitch is true when this switch is in the on (down) position.  This switch work like
  // a single-pole light switch.  When the switch is turned on (down position), the engine should start
  // and continuing running until the switch it turned off (up position).
  //
  // To start the engine, the code will engage the starter to crank the engine.
  // To stop the engine, the code will activate the kill engine relay.

  #define ENGINE_START_DURATION 3000         // 3 seconds
  #define ENGINE_KILL_DURATION 5000          // 5 seconds
  #define ENGINE_RUNNING_THRESHOLD 2.0       // 2 Amps - adjust if needed

  // Read the ACS758LCB-100B current sensor to determine if the engine is running
  #define ADC_RESOLUTION 1023.0
  #define VREF 5.0
  #define ZERO_CURRENT_VOLTAGE 2.5
  #define SENSITIVITY 0.066
  #define NOISE_THRESHOLD 0.5

  int rawValue = analogRead(CURRENT_SENSOR_PIN);
  float voltage = (rawValue / ADC_RESOLUTION) * VREF;
  float currentAmps = (voltage - ZERO_CURRENT_VOLTAGE) / SENSITIVITY;
  if (currentAmps < NOISE_THRESHOLD) currentAmps = 0.0;

  // Get moving average of current amps to smooth input values
  float averagedCurrentAmps = calculateCurrentMovingAverage(currentAmps);

  // Determine if engine is running or stopped based on generator current output
  engineRunning = (averagedCurrentAmps >= ENGINE_RUNNING_THRESHOLD);

  // Print debug values before determining engine control action
  #if DEBUG_PRINT_ENGINE_STATE_INFO
    Serial.print("Switch 5: "); Serial.print(engineRunStopSwitch ? "Run " : "Stop");
    Serial.print(" | Engine: "); Serial.print(engineRunning ? "ON " : "OFF");
    Serial.print(" | Avg Amps: "); Serial.print(averagedCurrentAmps, 2);
    Serial.print(" | Raw Amps: "); Serial.print(currentAmps, 2);
    Serial.print(" | Starter: "); Serial.print(starterActive ? "ON " : "OFF");
    Serial.print(" | Kill Relay: "); Serial.println(killActive ? "ON " : "OFF");
  #endif

  // If the Run/Stop switch was just been moved to the "Run" position, engage the starter to crank the
  // engine if the engine is not running and the engine kill relay is not currently activated
  if (engineRunStopSwitch && !lastEngineRunStopSwitch && !engineRunning && !killActive) {
    digitalWrite(STARTER_RELAY_PIN, HIGH);
    startTime = millis();
    starterActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Engaging starter to ON to crank engine ...");
    #endif
  }

  // Else if the Run/Stop switch was just been moved to the "Stop" position, activate the engine kill relay
  // to stop the engine from running.  Note: the check if the engine is running has been deleted.
  else if (!engineRunStopSwitch && lastEngineRunStopSwitch) {
    digitalWrite(KILL_RELAY_PIN, HIGH);
    killTime = millis();
    killActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Activing kill relay to ON to stop engine ...");
    #endif
  }

  // If the engine kill relay is currently activated to stop the engine and the engine kill time duration
  // has expired, deactivate the engine kill relay
  if (killActive && millis() - killTime >= ENGINE_KILL_DURATION) {
    digitalWrite(KILL_RELAY_PIN, LOW);
    killActive = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Kill relay OFF - kill time limit duration has expired.");
    #endif
  }

  // If the starter is actively cranking the engine, disengage the starter if the engine is now running
  // or the engine start time duration has expired or the engine kill relay is currently activated
  boolean timeDurationExpired = ((millis() - startTime) >= ENGINE_START_DURATION);
  if (starterActive && (engineRunning || timeDurationExpired || killActive )) {
    digitalWrite(STARTER_RELAY_PIN, LOW);
    starterActive = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      if (timeDurationExpired) {
        Serial.println("Starter OFF - start time duration expired.");
      } else {
        Serial.println("Starter OFF - engine is running.");
    }
    #endif
  }

  // Save current engineRunStopSwitch value for next iteration
  lastEngineRunStopSwitch = engineRunStopSwitch;
}


void controlEngineChoke(long ch4) {
  // This contraint is redundant but included anyway to insure the Choke servo
  // is never driven outside its range of travel of 1000 to 1500
  long constrainedCh4 =  constrain(ch4, 1000, 1500);

  // Convert Ch4 value into servo angle
  int newServoAngle = map(constrainedCh4, 1000, 2000, 45, 220);

  // To prevent servo dithering, only move the servo if the new angle is more than
  // 5 degree different than the current value
  if (abs(currentChokeServoAngle - newServoAngle) > 5) {
    engineChokeServo.write(newServoAngle);
    #if DEBUG_PRINT_ENGINE_CHOKE_INFO
      Serial.print("Updated Engine Choke Servo: ");
      Serial.print(newServoAngle);
      Serial.println(" degrees");
    #endif
    currentChokeServoAngle = newServoAngle;
  } else {
    // engineChokeServo.write(currentChokeServoAngle);
  }

}


void controlEngineThrottle(long ch3) {
  // This contraint is redundant but included anyway to insure the Throttle servo
  // is never driven outside its range of travel of 1250 to 1750
  long constrainedCh3 = constrain(ch3, 1250, 1750);

  // Convert Ch3 value into servo angle - engine throttle input has been changed so that
  // "idle" is now a value of 1250 and "full gas" is a value of 1750
  // int newServoAngle = map(constrainedCh3, 1000, 2000, 160, 0);      // Initial values
  // int newServoAngle = map(constrainedCh3, 1000, 2000, 160, 40);     // Updated during 7/18/2025 testing
  int newServoAngle = map(constrainedCh3, 1250, 1750, 130, 70);        // Ratioed to 1250 - 1750 range

  // To prevent servo dithering, only move the servo if the new angle is more than
  // 5 degree different than the current value
  if (abs(currentThrottleServoAngle - newServoAngle) > 5) {
    engineThrottleServo.write(newServoAngle);
    #if DEBUG_PRINT_ENGINE_THROTTLE_INFO
      Serial.print("Updated Engine Throttle Servo: ");
      Serial.print(newServoAngle);
      Serial.println(" degrees");
    #endif
    currentThrottleServoAngle = newServoAngle;
  } else {
    // engineThrottleServo.write(currentThrottleServoAngle);
  }

}


void controlAutoChokeOnEngineStart() {

  #define AUTO_CHOKE_DURATION_AFTER_ENGINE_START 500          // 0.5 seconds

  // If starter is engaged, enable auto choke and set choke to full closed position
  if (starterActive) {
    autoChokeEnabled = true;
    autoChokePWMSetting = 1000;
    #if DEBUG_PRINT_ENGINE_CHOKE_INFO
      Serial.println("AutoChoke Control: Auto choke enabled by starter cranking engine");
    #endif
  }
  // Else if engine just started, save engine start time for later use
  // (Auto choke remains enabled and choke remains in full closed position)
  else if (!starterActive && starterWasActive && engineRunning) {
    autoChokeEngineStartTime = millis();
    #if DEBUG_PRINT_ENGINE_CHOKE_INFO
      Serial.println("AutoChoke Control: Engine start detected ... auto choke remains enabled");
    #endif
  }
  // Else if engine is running and auto choke is enabled, continue auto choke for specified duration
  else if (engineRunning && autoChokeEnabled) {
    long autoChockRunTime = millis() - autoChokeEngineStartTime;
    if (autoChockRunTime >= AUTO_CHOKE_DURATION_AFTER_ENGINE_START) {
      autoChokeEnabled = false;
      autoChokePWMSetting = 1500;
      #if DEBUG_PRINT_ENGINE_CHOKE_INFO
        Serial.println("AutoChoke Control: Auto choke duration at time limit - auto choke disabled.");
      #endif
    } else {
      #if DEBUG_PRINT_ENGINE_CHOKE_INFO
        Serial.print("AutoChoke Control: Auto choke remains enable - current run time is ");
        Serial.print(autoChockRunTime);
        Serial.println(" mS");
      #endif
    }
  }
  // Else the engine is not running so disable the auto choke
  else {
    autoChokeEnabled = false;
    autoChokePWMSetting = 1500;
    if (autoChokeEnabled) {
      #if DEBUG_PRINT_ENGINE_CHOKE_INFO
        Serial.println("AutoChoke Control: Auto choke disabled because engine is not running.");
      #endif
    }
  }

  // Save current starterActive value for next auto choke iteration
  starterWasActive = starterActive;
}


//
// Control function - Clutch
//

void controlClutch(bool engageClutch) {
  // Clutch control
  digitalWrite(CLUTCH_RELAY_PIN, engageClutch ? HIGH : LOW);
  clutchEngaged = engageClutch;

  #if DEBUG_PRINT_CLUTCH_STATE_INFO
    Serial.print("Clutch: ");
    Serial.println(engageClutch ? "ENGAGED" : "DISENGAGED");
  #endif
}


//
// Control function - Chutes
//

void controlChuteDeflection(long ch7) {

  #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
    Serial.print("Knob 7: "); Serial.print(ch7);
  #endif

  if (ch7 > 1600) {
    #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | Raising Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 0);
    analogWrite(DEFLECTOR_LPWM, 255);
  } else if (ch7 < 1400) {
    #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | Lower Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 255);
    analogWrite(DEFLECTOR_LPWM, 0);
  } else {
    #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | No Change In Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 0);
    analogWrite(DEFLECTOR_LPWM, 0);
  }

  #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
   Serial.println("");
  #endif
}


void controlChuteRotation(long ch8) {

  #define CHUTE_MAX_DURATION 3600       // 3500 is 3.5 seconds
  #define CHUTE_MIN_DURATION -3600      // -3500 is 3.5 seconds

  #define CH8_MAX_MAPPING_DURATION  (CHUTE_MAX_DURATION - 100)
  #define CH8_MIN_MAPPING_DURATION  (CHUTE_MIN_DURATION + 100)

  #define POSITION_STOP_THRESHOLD 250   // Adjust between 200 and 500
  #define CURRENT_THRESHOLD 700         // Adjust if needed (600 - 800)

  #define PRE_AUTORIG_DURATION 4000     // 4000 is 4.0seconds
  #define AUTORIG_DURATION -4000        // -4000 is 4.0 seconds

  int desiredPosition = 0;

  // Calculate time since last call to this function
  unsigned long now = millis();
  float delta = now - chuteLastUpdate;
  chuteLastUpdate = now;
  if (delta > 1000) delta = 1000;

  // Read current sense
  int rsValue = analogRead(CHUTE_RS_PIN);
  int lsValue = analogRead(CHUTE_LS_PIN);

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print("Knob 8: "); Serial.print(ch8);
    Serial.print(" | Position: "); Serial.print(chutePosition);
    Serial.print(" | Delta: "); Serial.print(delta);
    Serial.print(" | RS: "); Serial.print(rsValue);
    Serial.print(" | LS: "); Serial.print(lsValue);
  #endif

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

  // After first movement of knob, start autorig by going all the left until left stop
  // is hit otherwise use position specified by knob
  if (knobMoved & !chutePositionRigged) {
    if(!chutePositionRiggedStarted) {
      chutePositionRiggedStarted = true;
      chutePosition = PRE_AUTORIG_DURATION;
    }
    desiredPosition = AUTORIG_DURATION;
  } else {
    desiredPosition = map(ch8, 1000, 2000, CH8_MIN_MAPPING_DURATION, CH8_MAX_MAPPING_DURATION);
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print(" | Desired Pos: "); Serial.print(desiredPosition);
  #endif

  // If knob has been moved, start chute rotation to desired position
  if (knobMoved) {
    // Stop if within position stop window
    if (abs(desiredPosition - chutePosition) < POSITION_STOP_THRESHOLD) {
      analogWrite(CHUTE_RPWM, 0);
      analogWrite(CHUTE_LPWM, 0);
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Stopped - At desired postion");
      #endif
    }
    // Clockwise (Right)
    else if ((desiredPosition > chutePosition) && (chutePosition <= CHUTE_MAX_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Rotating Clockwise");
      #endif
      if (lsValue < CURRENT_THRESHOLD) {
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 255);
        chutePosition += delta;
        if (chutePosition > CHUTE_MAX_DURATION) chutePosition = CHUTE_MAX_DURATION;
      } else {
        #if DEBUG_PRINT_CHUTE_ROTATION_INFO
          Serial.print(" - STALL");
        #endif
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 0);
      }
    }
    // Counter-clockwise (Left)
    else if ((desiredPosition < chutePosition) && (chutePosition >= CHUTE_MIN_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Rotating Counter-clockwise");
      #endif
      if (rsValue < CURRENT_THRESHOLD) {
        analogWrite(CHUTE_RPWM, 255);
        analogWrite(CHUTE_LPWM, 0);
        chutePosition -= delta;
        if (chutePosition < -CHUTE_MAX_DURATION) chutePosition = CHUTE_MIN_DURATION;
      } else {
        #if DEBUG_PRINT_CHUTE_ROTATION_INFO
          Serial.print(" - STALL");
        #endif
        chutePositionRigged = true;
        chutePosition = CHUTE_MIN_DURATION;
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 0);
      }
    }
    // Stop
    else {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Stopped - we should never get here!!!");
      #endif
      analogWrite(CHUTE_RPWM, 0);
      analogWrite(CHUTE_LPWM, 0);
    }
  } else {
    #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print(" | Stopped - Knob has not yet moved.");
      #endif
    analogWrite(CHUTE_RPWM, 0);
    analogWrite(CHUTE_LPWM, 0);
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.println("");
  #endif
}


//
// Control function - Blower
//

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

  if (ch1 < 1460 && ch2 < 1460) {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 255);
    wasReverse = true;
    wasForward = false;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lifting Blower (triggered by reverse)");
    #endif
  } else if (wasReverse) {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 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(BLOWER_RPWM, 255);
    analogWrite(BLOWER_LPWM, 0);
    wasForward = true;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (triggered by forward)");
    #endif
  } else if (wasForward) {
    analogWrite(BLOWER_RPWM, 255);
    analogWrite(BLOWER_LPWM, 0);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (persisting forward)");
    #endif
    if (ch1 < 1460 && ch2 < 1460) {
      wasForward = false;
    }
  } else {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 0);
  }
}


//
// Control function - Crawler
//

// =================== Tracked Crawler Mixer (Flysky + Sabertooth) ===================
// Inputs: ch1_us (steer L/R), ch2_us (throttle F/R) in microseconds (~1000..2000).
// Output: ST.motor(1|2, -127..127).  Deadband, slew, dynamic turn gain,
// straight-line assist (forward & reverse), optional pivot mode, soft direction-change
// braking, and basic failsafe.
//
// Wiring/options --------------------------------------------------------------------
#define RIGHT_MOTOR_CH     1     // Sabertooth output channel for RIGHT track
#define LEFT_MOTOR_CH      2     // Sabertooth output channel for LEFT track

// If one side runs “forward” when it should reverse, flip INVERT_RIGHT/LEFT
#define INVERT_RIGHT       false // set true if your right track runs backwards
#define INVERT_LEFT        false // set true if your left track runs backwards

// RC mapping ------------------------------------------------------------------------
static const long  RC_CENTER     = 1500; // adjust if your radio isn’t perfectly centered
static const long  RC_SPAN       = 500;  // 1000..2000 -> 1500±500
static const int   RC_DEADBAND   = 30;   // μs deadband around center
static const int   RC_MIN_VALID  = 900;  // basic failsafe window
static const int   RC_MAX_VALID  = 2100;

// Motion shaping --------------------------------------------------------------------
static const float TURN_GAIN_BASE   = 0.75f; // yaw authority near zero speed (0.5–0.9)
static const float TURN_GAIN_MIN    = 0.45f; // yaw authority at high forward speed
static const int   SLEW_STEP        = 6;     // max change per loop in motor cmd (1..12 typical)
// static const uint16_t LOOP_DT_MS    = 20;    // call frequency ~50 Hz (for ramp math)
static const uint16_t LOOP_DT_MS    = FRAME_DURATION_IN_MS;
static const int   BRAKE_SOFTEN     = 18;    // limit how fast we cross through zero (anti-snap)

// Straight-line assist (applied to turn before mixing) -------------------------------
// These mirror the JS version you used: a speed-proportional “wedge” deadband on turn,
// with optional attenuation of the residual turn after the wedge.
static const int    FWD_SNAP_THRESH     = 16;   // only apply assist when |forward| >= this
static const int    TURN_DEAD_BASE      = 6;    // minimum deadband on turn (units of -127..127)
static const float  TURN_DEAD_SLOPE_FWD = 0.262f;// extra deadband grows with |forward| (forward)
static const float  TURN_DEAD_SLOPE_REV = 0.262f;// extra deadband grows with |forward| (reverse)
static const float  TURN_ATTEN_FWD      = 1.00f;// residual turn scale after deadband (forward)
static const float  TURN_ATTEN_REV      = 1.00f;// residual turn scale after deadband (reverse)

// Pivot mode ------------------------------------------------------------------------
static const bool  ENABLE_PIVOT     = true;
static const int   PIVOT_THRESH     = 50;    // |turn| above this (out of 127) can pivot
static const int   FORWARD_NEAR0    = 12;    // |forward| below this counts as "stopped"
static const uint16_t PIVOT_RAMP_MS = 450;   // time to ramp into full pivot

// Debug -----------------------------------------------------------------------------
// #define DEBUG_PRINT_CRAWLER_MOVEMENT_INFO 1

// -----------------------------------------------------------------------------------
static int16_t prevL = 0, prevR = 0;         // slew state
static int16_t pivotTarget = 0;              // 0..127 magnitude of pivot ramp
static uint32_t pivotTimerMs = 0;

// Map RC μs to -127..127 with deadband & clamp
int16_t mapStickTo127(long us, long center=RC_CENTER, long span=RC_SPAN, int dead=RC_DEADBAND) {
  long d = us - center;
  if (labs(d) < dead) return 0;
  if (d >  span) d =  span;
  if (d < -span) d = -span;
  return (int16_t)((d * 127L) / span);
}

// Slew limiter (ramps toward target by at most "step" each call)
int16_t slewToward(int16_t target, int16_t &prev, int step=SLEW_STEP) {
  if      (target > prev + step) prev += step;
  else if (target < prev - step) prev -= step;
  else                           prev  = target;
  return prev;
}

// Soft zero-cross (reduce instantaneous flips from + to -)
// Macro-safe (avoids min/max template vs. macro collisions).
int16_t softenDirectionFlip(int16_t target, int16_t current) {
  if ((target > 0 && current < 0) || (target < 0 && current > 0)) {
    if (current > 0) {
      int16_t next = current - BRAKE_SOFTEN;
      return (next < 0) ? 0 : next;
    } else {
      int16_t next = current + BRAKE_SOFTEN;
      return (next > 0) ? 0 : next;
    }
  }
  return target;
}

// Optional: linear ramp 0..max over PIVOT_RAMP_MS
int16_t pivotRamp(int16_t desiredAbs, uint16_t dtMs) {
  if (desiredAbs == 0) { pivotTarget = 0; return 0; }
  // increase toward desiredAbs
  uint16_t step = (uint32_t)127 * dtMs / PIVOT_RAMP_MS; // ramp time to ~127
  if (step < 1) step = 1;
  int16_t next = pivotTarget + (int16_t)step;
  if (next > desiredAbs) next = desiredAbs;
  pivotTarget = next;
  return pivotTarget;
}

// Dynamic turn gain: reduce yaw as |forward| grows
float dynamicTurnGain(int16_t forward127) {
  float f = fabsf(forward127) / 127.0f; // 0..1
  // interpolate between BASE (at low speed) and MIN (at high speed)
  return TURN_GAIN_BASE - (TURN_GAIN_BASE - TURN_GAIN_MIN) * f;
}

bool pulsesValid(long ch1_us, long ch2_us) {
  return (ch1_us >= RC_MIN_VALID && ch1_us <= RC_MAX_VALID &&
          ch2_us >= RC_MIN_VALID && ch2_us <= RC_MAX_VALID);
}

// Main control function -------------------------------------------------------------
void controlCrawlerMovement(long ch1_us, long ch2_us) {
  // Failsafe: invalid pulses => stop smoothly
  if (!pulsesValid(ch1_us, ch2_us)) {
    slewToward(0, prevL, SLEW_STEP);
    slewToward(0, prevR, SLEW_STEP);
    int16_t outL = prevL, outR = prevR;
    if (INVERT_LEFT)  outL = -outL;
    if (INVERT_RIGHT) outR = -outR;
    ST.motor(RIGHT_MOTOR_CH, outR);
    ST.motor(LEFT_MOTOR_CH,  outL);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.print("Crawler Movement: Failsafe Mode | Speed: Left="); Serial.print(outL); Serial.print(" Right=");Serial.println(outR);
    #endif
    return;
  }

  // Sticks -> -127..127 (raw throttle/steer)
  int16_t forward = mapStickTo127(ch2_us);   // +forward / -reverse
  int16_t turn    = mapStickTo127(ch1_us);   // +right   / -left

  // --- Straight-line assist (forward & reverse), same behavior as JS version ----
  if (abs(forward) >= FWD_SNAP_THRESH) {
    const float slope = (forward >= 0) ? TURN_DEAD_SLOPE_FWD : TURN_DEAD_SLOPE_REV;
    const float atten = (forward >= 0) ? TURN_ATTEN_FWD      : TURN_ATTEN_REV;

    // dead = max(TURN_DEAD_BASE, round(|forward| * slope))
    int dead = TURN_DEAD_BASE;
    int computed = (int)(fabsf((float)forward) * slope + 0.5f);
    if (computed > dead) dead = computed;

    if (abs(turn) <= dead) {
      turn = 0; // snap straight when near-forward/back
    } else {
      // pass-through beyond dead zone, optionally attenuated
      int mag = abs(turn) - dead;
      int scaled = (int)(mag * atten + 0.5f);
      turn = (turn >= 0) ? scaled : -scaled;
    }
  }
  // ------------------------------------------------------------------------------

  // Calculate turn gain that shrinks as speed grows
  float K = dynamicTurnGain(forward);

  // Decide if we should pivot (counter-rotate) when nearly stopped
  bool wantPivot = ENABLE_PIVOT && (abs(forward) <= FORWARD_NEAR0) && (abs(turn) >= PIVOT_THRESH);

  int16_t cmdL, cmdR;

  if (wantPivot) {
    // Ramp into pivot to prevent sudden snap
    int16_t desired = abs(turn);
    if (desired > 127) desired = 127;
    int16_t mag = pivotRamp(desired, LOOP_DT_MS);
    int8_t sgn  = (turn >= 0) ? +1 : -1;   // +right means right track fwd, left rev
    cmdL = -sgn * mag;
    cmdR =  sgn * mag;
  } else {
    // Not pivoting: standard arcade mixing
    pivotTarget = 0; // reset pivot ramp
    int16_t L = forward - (int16_t)(K * turn);
    int16_t R = forward + (int16_t)(K * turn);
    // clamp
    if (L > 127) L = 127; if (L < -127) L = -127;
    if (R > 127) R = 127; if (R < -127) R = -127;
    cmdL = L;
    cmdR = R;
  }

  // Soften instantaneous reversals to keep tracks from grabbing
  cmdL = softenDirectionFlip(cmdL, prevL);
  cmdR = softenDirectionFlip(cmdR, prevR);

  // Slew-limit toward targets for glide
  int16_t outL = slewToward(cmdL, prevL, SLEW_STEP);
  int16_t outR = slewToward(cmdR, prevR, SLEW_STEP);

  // Optional per-side inversion
  if (INVERT_LEFT)  outL = -outL;
  if (INVERT_RIGHT) outR = -outR;

  // Defensive clamp
  if (outL > 127) outL = 127; if (outL < -127) outL = -127;
  if (outR > 127) outR = 127; if (outR < -127) outR = -127;

  // Final drive (swap channels if your wiring expects opposite)
  ST.motor(RIGHT_MOTOR_CH, outR);
  ST.motor(LEFT_MOTOR_CH,  outL);
  
  #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
    Serial.print("Crawler Movement: Normal Mode | Speed: Left="); Serial.print(outL); Serial.print(" Right=");Serial.println(outR);
  #endif
}