// https://forum.arduino.cc/t/continuous-servo-issues-code-fixes/1440288

///////////////////////////////////////
/// HOME A.D.S - AIR DEFENSE SYSTEM ///
///////////////////////////////////////

#include <Servo.h>

// Pins
const int JOY_X_PIN = A0;
const int JOY_Y_PIN = A1;
const int JOY_SW_PIN = 26;

const int PIR_PIN  = 22;

const int TRIG_PIN = 24;
const int ECHO_PIN = 25;

const int LEFT_DRIVE_SERVO_PIN  = 6;
const int RIGHT_DRIVE_SERVO_PIN = 7;
const int TOP_SERVO_PIN         = 8;
const int SCAN_SERVO_PIN        = 9;

// RGB LED pins
const int RGB_RED_PIN   = 30;
const int RGB_GREEN_PIN = 31;
const int RGB_BLUE_PIN  = 32;

// Servo objects
Servo leftDriveServo;
Servo rightDriveServo;
Servo topServo;
Servo scanServo;

// Separate stop values
const int LEFT_DRIVE_STOP  = 90;
const int RIGHT_DRIVE_STOP = 94;
const int TOP_SERVO_STOP   = 90;

// Drive ranges
const int DRIVE_OFFSET_MIN = -30;
const int DRIVE_OFFSET_MAX = 30;
const int JOYSTICK_DEADZONE = 3;

// Top servo offsets
const int TOP_CW_OFFSET  = 25;
const int TOP_CCW_OFFSET = -25;

// Scan servo settings
const int SCAN_START_POS = 10;
const int SCAN_MIN = 10;
const int SCAN_MAX = 170;

// Reverse options
const bool REVERSE_LEFT_DRIVE  = false;
const bool REVERSE_RIGHT_DRIVE = true;
const bool REVERSE_TOP_SERVO   = false;

// Current commands
int leftDriveCmd = LEFT_DRIVE_STOP;
int rightDriveCmd = RIGHT_DRIVE_STOP;

// Top state
int topState = 0;   // 0 = front, 180 = back
int topTargetState = 0;
bool topTurning = false;
unsigned long topTurnStartTime = 0;
unsigned long TOP_TURN_TIME_MS = 900;

// Top output tracking
int topTurnOutputCmd = TOP_SERVO_STOP;

// Scan state
int scanPos = SCAN_START_POS;
bool scanForward = true;

// Keyboard override
bool keyboardMode = false;
int kbMove = 0;
int kbTurn = 0;

// Alert settings
const int DETECTION_DISTANCE_CM = 60;
unsigned long ACTION_COOLDOWN_MS = 5000;
unsigned long lastActionTime = 0;
unsigned long ALERT_HOLD_MS = 1200;
unsigned long alertHoldUntil = 0;

// Timers
unsigned long lastScanUpdate = 0;
unsigned long lastRadarSend = 0;
unsigned long lastDistanceRead = 0;
unsigned long lastJoyButtonTime = 0;

// Timing values
const unsigned long SCAN_INTERVAL_MS = 15;
const unsigned long RADAR_SEND_INTERVAL_MS = 35;
const unsigned long DISTANCE_READ_INTERVAL_MS = 40;
const unsigned long BUTTON_DEBOUNCE_MS = 250;

// Distance cache
long lastDistance = -1;

// Alert flash state machine
bool alertActive = false;
bool alertLedOn = false;
int alertToggleCount = 0;
int alertTargetToggles = 0;
unsigned long alertLastChange = 0;
unsigned long alertOnTime = 150;
unsigned long alertOffTime = 150;

// Helper functions
int clampValue(int value, int minVal, int maxVal) {
  if (value < minVal) {
    return minVal;
  } else if (value > maxVal) {
    return maxVal;
  } else {
    return value;
  }
}

int applyContinuousCommand(int stopValue, int offsetValue, bool reversed) {
  int cmd;

  if (reversed) {
    cmd = stopValue - offsetValue;
  } else {
    cmd = stopValue + offsetValue;
  }

  return clampValue(cmd, 0, 180);
}

// RGB LED helpers
void setRGB(bool redOn, bool greenOn, bool blueOn) {
  digitalWrite(RGB_RED_PIN, redOn ? HIGH : LOW);
  digitalWrite(RGB_GREEN_PIN, greenOn ? HIGH : LOW);
  digitalWrite(RGB_BLUE_PIN, blueOn ? HIGH : LOW);
}

void startRedAlert(int times, unsigned long onTime, unsigned long offTime) {
  alertActive = true;
  alertLedOn = false;
  alertToggleCount = 0;
  alertTargetToggles = times * 2;
  alertLastChange = millis();
  alertOnTime = onTime;
  alertOffTime = offTime;
  setRGB(false, false, false);
}

