// Kayla Frost 300390878
// Sprint 1 Project
// Oct. 28, 2025

// Include servo library for the Servo motor
#include <Servo.h>

// Define pin number to LED
const int tled = 11; // turquoise led connected to pin 11

const int ServoPin = 12; // Servo is attached to pin 12
Servo myservo; // The servo motor is called myservo
int angle = 0; // Variable to store servo motor's angle position (in degrees)

const int xPin = A0; // horiz (blue wire)
const int yPin = A1; // vert (green wire)
const int buttonPin = 9; // joystick button (magenta wire)
int xValue;
int yValue; 
int buttonState; // is button pressed or unpressed?

int xOffset = 0;
int yOffset = 0;
float angleRad = 0.0;
float angleDeg = 0.0;

void setup() {
  // put your setup code here, to run once:

  //Define LED as an output
  pinMode(tled, OUTPUT);

  //Initially, turn off LED
  digitalWrite(tled, HIGH);

  //Set up servo motor
  myservo.attach(ServoPin); // attach servo motor to its pin (pin 12)
  myservo.write(0); // set initial servo position to be 0 degrees

  Serial.begin(9600);
  pinMode(xPin, INPUT);
  pinMode(yPin, INPUT);
  pinMode(buttonPin, INPUT_PULLUP);

}

void loop() {
  // put your main code here, to run repeatedly:
  
  // --- Read Joystick ---
  xValue = analogRead(xPin);
  yValue = analogRead(yPin);
  buttonState = digitalRead(buttonPin);

  // --- LED Control ---
  if (buttonState == LOW) //if the button is pressed
  {
    digitalWrite(tled, LOW);  // LED ON when pressed
  } 
  else 
  {
    digitalWrite(tled, HIGH);   // LED OFF when not pressed
  }

  // --- Servo Control ---
  // Center joystick offset around (0,0)
  xOffset = xValue - 512;
  yOffset = yValue - 512;


  // Calculate joystick angle (in degrees)
  angleRad = atan2(yOffset, xOffset); // Radians (-pi to +pi)
  angleDeg = angleRad * 180.0 / PI;  // Convert to degrees (-180 to +180)

  angle = map((int)angleDeg, -180, 180, 0, 180);
  angle = constrain(angle, 0, 180);
  
  myservo.write(angle);

  // --- Debugging info ---
  Serial.print("X: "); Serial.print(xValue);
  Serial.print(" | Y: "); Serial.print(yValue);
  Serial.print(" | Yoffset: "); Serial.print(yOffset);
  Serial.print(" | Angle: "); Serial.print(angleDeg);
  Serial.print(" | Servo: "); Serial.println(angle);

  delay(20);

}