#include <Ethernet.h>
#include <Servo.h>
#include <Wire.h>
#include <MPU6050.h>
#include <PID_v1.h>

// Ethernet configuration (update with your network details)
byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED }; // MAC address of Ethernet Shield
IPAddress ip(192, 168, 1, 10);  // Arduino's IP address
EthernetServer server(80);      // Web server on port 80

// Servo objects for controlling the steering
Servo servo1; // Left servo
Servo servo2; // Right servo
Servo thruster; // Thruster control

// MPU-6050 object
MPU6050 mpu;

// PID control variables for steering
double desiredHeading = 0; // Desired heading input via web interface
double currentHeading = 0; // Current heading from Kalman filter
double error = 0;          // Difference between desired and current heading
double pidOutput = 0;      // Output from PID controller

// PID constants (Further reduce values)
double Kp = 5.0, Ki = 0.0, Kd = 0.005;

// Create a PID object
PID myPID(&error, &pidOutput, 0, Kp, Ki, Kd, DIRECT);  // PID uses error to calculate output

// Store current angle for the servos
int currentAngle1 = 90;  // Left servo's current angle (0-180)
int currentAngle2 = 90;  // Right servo's current angle (0-180)

// Store current speed for the thruster
int currentThrusterPWM = 1500; // Thruster PWM value (1100-1900, with 1500 as neutral)

// Kalman filter variables
float Q_angle = 0.001; // Process noise variance for the accelerometer
float Q_bias = 0.003;  // Process noise variance for the gyroscope bias
float R_measure = 0.03; // Measurement noise variance

float angle = 0; // The angle calculated by the Kalman filter
float bias = 0;  // The gyroscope bias calculated by the Kalman filter
float rate = 0;  // The rate of angle change as measured by the gyroscope

float P[2][2] = {{0, 0}, {0, 0}}; // Error covariance matrix

unsigned long lastTime; // Time tracking for the Kalman filter

// MPU-6050 calibration offsets
float accelXOffset = 0, accelYOffset = 0, accelZOffset = 0;
float gyroXOffset = 0, gyroYOffset = 0, gyroZOffset = 0;

// Heading offset after calibration
float headingOffset = 0;

// Set a deadband threshold for small errors (in degrees)
double errorTolerance = 10.0; // Error threshold where we consider the system to be "close enough"

void setup() {
  // Start Ethernet and server
  Ethernet.begin(mac, ip);
  server.begin();

  // Attach the servos to pins
  servo1.attach(7);  // Pin 7 for Left Servo
  servo2.attach(8); // Pin 10 for Right Servo
  thruster.attach(11); // Pin 11 for Thruster control

  // Initialize MPU-6050
  Wire.begin();
  mpu.initialize();

  // Start serial for debugging
  Serial.begin(9600);
  Serial.println("Server started, waiting for client...");
 
  // Check MPU connection
  if (mpu.testConnection()) {
    Serial.println("MPU6050 connection successful");
  } else {
    Serial.println("MPU6050 connection failed");
  }

  // Calibrate MPU-6050 and set heading offset
  calibrateMPU6050();

  // Set initial positions of the servos
  servo1.write(currentAngle1);
  servo2.write(currentAngle2);

  // Initialize the PID controller
  myPID.SetMode(AUTOMATIC);

  // Set output range for PID (appropriate for servo control, 0 to 180 degrees)
  myPID.SetOutputLimits(-90, 90);  // Smaller range to limit corrections further
 
  // Initialize Kalman filter
  lastTime = millis();
}