void updateAlertFlash() {
  if (!alertActive) {
    return;
  }

  unsigned long now = millis();
  unsigned long interval = alertLedOn ? alertOnTime : alertOffTime;

  if (now - alertLastChange >= interval) {
    alertLastChange = now;
    alertLedOn = !alertLedOn;
    alertToggleCount++;

    if (alertLedOn) {
      setRGB(true, false, false);
    } else {
      setRGB(false, false, false);
    }

    if (alertToggleCount >= alertTargetToggles) {
      alertActive = false;
      alertLedOn = false;
      setRGB(false, false, false);
    }
  }
}

// Top control
void stopTopServo() {
  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurning = false;
}

void startTopTurnToState(int newTargetState) {
  if (topTurning) {
    return;
  }

  if (newTargetState == topState) {
    return;
  }

  topTargetState = newTargetState;

  int offset = 0;
  if (newTargetState == 180) {
    offset = TOP_CW_OFFSET;
  } else {
    offset = TOP_CCW_OFFSET;
  }

  topTurnOutputCmd = applyContinuousCommand(TOP_SERVO_STOP, offset, REVERSE_TOP_SERVO);
  topServo.write(topTurnOutputCmd);
  topTurnStartTime = millis();
  topTurning = true;
}

void updateTopTurn() {
  if (!topTurning) {
    return;
  }

  unsigned long now = millis();

  if (now - topTurnStartTime >= TOP_TURN_TIME_MS) {
    stopTopServo();
    topState = topTargetState;
  }
}

void zeroTopState() {
  stopTopServo();
  topState = 0;
  topTargetState = 0;
}

// Serial command handling
// Commands:
// KB,move,turn
// KBMODE,0
// KBMODE,1
// ALERT
// ZERO
void handleSerialLine(String line) {
  line.trim();

  if (line.length() == 0) {
    return;
  }

  if (line.startsWith("KBMODE,")) {
    int mode = line.substring(7).toInt();

    if (mode == 1) {
      keyboardMode = true;
    } else {
      keyboardMode = false;
      kbMove = 0;
      kbTurn = 0;
    }
  } else if (line.startsWith("KB,")) {
    int firstComma = line.indexOf(',');
    int secondComma = line.indexOf(',', firstComma + 1);

    if (firstComma >= 0 && secondComma > firstComma) {
      kbMove = clampValue(line.substring(firstComma + 1, secondComma).toInt(), -30, 30);
      kbTurn = clampValue(line.substring(secondComma + 1).toInt(), -30, 30);
    }
  } else if (line == "ALERT") {
    startRedAlert(4, 150, 150);
  } else if (line == "ZERO") {
    zeroTopState();
  }
}

void readSerialCommands() {
  while (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    handleSerialLine(line);
  }
}

// Drive control
void updateDrive() {
  int moveValue = 0;
  int turnValue = 0;

  if (keyboardMode) {
    moveValue = kbMove;
    turnValue = kbTurn;
  } else {
    int xVal = analogRead(JOY_X_PIN);
    int yVal = analogRead(JOY_Y_PIN);

    moveValue = map(yVal, 0, 1023, -30, 30);
    turnValue = map(xVal, 0, 1023, -30, 30);

    if (abs(moveValue) < JOYSTICK_DEADZONE) {
      moveValue = 0;
    }

    if (abs(turnValue) < JOYSTICK_DEADZONE) {
      turnValue = 0;
    }
  }

  int leftOffset = clampValue(moveValue + turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);
  int rightOffset = clampValue(moveValue - turnValue, DRIVE_OFFSET_MIN, DRIVE_OFFSET_MAX);

  leftDriveCmd = applyContinuousCommand(LEFT_DRIVE_STOP, leftOffset, REVERSE_LEFT_DRIVE);
  rightDriveCmd = applyContinuousCommand(RIGHT_DRIVE_STOP, rightOffset, REVERSE_RIGHT_DRIVE);

  leftDriveServo.write(leftDriveCmd);
  rightDriveServo.write(rightDriveCmd);
}

// Joystick button reset
void updateJoystickCalibration() {
  unsigned long now = millis();

  if (digitalRead(JOY_SW_PIN) == LOW) {
    if (now - lastJoyButtonTime >= BUTTON_DEBOUNCE_MS) {
      zeroTopState();
      lastJoyButtonTime = now;
    }
  }
}

