#include <Wire.h>
#include <MPU6050.h>
#include <ESP32Servo.h>
#include <U8g2lib.h>

// ========= OLED SH1107 =========
U8G2_SH1107_SEEED_128X128_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

// ========= MPU =========
MPU6050 mpu;

// ========= Servos =========
Servo rudderServo;
Servo shipServo;
Servo indicatorServo;

// ========= Pins =========
#define JOY_PIN 34
#define POT_PIN 35
#define MODE_PIN 26
#define AUTO_BTN 33
#define RESET_BTN 25
#define LED_LEFT 4
#define LED_RIGHT 5
#define BUZZER 13

// ========= Settings =========
int centerAngle = 90;
int rudderLimit = 35;
int autoLimit = 20;

float courseHeading = 0;
bool autoMode = false;

// ========= JOYSTICK RATE CONTROL =========
float commandedAngle = 90;
unsigned long lastMoveTime = 0;
const unsigned long moveInterval = 200;   // 1 degree every 0.2 sec

// ======================================

void setup() {

  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();

  rudderServo.attach(18);
  shipServo.attach(19);
  indicatorServo.attach(23);

  pinMode(MODE_PIN, INPUT_PULLUP);
  pinMode(AUTO_BTN, INPUT_PULLUP);
pinMode(RESET_BTN, INPUT_PULLUP);
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(BUZZER, OUTPUT);

  // OLED init
  u8g2.begin();
  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_6x12_tr);
  u8g2.drawStr(0,15,"Ship Control System");
  u8g2.drawStr(0,30,"Booting...");
  u8g2.sendBuffer();
  delay(1500);
}

// ======================================

void loop() {

  bool useJoystick = digitalRead(MODE_PIN);
  bool autoButtonPressed = !digitalRead(AUTO_BTN);

  // ----- SET AUTO COURSE -----
  if (autoButtonPressed) {
    courseHeading = getHeading();
    autoMode = true;
    delay(400);
  }

 // ----- MANUAL CONTROL -----
if (!autoMode) {

  // -------- JOYSTICK MODE --------
  if (useJoystick == LOW) {

    int joyValue = analogRead(JOY_PIN);

    int deadZoneLow = 1800;
    int deadZoneHigh = 2300;

    unsigned long currentTime = millis();

    if (currentTime - lastMoveTime >= moveInterval) {

      if (joyValue < deadZoneLow) {
        commandedAngle -= 1;
        lastMoveTime = currentTime;
      }
      else if (joyValue > deadZoneHigh) {
        commandedAngle += 1;
        lastMoveTime = currentTime;
      }

      commandedAngle = constrain(commandedAngle,
                                 centerAngle - rudderLimit,
                                 centerAngle + rudderLimit);
    }
  }

  // -------- POT MODE --------
  else {

    int potValue = analogRead(POT_PIN);

    commandedAngle = map(potValue,
                         0, 4095,
                         centerAngle - rudderLimit,
                         centerAngle + rudderLimit);
  }
}
bool resetPressed = !digitalRead(RESET_BTN);

if (!autoMode && resetPressed) {

  // Smooth return to center
  while (commandedAngle != centerAngle) {

    if (commandedAngle > centerAngle)
      commandedAngle--;
    else if (commandedAngle < centerAngle)
      commandedAngle++;

    rudderServo.write(commandedAngle);
    indicatorServo.write(commandedAngle);

    delay(15);  // smooth movement
  }
}
  // ----- AUTO MODE -----
  else if (autoMode) {

    float currentHeading = getHeading();
    float error = courseHeading - currentHeading;

    error = constrain(error, -autoLimit, autoLimit);

    commandedAngle = centerAngle + error;

    if (abs(error) > 60) {
      tone(BUZZER, 2000);
    } else {
      noTone(BUZZER);
    }
  }

  // Final safety constrain
  commandedAngle = constrain(commandedAngle,
                             centerAngle - rudderLimit,
                             centerAngle + rudderLimit);

  rudderServo.write(commandedAngle);
  indicatorServo.write(commandedAngle);

  simulateShip(commandedAngle);
  updateLEDs(commandedAngle);
  updateDisplay(getHeading(), commandedAngle, autoMode, useJoystick);

  delay(20);
}

// ======================================

float getHeading() {
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);
  return gz / 131.0;
}

// ======================================

void simulateShip(int rudderAngle) {

  static float shipAngle = 90;

  float turnRate = (rudderAngle - 90) * 0.02;
  shipAngle += turnRate;

  shipAngle = constrain(shipAngle, 0, 180);
  shipServo.write(shipAngle);
}

// ======================================

void updateLEDs(int commandedAngle) {

  if (commandedAngle < centerAngle) {
    digitalWrite(LED_RIGHT, HIGH);
    digitalWrite(LED_LEFT, LOW);
  }
  else if (commandedAngle > centerAngle) {
    digitalWrite(LED_LEFT, HIGH);
    digitalWrite(LED_RIGHT, LOW);
  }
  else {
    digitalWrite(LED_LEFT, LOW);
    digitalWrite(LED_RIGHT, LOW);
  }
}

// ======================================

void updateDisplay(float heading, int rudderAngle, bool autoMode, bool useJoystick) {

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_6x12_tr);

  char buffer[30];

  sprintf(buffer, "Heading: %.1f", heading);
  u8g2.drawStr(0,15,buffer);

  sprintf(buffer, "Course : %.1f", courseHeading);
  u8g2.drawStr(0,30,buffer);

  sprintf(buffer, "Rudder : %d", rudderAngle - 90);
  u8g2.drawStr(0,45,buffer);

  if(autoMode)
    u8g2.drawStr(0,60,"Mode: AUTO");
  else if(useJoystick == LOW)
    u8g2.drawStr(0,60,"Mode: JOYSTICK");
  else
    u8g2.drawStr(0,60,"Mode: POT");

  u8g2.sendBuffer();
}