/*
  Circuit Atlas reference sketch
  Project 302 - Android-controlled Arduino robot snake

  Target board: Arduino Mega 2560
  Required library: Servo by Arduino

  Bluetooth commands (send as single characters from a serial-terminal app):
    F = forward wave       B = reverse wave
    L = curve left         R = curve right
    S = stop and centre    A = autonomous IR avoidance
    1..5 = gait speed      ? = print command help

  POWER SAFETY
  - Twelve servos require a separate, high-current regulated 5-6 V supply.
  - Do not connect a raw 7.4 V battery directly to standard 5-6 V servos.
  - Never power the servos from the Mega 5 V pin.
  - Join the servo-supply, Bluetooth, sensor, and Mega grounds together.
  - Add bulk capacitance near the servo power bus and fuse the battery lead.
*/

#include <Servo.h>
#include <math.h>

constexpr uint8_t SEGMENT_COUNT = 12;
constexpr uint8_t SERVO_PINS[SEGMENT_COUNT] = {
  2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13
};

// Optional digital obstacle sensor. Most IR modules pull LOW near an object.
constexpr uint8_t IR_OBSTACLE_PIN = 22;
constexpr bool IR_ACTIVE_LOW = true;

constexpr int SERVO_CENTRE_DEG = 90;
constexpr int SERVO_MIN_DEG = 35;
constexpr int SERVO_MAX_DEG = 145;
constexpr float BASE_AMPLITUDE_DEG = 34.0F;
constexpr float SEGMENT_PHASE_RAD = 0.62F;
constexpr unsigned long FRAME_INTERVAL_MS = 20;
constexpr unsigned long OBSTACLE_TURN_MS = 1250;

Servo segments[SEGMENT_COUNT];

enum class Gait : uint8_t {
  Stopped,
  Forward,
  Reverse,
  Left,
  Right,
  Autonomous
};

Gait gait = Gait::Stopped;
float wavePhase = 0.0F;
float phaseStep = 0.13F;
unsigned long lastFrameMs = 0;
unsigned long avoidanceUntilMs = 0;
bool avoidanceTurnLeft = true;

bool obstacleDetected() {
  const bool level = digitalRead(IR_OBSTACLE_PIN);
  return IR_ACTIVE_LOW ? level == LOW : level == HIGH;
}

void centreSnake() {
  for (uint8_t index = 0; index < SEGMENT_COUNT; ++index) {
    segments[index].write(SERVO_CENTRE_DEG);
  }
}

void printHelp() {
  Serial.println(F("Commands: F forward, B reverse, L left, R right"));
  Serial.println(F("          S stop, A autonomous, 1..5 speed, ? help"));
  Serial1.println(F("F/B/L/R/S/A, speed 1..5, ? help"));
}

void setSpeedFromCommand(char command) {
  const uint8_t level = static_cast<uint8_t>(command - '0');
  phaseStep = 0.07F + (0.025F * level);
  Serial.print(F("Gait speed level: "));
  Serial.println(level);
}

void handleCommand(char command) {
  if (command >= 'a' && command <= 'z') command -= 32;

  switch (command) {
    case 'F': gait = Gait::Forward; break;
    case 'B': gait = Gait::Reverse; break;
    case 'L': gait = Gait::Left; break;
    case 'R': gait = Gait::Right; break;
    case 'A': gait = Gait::Autonomous; break;
    case 'S':
      gait = Gait::Stopped;
      centreSnake();
      break;
    case '1': case '2': case '3': case '4': case '5':
      setSpeedFromCommand(command);
      break;
    case '?': printHelp(); break;
    case '\r': case '\n': case ' ': break;
    default:
      Serial.print(F("Ignored command: "));
      Serial.println(command);
      return;
  }

  Serial.print(F("Accepted command: "));
  Serial.println(command);
}

void readCommands() {
  while (Serial1.available() > 0) handleCommand(static_cast<char>(Serial1.read()));
  while (Serial.available() > 0) handleCommand(static_cast<char>(Serial.read()));
}

void updateAutonomousState() {
  if (gait != Gait::Autonomous) return;

  const unsigned long now = millis();
  if (obstacleDetected() && now >= avoidanceUntilMs) {
    avoidanceUntilMs = now + OBSTACLE_TURN_MS;
    avoidanceTurnLeft = !avoidanceTurnLeft;
  }
}

void writeWaveFrame() {
  const unsigned long now = millis();
  if (now - lastFrameMs < FRAME_INTERVAL_MS) return;
  lastFrameMs = now;

  if (gait == Gait::Stopped) return;

  updateAutonomousState();

  float direction = 1.0F;
  float turnBiasDeg = 0.0F;
  float amplitudeDeg = BASE_AMPLITUDE_DEG;

  switch (gait) {
    case Gait::Reverse:
      direction = -1.0F;
      break;
    case Gait::Left:
      turnBiasDeg = -13.0F;
      amplitudeDeg = 27.0F;
      break;
    case Gait::Right:
      turnBiasDeg = 13.0F;
      amplitudeDeg = 27.0F;
      break;
    case Gait::Autonomous:
      if (now < avoidanceUntilMs) {
        turnBiasDeg = avoidanceTurnLeft ? -18.0F : 18.0F;
        amplitudeDeg = 25.0F;
      }
      break;
    case Gait::Forward:
    case Gait::Stopped:
      break;
  }

  for (uint8_t index = 0; index < SEGMENT_COUNT; ++index) {
    // Alternating a small bias keeps a turn smooth across the body instead of
    // forcing every joint against the same mechanical limit.
    const float jointPhase = wavePhase + (direction * index * SEGMENT_PHASE_RAD);
    const float distributedBias = turnBiasDeg * (0.35F + 0.65F * index / (SEGMENT_COUNT - 1.0F));
    const int angle = constrain(
      static_cast<int>(SERVO_CENTRE_DEG + amplitudeDeg * sin(jointPhase) + distributedBias),
      SERVO_MIN_DEG,
      SERVO_MAX_DEG
    );
    segments[index].write(angle);
  }

  wavePhase += direction * phaseStep;
  if (wavePhase > TWO_PI) wavePhase -= TWO_PI;
  if (wavePhase < 0.0F) wavePhase += TWO_PI;
}

void setup() {
  pinMode(IR_OBSTACLE_PIN, INPUT_PULLUP);

  Serial.begin(115200);
  // HC-05/HC-06: Mega RX1=19 receives module TX; Mega TX1=18 sends through
  // a voltage divider to module RX when the breakout is not 5 V tolerant.
  Serial1.begin(9600);

  for (uint8_t index = 0; index < SEGMENT_COUNT; ++index) {
    segments[index].attach(SERVO_PINS[index]);
  }
  centreSnake();

  Serial.println(F("Circuit Atlas robot snake ready"));
  printHelp();
  delay(700);
}

void loop() {
  readCommands();
  writeWaveFrame();
}