// Motion response
void updateMotionResponse() {
  if (topTurning) {
    return;
  }

  if (alertActive) {
    return;
  }

  if (millis() < alertHoldUntil) {
    return;
  }

  int motionDetected = digitalRead(PIR_PIN);

  if (motionDetected == HIGH) {
    if (topState != 180) {
      startTopTurnToState(180);
    }
  } else {
    if (topState != 0) {
      startTopTurnToState(0);
    }
  }
}

// Ultrasonic
long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) {
    return -1;
  } else {
    return duration * 0.034 / 2;
  }
}

void updateDistanceReading() {
  unsigned long now = millis();

  if (now - lastDistanceRead >= DISTANCE_READ_INTERVAL_MS) {
    lastDistanceRead = now;
    lastDistance = readDistanceCM();
  }
}

// Scan servo
void updateScanServo() {
  unsigned long now = millis();

  if (now - lastScanUpdate >= SCAN_INTERVAL_MS) {
    lastScanUpdate = now;

    if (scanForward) {
      scanPos++;
      if (scanPos >= SCAN_MAX) {
        scanPos = SCAN_MAX;
        scanForward = false;
      }
    } else {
      scanPos--;
      if (scanPos <= SCAN_MIN) {
        scanPos = SCAN_MIN;
        scanForward = true;
      }
    }

    scanServo.write(scanPos);
  }
}

// Directional alert behavior
void handleUltrasonicAlert(long distance) {
  unsigned long currentTime = millis();

  if (distance > 0 && distance <= DETECTION_DISTANCE_CM) {
    if (currentTime - lastActionTime >= ACTION_COOLDOWN_MS) {
      if (!topTurning) {
        if (scanPos >= 90 && topState != 180) {
          startTopTurnToState(180);
        } else if (scanPos < 90 && topState != 0) {
          startTopTurnToState(0);
        }
      }

      startRedAlert(4, 150, 150);
      lastActionTime = currentTime;
      alertHoldUntil = currentTime + ALERT_HOLD_MS;
    }
  }
}

// Serial output
// SCAN,angle,distance,motion,topState,leftCmd,rightCmd,keyboardMode
void sendRadarData(long distance) {
  int motionDetected = digitalRead(PIR_PIN);

  Serial.print("AZ:");
  pad(scanPos);
  Serial.print(" DIST:");
  pad(distance);
  Serial.print(" PIR:");
  Serial.print(motionDetected);
  Serial.print(" TOP:");
  pad(topState);
  Serial.print(" L:");
  pad(leftDriveCmd);
  Serial.print(" R:");
  pad(rightDriveCmd);
  Serial.print(" KYBD:");
  Serial.println(keyboardMode ? 1 : 0);
}

void pad(int val) {
  if (val < 1000) Serial.print("0");
  if (val < 100) Serial.print("0");
  if (val < 10) Serial.print("0");
  Serial.print(val);
}

void updateRadarSend() {
  unsigned long now = millis();

  if (now - lastRadarSend >= RADAR_SEND_INTERVAL_MS) {
    lastRadarSend = now;
    sendRadarData(lastDistance);
  }
}

// Setup
void setup() {
  Serial.begin(9600);
  Serial.setTimeout(10);

  pinMode(JOY_SW_PIN, INPUT_PULLUP);
  pinMode(PIR_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  pinMode(RGB_BLUE_PIN, OUTPUT);

  leftDriveServo.attach(LEFT_DRIVE_SERVO_PIN);
  rightDriveServo.attach(RIGHT_DRIVE_SERVO_PIN);
  topServo.attach(TOP_SERVO_PIN);
  scanServo.attach(SCAN_SERVO_PIN);

  leftDriveServo.write(applyContinuousCommand(LEFT_DRIVE_STOP, 0, REVERSE_LEFT_DRIVE));
  rightDriveServo.write(applyContinuousCommand(RIGHT_DRIVE_STOP, 0, REVERSE_RIGHT_DRIVE));
  topServo.write(applyContinuousCommand(TOP_SERVO_STOP, 0, REVERSE_TOP_SERVO));
  scanServo.write(scanPos);

  setRGB(false, false, false);
}

// Main loop
void loop() {
  readSerialCommands();
  updateDrive();
  updateJoystickCalibration();
  updateTopTurn();
  updateScanServo();
  updateDistanceReading();
  handleUltrasonicAlert(lastDistance);
  updateMotionResponse();
  updateAlertFlash();
  updateRadarSend();
}


// My main sources are listed below:
// Joystick Code based on “arduinogetstarted.com/tutorials/arduino-joystick”
// Motion Sensor Code based on “arduinogetstarted.com/tutorials/arduino-motion-sensor”
// PC-to-Arduino Serial Code based on “arduinogetstarted.com/tutorials/arduino-serial-monitor”
// Millis() Function Syntax based on “docs.arduino.cc/language-reference/en/functions/time/millis/”