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

Servo sg90X; // Servo for X-axis
Servo sg90Y; // Servo for Y-axis
Servo sg90Flex; // Servo controlled by flex sensor
Servo sg90Button; // Servo controlled by push button

int servo_pinX = 2; // Servo pin for X-axis
int servo_pinY = 3; // Servo pin for Y-axis
int servo_pinFlex = 6; // Servo pin for flex sensor
int servo_pinButton = 5; // Servo pin for push button control

int flexPin = A0; // Flex sensor pin
int buttonPin = 7; // Push button pin

MPU6050 sensor;

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  sg90X.attach(servo_pinX);
  sg90Y.attach(servo_pinY);
  sg90Flex.attach(servo_pinFlex);
  sg90Button.attach(servo_pinButton);
  
  Wire.begin();
  Serial.begin(9600);
  Serial.println("Initializing the sensor");
  sensor.initialize();
  delay(1000);
  
  pinMode(buttonPin, INPUT_PULLUP); // Configure button pin as input with internal pull-up resistor
}

void loop() {
  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  
  int servoPos_X = map(ax, -17000, 17000, 0, 180); // Mapping accelerometer X-axis data to servo position
  int servoPos_Y = map(ay, -17000, 17000, 0, 180); // Mapping accelerometer Y-axis data to servo position
  int flexPos = map(analogRead(flexPin), 0, 1023, 0, 180); // Mapping flex sensor reading to servo position
  int buttonPos = digitalRead(buttonPin) == LOW ? 90 : 0; // If button is pressed, set position to 90 degrees, otherwise 0 degrees
  


  sg90X.write(constrain(servoPos_X, 0, 180)); // Constrain servo positions within 0-180 degrees
  sg90Y.write(constrain(servoPos_Y, 0, 180));
  sg90Flex.write(constrain(flexPos, 0, 180));
  sg90Button.write(constrain(buttonPos, 0, 180));

  delay(200);
}