// Running a stepper motor & hardware Serial simultaneously
// https://forum.arduino.cc/t/running-a-stepper-motor-hardware-serial-simultaneously/1399321


/*This is version 2.5, made for the RoboClaw Solo30,
  Packet Serial @ 115200 BAUD
  It also has the AS5600 connected via I2C
  DEBUG 1 give serial output, but you must disconnect the Roboclaw
  TODO:
  1. Sharp Turn Mode - don't start motor until the stepper is done
     It doesn't work well until load of thrust.
  2. Find the smoothest stepper option for turning in normal & auto modes
*/

#include <EEPROM.h>
#include <math.h>

// Debugging switches and macros
#define DEBUG 1  // Switch debug output on and off by 1 or 0

#if DEBUG
#define PRINTS(s) \
  { Serial.print(F(s)); }
#define PRINT(s, v) \
  { \
    Serial.print(F(s)); \
    Serial.print(v); \
  }
#else
#define PRINTS(s)
#define PRINT(s, v)
#endif

#include "RoboClaw.h"

RoboClaw roboclaw(&Serial, 10000);

#include <Wire.h>
#include "AS5600.h"
#define GEARBOX_RATIO 5.1818
#define SENSOR_TICKS_PER_REV 4096.0
#define TICKS_PER_OUTPUT_REV (SENSOR_TICKS_PER_REV * GEARBOX_RATIO)
#define EEPROM_ADDR 0

AS5600 as5600;
long zeroCumulative = 0;  // Cumulative position at zero

#include <Arduino.h>
#include <AccelStepper.h>

// Define Stepper pin connections
const int dirPin = 9;      //Dir Pin
const int stepPin = 8;     //Step Pin
const int stepperEN = A3;  //Enable Pin
const int sleepPin = 7;    //sleep and reset pin both
const int MS1pin = 4;
const int MS2pin = 5;
const int MS3pin = 6;

#define motorInterfaceType 1  // Define motor interface type

// Creates an instance
AccelStepper myStepper(motorInterfaceType, stepPin, dirPin);

const int LEDpin = 10;        //LED on power switch
const int bttnPOWERPin = 12;  //Power Switch
const int bttnUPpin = 11;     //SW1 (momentary on rocker switch)
const int bttnDOWNpin = 13;   //SW2 (momentary on rocker switch)
const int Xpin = A0;          //analog joystick pin Thrust
const int Ypin = A1;          //analog joystick pin Rotation
const int BatPin = A2;        //monitor battery pin

const int microsteps = 4;  // confirmed by observation
const float stepsPerRevolution = 200.0 * microsteps * 5.1818;
const float stepsPerDegree = stepsPerRevolution / 360.0;

bool systemOn = false;   //set to true when power button is turned on
bool as5600_OK = false;  //set to true when the AS5600 is working
int duty = 0;            // Duty value for RoboClaw +/-32767
bool trollingMotorSynced = false;
int trollingMotorAngle = 0;  //current angle of trolling motor
float batteryAhUsed = 0.0;
unsigned long lastCurrentSampleTime = 0;

//int maxSteps = 200;   //259 * 4 is 90 degrees
unsigned long t;                   //counter for serial print
int Xdebounce = 100;               //debounce value for turning
int Ydebounce = 20;                //debounce value for thrust
int Y_forwardTurnThreshold = 100;  //min forward val for “moving forward” steering
bool isSteering = false;           //true when steering in AutoPilot mode
int Xreading;                      //reading of X axis
int Yreading;                      //reading of Y axis
int minThrust = 0;                 //limit the min thrust value to this
int maxThrust = 32767;             //limit the max thrust value to this
int X_cen = 512;                   //center reading of X - axis on joystick
int X_right = 233;                 //joystick pushed RIGHT gives lower value
int X_left = 792;                  //joystick pushed LEFT gives higher value
int Y_cen = 510;                   //center reading of Y - axis on joystick
int Y_up = 791;                    //joystick pushed UP gives higher value
int Y_down = 225;                  //joystick pushed DOWN gives lower value
enum LedState {
  LED_BLINK,
  LED_TRIPLE_BLINK,
  LED_RAPID_BLINK,
  LED_ON,
  LED_OFF
};
LedState LEDstate = LED_OFF;

