#include <Servo.h>

// Define the servos
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;

// Define the joystick pins
const int joyX = A0;
const int joyY = A1;
const int joyZ = A2;

// Define the servo pins
const int servo1Pin = 7;
const int servo2Pin = 8;
const int servo3Pin = 9;
const int servo4Pin = 10;
const int servo5Pin = 12;
const int servo6Pin = 12;

// Define the platform parameters
const float platformRadius = 50.0; // mm
const float baseRadius = 60.0; // mm
const float servoArmLength = 25.0; // mm
const float rodLength = 100.0; // mm

// Define the initial position of the platform
float x = 0.0;
float y = 0.0;
float z = -60.0;

// Define the moving average filter parameters
const int numReadings = 10;
int xReadings[numReadings];
int yReadings[numReadings];
int zReadings[numReadings];
int readIndex = 0;
int xTotal = 0;
int yTotal = 0;
int zTotal = 0;

void setup() {
  // Attach the servos to their pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
  servo4.attach(servo4Pin);
  servo5.attach(servo5Pin);
  servo6.attach(servo6Pin);

  // Set the initial position of the servos
  updateServos();

  // Initialize all the readings to 0
  for (int i = 0; i < numReadings; i++) {
    xReadings[i] = yReadings[i] = zReadings[i] = 0;
  }
}

void loop() {
  // Read the joystick values
  int joyXVal = analogRead(joyX);
  int joyYVal = analogRead(joyY);
  int joyZVal = analogRead(joyZ);

  // Map the joystick values to the platform position
  x = map(joyXVal, 0, 1023, 0, 40);
  y = map(joyYVal, 0, 1023, 0, 40);
  z = map(joyZVal, 0, 1023, 0, 40);

  // Update the servos
  updateServos();
}

void updateServos() {
    // Calculate the angles for each servo using inverse kinematics
    float theta1 = calculateAngle(x, y, z, PI/6);
    float theta2 = calculateAngle(x, y, z, PI/2);
    float theta3 = calculateAngle(x, y, z, (5*PI)/6);
    float theta4 = calculateAngle(x, y, z, (7*PI)/6);
    float theta5 = calculateAngle(x, y, z, (3*PI)/2);
    float theta6 = calculateAngle(x, y, z, (11*PI)/6);

    // Convert the angles to degrees and write them to the servos
    servo1.write(theta1 * (180/PI));
    servo2.write(theta2 * (180/PI));
    servo3.write(theta3 * (180/PI));
    servo4.write(theta4 * (180/PI));
    servo5.write(theta5 * (180/PI));
    servo6.write(theta6 * (180/PI));
}

float calculateAngle(float x, float y, float z, float alpha) {
    // Calculate the position of the rod end on the platform
    float xp = platformRadius * cos(alpha);
    float yp = platformRadius * sin(alpha);

    // Calculate the position of the rod end on the base
    float xb = baseRadius * cos(alpha);
    float yb = baseRadius * sin(alpha);

    // Calculate the distance between the rod ends
    float dx = xb - xp - x;
    float dy = yb - yp - y;
    float dz = -z;

    // Calculate the angle using inverse kinematics
    float l1 = sqrt(dx*dx + dy*dy + dz*dz);
    float l2 = sqrt(rodLength*rodLength - l1*l1/4);
    float beta1 = atan2(dz,l1/2) + atan2(l2,l1/2);
    float beta2 = atan2(dy,dx) + atan2(l2,servoArmLength);

    return beta1 - beta2;
}