void loop() {
  // Read current heading from MPU-6050 and apply offset
  currentHeading = getKalmanHeading() - headingOffset;

  // Calculate the error (difference between desired and current heading)
  error = desiredHeading - currentHeading;

  // Handle circular heading (e.g., 0 and 360 degrees should be close)
  if (error > 180) error -= 360;
  if (error < -180) error += 360;

  // Check if the error is within the tolerance (deadband)
  if (abs(error) < errorTolerance) {
    error = 0; // Prevent unnecessary corrections
  }
  // Compute the PID output using the error
  myPID.Compute();
  Serial.print("PID Output After Compute: ");
Serial.println(pidOutput);
  // Print the current heading, error, and PID output to the Serial Monitor
  Serial.print("Current Heading: ");
  Serial.println(currentHeading);
  Serial.print("Error: ");
  Serial.println(error);
  Serial.print("pidOutput: ");
  Serial.println(pidOutput);
  Serial.print("Desired Heading: ");
Serial.println(desiredHeading);
Serial.print("Kalman Heading: ");
Serial.println(currentHeading);
Serial.print("Kp: ");
Serial.println(Kp);
Serial.print("Ki: ");
Serial.println(Ki);
Serial.print("Kd: ");
Serial.println(Kd);



  // Update servo positions based on PID output
  // Servo angles are constrained to between 0 and 180 degrees
  currentAngle1 = constrain(90 + pidOutput, 0, 180); // Left servo
  currentAngle2 = constrain(90 - pidOutput, 0, 180); // Right servo

  servo1.write(currentAngle1);
  servo2.write(currentAngle2);

  // Set the thruster PWM value
  thruster.writeMicroseconds(currentThrusterPWM);

  // Check for client connection
  EthernetClient client = server.available();
  if (client) {
    boolean currentLineIsBlank = true;
    String request = "";

    while (client.connected()) {
      if (client.available()) {
        char c = client.read();
        request += c;

        // If the request ends with a blank line, process it
        if (c == '\n' && currentLineIsBlank) {
          // Check if the client is requesting the current heading
          if (request.indexOf("GET /heading") >= 0) {
            // Send the current heading in plain text
            client.println("HTTP/1.1 200 OK");
            client.println("Content-Type: text/plain");
            client.println("Connection: close");
            client.println();
            client.print(currentHeading);
            client.println();
          }
          // Process the normal web interface request
          else {
            // Send the HTTP header
            client.println("HTTP/1.1 200 OK");
            client.println("Content-Type: text/html");
            client.println("Connection: close");
            client.println();

            // Handle the GET request to update heading, PID, and thruster
            if (request.indexOf("GET /?") >= 0) {
              // Extract the desired heading
              int headingIndex = request.indexOf("heading=");
              if (headingIndex >= 0) {
                int headingValue = request.substring(headingIndex + 8, request.indexOf('&', headingIndex)).toInt();
                desiredHeading = constrain(headingValue, 0, 360);  // Constrain desired heading
              }

              // Extract PID values
              int kpIndex = request.indexOf("Kp=");
              if (kpIndex >= 0) {
                Kp = request.substring(kpIndex + 3, request.indexOf('&', kpIndex)).toDouble();
                myPID.SetTunings(Kp, Ki, Kd); // Update PID tunings
              }
              int kiIndex = request.indexOf("Ki=");
              if (kiIndex >= 0) {
                Ki = request.substring(kiIndex + 3, request.indexOf('&', kiIndex)).toDouble();
                myPID.SetTunings(Kp, Ki, Kd); // Update PID tunings
              }
              int kdIndex = request.indexOf("Kd=");
              if (kdIndex >= 0) {
                Kd = request.substring(kdIndex + 3, request.indexOf(' ', kdIndex)).toDouble();
                myPID.SetTunings(Kp, Ki, Kd); // Update PID tunings
              }

              // Extract thruster PWM
              int thrusterIndex = request.indexOf("thruster=");
              if (thrusterIndex >= 0) {
                int pwmValue = request.substring(thrusterIndex + 9, request.indexOf(' ', thrusterIndex)).toInt();
                currentThrusterPWM = constrain(pwmValue, 1100, 1900);  // Ensure PWM is between 1100 and 1900 µs
              }
            }

            // Send the HTML page with current values pre-filled
            client.println("<html>");
            client.println("<head><title>Boat Control</title></head>");
            client.println("<body>");
            client.println("<h1>Boat Control</h1>");
            client.print("Current Heading: ");
            client.print(currentHeading);
            client.println("<br>");
            client.print("Left Servo Angle: ");
            client.print(currentAngle1);
            client.println("<br>");
            client.print("Right Servo Angle: ");
            client.print(currentAngle2);
            client.println("<br>");
            client.println("<form action=\"/\" method=\"get\">");
            client.print("Desired Heading (0-360): <input type=\"number\" name=\"heading\" min=\"0\" max=\"360\" value=\"");
            client.print(desiredHeading);
            client.println("\"><br>");
            client.print("Kp: <input type=\"number\" name=\"Kp\" step=\"0.01\" value=\"");
            client.print(Kp);
            client.println("\"><br>");
            client.print("Ki: <input type=\"number\" name=\"Ki\" step=\"0.01\" value=\"");
            client.print(Ki);
            client.println("\"><br>");
            client.print("Kd: <input type=\"number\" name=\"Kd\" step=\"0.01\" value=\"");
            client.print(Kd);
            client.println("\"><br>");
            client.print("Thruster Speed (1100-1900): <input type=\"number\" name=\"thruster\" min=\"1100\" max=\"1900\" value=\"");
            client.print(currentThrusterPWM);
            client.println("\"><br>");
            client.println("<input type=\"submit\" value=\"Submit\">");
            client.println("</form>");
            client.println("</body>");
            client.println("</html>");
          }
          break;
        }
        if (c == '\n') {
          currentLineIsBlank = true;
        } else if (c != '\r') {
          currentLineIsBlank = false;
        }
      }
    }

    // Close the connection
    client.stop();
  }
}