enum DriveMode {
  Normal,
  Turning,
  AutoPilot,
  AngleChange
};
DriveMode driveMode = Normal;  //start out in normal mode

#include <Bounce2.h>
Bounce2::Button bttnPOWER = Bounce2::Button();
Bounce2::Button bttnUP = Bounce2::Button();
Bounce2::Button bttnDOWN = Bounce2::Button();

void setup() {
  Serial.begin(115200);
  roboclaw.begin(115200);
  delay(1000);  // Give RoboClaw a moment to start
  pinMode(LEDpin, OUTPUT);
  pinMode(stepperEN, OUTPUT);
  digitalWrite(stepperEN, HIGH);  // DISABLE stepper driver (idle)
  pinMode(sleepPin, OUTPUT);
  digitalWrite(sleepPin, HIGH);  // Enable driver by default

  pinMode(MS1pin, OUTPUT);
  pinMode(MS2pin, OUTPUT);
  pinMode(MS3pin, OUTPUT);

  // Set for 1/4 microstepping (A4988/DRV8825 standard)
  digitalWrite(MS1pin, LOW);
  digitalWrite(MS2pin, HIGH);
  digitalWrite(MS3pin, LOW);

  myStepper.setMaxSpeed(2000);
  myStepper.setAcceleration(3000);
  myStepper.setSpeed(3000);

  PRINTS("\nFloat Tube Controller Version 2");
  bttnPOWER.attach(bttnPOWERPin, INPUT_PULLUP);  // Attach with INPUT_PULLUP mode
  bttnPOWER.interval(200);                       //debounce interval
  bttnPOWER.setPressedState(LOW);
  bttnUP.attach(bttnUPpin, INPUT_PULLUP);  // Attach with INPUT_PULLUP mode
  bttnUP.interval(200);                    //debounce interval
  bttnUP.setPressedState(LOW);
  bttnDOWN.attach(bttnDOWNpin, INPUT_PULLUP);  // Attach with INPUT_PULLUP mode
  bttnDOWN.interval(200);                      //debounce interval
  bttnDOWN.setPressedState(LOW);
  Wire.begin();
  init_AS5600();  //initiate the AS5600
  PRINT("\nAS5600 connected: ", as5600.isConnected());
}

void loop() {
  checkButtons();
  updateLED();
  updateJoystick();
  updateRoboClawDuty();            //send roboclaw serial every 100ms
  as5600.getCumulativePosition();  //keep the AS5600 updated
  if (systemOn) {
    myStepper.run();  //update constantly
    if (driveMode != AngleChange) {
      syncStepperToAS5600();  //update as needed
    }
    if (duty != 0 || myStepper.distanceToGo() != 0 || isSteering) {
      digitalWrite(stepperEN, LOW);  // ENABLE stepper driver
    } else {
      digitalWrite(stepperEN, HIGH);  // DISABLE stepper driver (idle)
    }

    switch (driveMode) {
      case Normal:
        normalModeThrust();
        handleFineSteeringNormalThrust();
        break;
      case Turning:
        handleSharpTurn();
        break;
      case AutoPilot:
        autoModeThrust();
        handleFineSteeringInAutoPilot();
        break;
      case AngleChange:
        handleAngleChange(); //this mode is used to re zero the angle
        break;
    }
  } else {                          //system is off
    digitalWrite(stepperEN, HIGH);  //disable
    duty = 0;                       //set motor speed
  }
}

