#include <Servo.h>

Servo servo;
const int lightBtn = 3, lightTransistor = 5, gimbalBtn = 4;
boolean lightOn = false, lX = false, gimbalOn = false;
int lightBtnState = LOW, newLightBtnState;
int gimbalBtnState = LOW, newGimbalBtnState;

void setup() {
   pinMode(lightTransistor, OUTPUT);
   pinMode(lightBtn, INPUT_PULLUP);
   servo.attach(18); // Change pin later
}

void loop() {
  // Gets state from the light button
  newLightBtnState = digitalRead(lightBtn);
  newGimbalBtnState = digitalRead(gimbalBtn);

  // Toggles light on or off
  if (newLightBtnState == HIGH && lightBtnState == LOW) {
    if (lX == false) {
      digitalWrite(lightTransistor, LOW);
      lX = true;
    } else {
      digitalWrite(lightTransistor, HIGH);
      lX = false;
    }
  }
  lightBtnState = newLightBtnState;

 if (newGimbalBtnState == HIGH && gimbalBtnState == LOW) {
    if (lX == false) {
      //digitalWrite(lightTransistor, LOW);
      gimbalOn = true;
    } else {
      //digitalWrite(lightTransistor, HIGH);
      gimbalOn = false;
    }
  }
  gimbalBtnState = newGimbalBtnState;

  if (gimbalOn) {
    updateGimbal();
  }
}

void updateGimbal() {
  // Based on the assumption that the light is facing straight
  int test;
}

// CHECK BOARD SETTINGS!