#include <Wire.h>
#include <MechaQMC5883.h>
#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>
#include <TinyStepper_28BYJ_48.h>

#define RXPin 3
#define TXPin 4
#define GPSBaud 9600
#define LEDPin 13

MechaQMC5883 qmc;
TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);
TinyStepper_28BYJ_48 stepper;

float HOME_LAT = 0.0;
float HOME_LON = 0.0;
float DECLINATION_ANGLE = +2.20;

const int stepsPerRevolution = 2048;
float currentHeading = 0;
const float MIN_HEADING_DIFF = 5.0;

void setup() {
  initializeComponents();
  Serial.println(F("GPS, Compass, and Stepper Motor Test"));
  Serial.println(F("Waiting for GPS data..."));
}

void loop() {
  updateGPSData();
  if (gps.location.isValid()) {
    executeNavigationLogic();
  } else {
    digitalWrite(LEDPin, LOW);
  }
}

void initializeComponents() {
  Wire.begin();
  Serial.begin(9600);
  ss.begin(GPSBaud);
  pinMode(LEDPin, OUTPUT);
  digitalWrite(LEDPin, LOW);
  qmc.init();
  setupStepper();
}

void setupStepper() {
  stepper.connectToPins(8, 9, 10, 11);
  stepper.setSpeedInStepsPerSecond(256);
  stepper.setAccelerationInStepsPerSecondPerSecond(512);
}

void updateGPSData() {
  while (ss.available()) {
    gps.encode(ss.read());
  }
}

void executeNavigationLogic() {
  digitalWrite(LEDPin, HIGH);
  displayGPSData();
  float headingDegrees = readCompass();
  float angleDegrees = calculateAngleToHome();
  adjustMotor(headingDegrees, angleDegrees);
}

void displayGPSData() {
  Serial.print("Latitude: ");
  Serial.println(gps.location.lat(), 6);
  Serial.print("Longitude: ");
  Serial.println(gps.location.lng(), 6);
}

float readCompass() {
  int x, y, z;
  qmc.read(&x, &y, &z);
  float heading = atan2(y, x);
  heading += DECLINATION_ANGLE;
  normalizeAngle(heading);
  float headingDegrees = heading * 180 / M_PI;
  return headingDegrees;
}

void normalizeAngle(float &angle) {
  if (angle < 0) angle += 2 * PI;
  if (angle > 2 * PI) angle -= 2 * PI;
}

float calculateAngleToHome() {
  float angleToHome = atan2(HOME_LON - gps.location.lng(), HOME_LAT - gps.location.lat());
  angleToHome += DECLINATION_ANGLE;
  normalizeAngle(angleToHome);
  return angleToHome * 180 / M_PI;
}

void adjustMotor(float headingDegrees, float angleDegrees) {
  float headingDifference = angleDegrees - currentHeading;
  normalizeHeadingDifference(headingDifference);
  currentHeading = headingDegrees;
  if (abs(headingDifference) >= MIN_HEADING_DIFF) {
    int targetSteps = headingDifference * (stepsPerRevolution / 360);
    for (int i = 0; i < abs(targetSteps); i++) {
      stepMotor(targetSteps, headingDifference, angleDegrees);
    }
  }
}

void normalizeHeadingDifference(float &headingDifference) {
  if (headingDifference > 180) {
    headingDifference -= 360;
  } else if (headingDifference < -180) {
    headingDifference += 360;
  }
}

void stepMotor(int targetSteps, float &headingDifference, float angleDegrees) {
  float headingChangePerStep = headingDifference / targetSteps;
  if (targetSteps > 0) {
    currentHeading += headingChangePerStep;
    stepper.moveRelativeInSteps(1);
  } else {
    currentHeading -= headingChangePerStep;
    stepper.moveRelativeInSteps(-1);
  }
  normalizeAngle(currentHeading);
  headingDifference = angleDegrees - currentHeading;
  normalizeHeadingDifference(headingDifference);
  delay(10);
}