void checkButtons() {
  bttnPOWER.update();
  bttnUP.update();
  bttnDOWN.update();

  // --- POWER button logic: handle ON/OFF ---
  if (bttnPOWER.pressed()) {  // just turned on
    LEDstate = LED_ON;
    PRINTS("\npower turned on");
    systemOn = true;
    duty = 0;
    driveMode = Normal;
    digitalWrite(stepperEN, HIGH);  // DISABLE stepper driver (idle)
    digitalWrite(LEDpin, HIGH);
#if DEBUG
    init_AS5600();  //so we don't have to keep unplugging it
#endif
  }
  if (bttnPOWER.released()) {  // just turned off
    LEDstate = LED_OFF;
    PRINTS("\npower turned off");
    systemOn = false;
    driveMode = Normal;
    duty = 0;
    digitalWrite(stepperEN, HIGH);  // DISABLE stepper driver (idle)
  }

  // --- UP button toggles AutoPilot ---
  if (bttnUP.pressed() && systemOn) {
    if (driveMode == AutoPilot) {
      LEDstate = LED_ON;
      PRINTS("\ndrive mode: Normal");
      driveMode = Normal;
    } else {
      LEDstate = LED_BLINK;
      PRINTS("\ndrive mode: AutoPilot");
      driveMode = AutoPilot;
    }
    duty = 0;
  }
  // --- DOWN button single press: AngleChange mode ---
  if (bttnDOWN.pressed() && systemOn) {
    LEDstate = LED_RAPID_BLINK;
    driveMode = AngleChange;
    PRINTS("\nDOWN pressed");
  }
}

void normalModeThrust() {
  LEDstate = LED_ON;  // On for normal mode
  int offset = abs(Yreading - Y_cen);

  if (offset < Ydebounce) {
    // Deadband: joystick is near the center
    duty = 0;  //set motor speed
  } else if (Yreading > Y_cen) {
    // Forward thrust (up direction)
    if (Yreading > Y_up) Yreading = Y_up;  //otherwise an int can't handle the larger size
    duty = map(Yreading, Y_cen + Ydebounce, Y_up, minThrust, maxThrust);
    duty = constrain(duty, minThrust, maxThrust);
  } else {
    // Reverse thrust
    if (Yreading < Y_down) Yreading = Y_down;  //stay inbounds for an int
    duty = map(Yreading, Y_cen - Ydebounce, Y_down, minThrust, maxThrust);
    duty = constrain(duty, minThrust, maxThrust);
    duty = -duty;  //reverse
  }

  static int prevDuty;
  if (prevDuty != duty) {
    //PRINT("\nSpeed: ", duty);
  }
  prevDuty = duty;

  // Sharp turn entry logic is unchanged
  int xOffset = Xreading - X_cen;
  bool stickSideways = abs(xOffset) > ((X_left - X_cen) * 4 / 10);       //about half way
  bool stickCenteredY = abs(Yreading - Y_cen) < Y_forwardTurnThreshold;  // not forward/back

  if (duty == 0 && stickCenteredY && stickSideways) {
    PRINTS("entering turn mode");
    driveMode = Turning;  // Enter sharp turn mode
  }
}

void autoModeThrust() {
  static unsigned long prevThrustMillis = 0;
  unsigned long now = millis();

  const unsigned long accelInterval = 10;  // ms for all modes
  const int fullStep = 103;
  const int partialStep = 62;

  int accel = 0;
  int accelStep = 1;

  if (Yreading > Y_up - 20) {
    accel = 1;
    accelStep = fullStep;
  } else if (Yreading > Y_cen + 50) {
    accel = 1;
    accelStep = partialStep;
  } else if (Yreading < Y_down + 20) {
    duty = 0;
    accel = 0;
  } else if (Yreading < Y_cen - 50) {
    accel = -1;
    accelStep = partialStep;  // adjust as desired for decel
  } else {
    accel = 0;
  }

  if (accel != 0 && (now - prevThrustMillis > accelInterval)) {
    prevThrustMillis = now;
    duty += accel * accelStep;
    if (accel > 0 && duty < 2000) duty = 2000;
  }

  duty = constrain(duty, 0, maxThrust);
}

