#include <Adafruit_MPU6050.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>

Servo yawController;
Adafruit_MPU6050 mpu;

int yawPin = 22;
int slideYawPin = 28;
int openPin = 3;
int buttonPin = 17;
const int ledCount = 10;

int ledPins[] = {
  6, 7, 8, 9, 10, 11, 12, 13, 14, 16
};

void setup() {
  for (int led = 0; led < ledCount; led++) {
    pinMode(ledPins[led], OUTPUT);
  }
  yawController.attach(yawPin);
  Serial1.begin(115200);
  Serial1.println("Hello, Raspberry Pi Pico W!");
  while (!mpu.begin()) {
    Serial1.println("MPU6050 not connected!");
    delay(500);
  }
  pinMode(slideYawPin, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);
}

sensors_event_t event;
int targetYaw;
int yAccel;
int ledIndex;
int oldValue = HIGH;
bool isOpen = false;

void loop() {

  targetYaw = analogRead(slideYawPin);
  Serial1.print("Raw targetYaw: ");
  Serial1.println(targetYaw);
  targetYaw = map(targetYaw, 0, 1023, 0, 180);
  yawController.write(targetYaw);
  mpu.getAccelerometerSensor()->getEvent(&event);
  yAccel = event.acceleration.y;

  if (yAccel > 5) {yAccel = 5;}
  if (yAccel < -5) {yAccel = -5;}

  ledIndex = yAccel + 5;

  for (int led=0; led<ledCount; led++) {
    digitalWrite(ledPins[led], LOW);
  }

  if (ledIndex >= 0 && ledIndex < ledCount) {
    digitalWrite(ledPins[ledIndex], HIGH);
  }

  int newValue = digitalRead(buttonPin);
  if (newValue != oldValue) {
    isOpen = !isOpen;
  }

  if (isOpen) {
    Serial1.println("OPEN");
    digitalWrite(openPin, HIGH);
  } else {
    digitalWrite(openPin, LOW);
  }

  delay(100);
}