// Function to read heading from MPU-6050 using Kalman filter
float getKalmanHeading() {
  int16_t accelX, accelY, accelZ;
  int16_t gyroX, gyroY, gyroZ;

  mpu.getMotion6(&accelX, &accelY, &accelZ, &gyroX, &gyroY, &gyroZ);

  // Convert raw accelerometer data to angles
  float accelAngle = atan2(accelY, accelX) * 180 / PI;
  if (accelAngle < 0) accelAngle += 360;

  // Calculate the time elapsed (dt) in seconds
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;

  // Kalman filter update
  rate = (gyroZ - gyroZOffset) / 131.0; // Convert gyro to degrees/second
  angle += dt * (rate - bias);

  // Update estimation error covariance
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  // Calculate the Kalman gain
  float S = P[0][0] + R_measure;
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;

  // Update estimate with accelerometer data
  float y = accelAngle - angle;
  angle += K[0] * y;
  bias += K[1] * y;

  // Update covariance matrix
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];

  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
}

// Function to calibrate MPU-6050 and set the heading offset
void calibrateMPU6050() {
  const int calibrationSamples = 1000;
  long axSum = 0, aySum = 0, azSum = 0;
  long gxSum = 0, gySum = 0, gzSum = 0;

  Serial.println("Calibrating MPU6050...");

  for (int i = 0; i < calibrationSamples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    axSum += ax;
    aySum += ay;
    azSum += az;
    gxSum += gx;
    gySum += gy;
    gzSum += gz;

    delay(3); // Small delay between samples
  }

  accelXOffset = axSum / calibrationSamples;
  accelYOffset = aySum / calibrationSamples;
  accelZOffset = azSum / calibrationSamples;
  gyroXOffset = gxSum / calibrationSamples;
  gyroYOffset = gySum / calibrationSamples;
  gyroZOffset = gzSum / calibrationSamples;

  // Set the heading offset after calibration
  headingOffset = getKalmanHeading();

  Serial.println("MPU6050 Calibration complete.");
  Serial.print("Accel Offsets: ");
  Serial.print(accelXOffset); Serial.print(", ");
  Serial.print(accelYOffset); Serial.print(", ");
  Serial.println(accelZOffset);
  Serial.print("Gyro Offsets: ");
  Serial.print(gyroXOffset); Serial.print(", ");
  Serial.print(gyroYOffset); Serial.print(", ");
  Serial.println(gyroZOffset);
  Serial.print("Heading Offset: ");
  Serial.println(headingOffset);
}