void handleFineSteeringNormalThrust() {

  // Only allow small left/right steering when moving forward strongly
  if ((Yreading - Y_cen) > Y_forwardTurnThreshold && duty > 0) {
    int xOffset = Xreading - X_cen;
    if (abs(xOffset) > Xdebounce) {
      int turnAngle = map(xOffset, X_right - X_cen, X_left - X_cen, -60, 60);  //
      myStepper.moveTo(turnAngle * stepsPerDegree);
    } else {
      myStepper.moveTo(0);  // Go straight if joystick is centered
    }
  }
}

void handleFineSteeringInAutoPilot() {
  int xOffset = Xreading - X_cen;
  static int state = 0;                         // -1 = right, 0 = center, 1 = left
  int halfwayThreshold = (X_left - X_cen) / 2;  // halfway left/right from center

  if (xOffset > halfwayThreshold) {
    // More than halfway to the left
    if (state != 1) {
      myStepper.moveTo(30 * stepsPerDegree);
      isSteering = true;
      state = 1;
    }
  } else if (xOffset < -halfwayThreshold) {
    // More than halfway to the right
    if (state != -1) {
      myStepper.moveTo(-30 * stepsPerDegree);
      isSteering = true;
      state = -1;
    }
  } else if (abs(xOffset) < Xdebounce) {
    // Centered (with hysteresis)
    if (state != 0) {
      myStepper.moveTo(0);
      isSteering = false;
      state = 0;
    }
  }
  // Otherwise, do nothing (hysteresis region)
}

void handleSharpTurn() {
  int halfwayleft = X_cen + ((X_left - X_cen) * 4) / 10;    //Left is a higher value
  int halfwayright = X_cen + ((X_right - X_cen) * 4) / 10;  //Right is a lower value

  //int halfwayleft = (X_cen + X_left) / 2;    //Left is a higher value
  //int halfwayright = (X_cen + X_right) / 2;  //Right is a lower value
  int sharpTurnMaxSpeed = 32767;
  int sharpTurnMinSpeed = 2000;

  if (Xreading > halfwayleft) {
    // Left sharp turn
    myStepper.moveTo(65 * stepsPerDegree);
    // Map Xreading from halfway left to max left for thrust
    if (Xreading > X_left) Xreading = X_left;  //keep in bounds
    duty = map(Xreading, halfwayleft, X_left, sharpTurnMinSpeed, sharpTurnMaxSpeed);
    duty = constrain(duty, sharpTurnMinSpeed, sharpTurnMaxSpeed);
  } else if (Xreading < halfwayright) {
    // Right sharp turn
    myStepper.moveTo(-65 * stepsPerDegree);
    // Map Xreading from halfway right to max right for thrust
    if (Xreading < X_right) Xreading = X_right;  //keep in bounds
    duty = map(Xreading, halfwayright, X_right, sharpTurnMinSpeed, sharpTurnMaxSpeed);
    duty = constrain(duty, sharpTurnMinSpeed, sharpTurnMaxSpeed);
  } else {
    myStepper.moveTo(0);  //Joystick back near center
    duty = 0;
  }
  // Exit Turning mode only when stick is centered AND turn is complete
  if (Xreading < halfwayleft && Xreading > halfwayright && myStepper.distanceToGo() == 0) {
    driveMode = Normal;
  }
}

void handleAngleChange() { //this mode is used to re zero the angle
  // Forward/backward controls thrust just like autoModeThrust
  autoModeThrust();

  // Joystick X controls stepper speed and direction
  int xOffset = Xreading - X_cen;
  int jogDeadband = Xdebounce;
  float maxJogSpeed = 300.0;
  float speed = 0.0;

  if (abs(xOffset) > jogDeadband) {
    speed = map(xOffset, X_right - X_cen, X_left - X_cen, -maxJogSpeed, maxJogSpeed);
  } else {
    speed = 0.0;
  }

  myStepper.setSpeed(speed);
  myStepper.runSpeed();

  // --- Only allow a DOWN press *after* a release since entering this mode ---
  static bool downReady = false;
  if (bttnDOWN.released()) {
    downReady = true;
  }
  if (downReady && bttnDOWN.pressed()) {
    setAS5600Zero();
#if DEBUG
    PRINT("\nAS5600 zero set. New zero value: ", zeroCumulative);
#endif
    driveMode = Normal;
    downReady = false;  // block until next release
  }
}

void updateRoboClawDuty() {
  static unsigned long lastUpdate = 0;
  unsigned long now = millis();
#if DEBUG
  if (now - lastUpdate >= 1000) {  // 1 second interval in debug
    lastUpdate = now;
    //PRINT("\nDEBUG Duty: ", duty);
  }
#else
  if (now - lastUpdate >= 100) {  // 100 ms interval normally
    lastUpdate = now;
    roboclaw.DutyM1(0x80, duty);
  }
#endif
}



void updateJoystick() {
  Xreading = analogRead(Xpin);  //analog reading
  delayMicroseconds(10);
  Xreading = analogRead(Xpin);                    //2nd analog reading is better
  if (Xreading < 100 || Xreading > 900) error();  //connector failure??
  Yreading = analogRead(Ypin);                    //analog reading
  delayMicroseconds(10);
  Yreading = analogRead(Ypin);                    //2nd analog reading is better
  if (Yreading < 100 || Yreading > 900) error();  //connector failure??
  PRINT("\nY:", Yreading);
  PRINT(" X:", Xreading);
}

void init_AS5600() {
  as5600.begin();
  as5600_OK = as5600.isConnected();

  // 1. Read saved zero from EEPROM
  long savedZeroCumulative = 0;
  uint16_t savedZeroRawAngle = 0;
  EEPROM.get(EEPROM_ADDR, savedZeroCumulative);
  EEPROM.get(EEPROM_ADDR + 10, savedZeroRawAngle);

  // 2. Read current AS5600 raw angle
  uint16_t currentRaw = as5600.rawAngle();

  // 3. Calculate shortest signed difference in ticks (wraparound safe)
  int16_t tickDiff = ((int16_t)savedZeroRawAngle - (int16_t)currentRaw + 6144) % 4096 - 2048;
  float stepsPerTick = stepsPerRevolution / TICKS_PER_OUTPUT_REV;
  long stepOffset = lround(tickDiff * stepsPerTick);

  // 4. Enable stepper driver so it can move
  digitalWrite(stepperEN, LOW);

  // 5. Set current stepper position (so our move command below works correctly)
  myStepper.setCurrentPosition(0);  // Assume we're at step 0 now

  // 6. Move stepper so that the sensor should reach the EEPROM-saved angle
  long targetStep = stepOffset;
  myStepper.moveTo(targetStep);

  while (myStepper.distanceToGo() != 0) {
    myStepper.run();
  }

  // 7. Set stepper software zero at the end
  myStepper.setCurrentPosition(0);

  // 8. Save the current position as new zero (for cumulative use)
  setAS5600Zero();
}

void syncStepperToAS5600() {
  if (!as5600_OK) return;
  if (duty == 0) {
    trollingMotorSynced = false;  // Reset sync flag
    return;
  }
  if (!trollingMotorSynced) {
    long currentCumulative = as5600.getCumulativePosition();
    long difference = currentCumulative - zeroCumulative;

    float stepsPerTick = stepsPerRevolution / TICKS_PER_OUTPUT_REV;
    float stepsToZero = lround(difference * stepsPerTick);

    PRINTS("\nSYNC DEBUG ----");
    PRINT("\nSETPOINT: ", zeroCumulative);
    PRINT("\nCURRENT: ", currentCumulative);
    PRINT("\ndifference: ", difference);
    PRINT("\nstepsPerTick: ", stepsPerTick);
    PRINT("\nstepsToZero: ", stepsToZero);
    PRINT("\nStepper before set: ", myStepper.currentPosition());

    myStepper.setCurrentPosition(stepsToZero);
    myStepper.moveTo(0);

    PRINT("\nStepper after set: ", myStepper.currentPosition());
    PRINT("\nStepper target: ", myStepper.targetPosition());

    digitalWrite(stepperEN, LOW);
    trollingMotorSynced = true;
  }
}

int getOutputAngle() {
  long cumulativePos = as5600.getCumulativePosition();
  float outputAngle = ((cumulativePos - zeroCumulative) / TICKS_PER_OUTPUT_REV) * 360.0;

  // Wrap to -180..+180
  if (outputAngle > 180.0) outputAngle -= 360.0;
  if (outputAngle < -180.0) outputAngle += 360.0;

  int angleInt = (int)round(outputAngle);
  return angleInt;
}

void setAS5600Zero() {
  if (!as5600_OK) return;  // if sensor is not available, exit

  zeroCumulative = as5600.getCumulativePosition();
  uint16_t zeroRawAngle = as5600.rawAngle();

  EEPROM.put(EEPROM_ADDR, zeroCumulative);
  EEPROM.put(EEPROM_ADDR + 10, zeroRawAngle);

  PRINTS("\nAS5600 zero set.");
  PRINT(" Zero cumulative: ", zeroCumulative);
  PRINT(" | Zero raw angle: ", zeroRawAngle);
}


void error() {  // the reading was way out of range (connector failure??)
  duty = 0;
  digitalWrite(stepperEN, HIGH);  // DISABLE stepper driver (idle)
  PRINTS("Error on joystick reading.  Out of range...");

  while (1) {
    updateRoboClawDuty();
    int i0 = analogRead(A0);
    int i1 = analogRead(A1);
    PRINT("\nReading on A0: ", i0);
    PRINT("  Reading on A1: ", i1);

    // If both readings are valid, exit the error loop
    if (i0 >= 100 && i0 <= 900 && i1 >= 100 && i1 <= 900) {
      PRINTS("\nJoystick readings OK. Exiting error state.\n");
      break;
    }
    delay(200);
  }
}

void updateLED() {
  static unsigned long lastToggle = 0;
  static bool ledOn = false;

  const unsigned long onTime = 200;
  const unsigned long offTime = 5000;

  switch (LEDstate) {
    case LED_ON:
      digitalWrite(LEDpin, HIGH);
      ledOn = true;
      break;

    case LED_OFF:
      digitalWrite(LEDpin, LOW);
      ledOn = false;
      break;

    case LED_BLINK:
      {
        unsigned long now = millis();
        unsigned long interval = ledOn ? onTime : offTime;
        if (now - lastToggle >= interval) {
          ledOn = !ledOn;
          digitalWrite(LEDpin, ledOn ? HIGH : LOW);
          lastToggle = now;
        }
        break;
      }

    case LED_RAPID_BLINK:
      {
        unsigned long now = millis();
        unsigned long interval = 200;  // Rapid: 200ms ON/OFF
        if (now - lastToggle >= interval) {
          ledOn = !ledOn;
          digitalWrite(LEDpin, ledOn ? HIGH : LOW);
          lastToggle = now;
        }
        break;
      }
      // no default
  }
}


void monitorAhUsage() {
  static unsigned long lastPoll = 0;
  static unsigned long lastCurrentSampleTime = 0;
  const unsigned long pollInterval = 100;  // ms (5 Hz)
  unsigned long now = millis();

  if (duty == 0) {
    lastCurrentSampleTime = now;
    return;
  }

  if (now - lastPoll < pollInterval) return;
  lastPoll = now;

  int16_t m1Current, m2Current;
  bool status = roboclaw.ReadCurrents(0x80, m1Current, m2Current);

  if (status) {
    float amps = m1Current * 0.01;
    if (lastCurrentSampleTime != 0) {
      float hours = (now - lastCurrentSampleTime) / 3600000.0;
      batteryAhUsed += amps * hours;
    }
    lastCurrentSampleTime = now;
  }
}


float readVoltage() {
  float R1 = 29980.0;  // 30K resistor
  float R2 = 7500.0;   // 7.5K resistor
  float VREF = 5.0;    //system voltage
  int reading = analogRead(A5);
  delayMicroseconds(10);
  reading = analogRead(A5);
  float vout = (reading * VREF) / 1023.0;
  float vin = vout / (R2 / (R1 + R2));
  return vin;
}