/*
  Test Scripts for Two Axis Drawing Arm 
  Date: 3/6/2026

*/

//-------------------------------------------
/*
  Test Script for testing the RGB LED for user feedback 1
  By: Andrew Ng
  Date: 3/6/2026
*/

// Define GPIO Pins (Use GPIO numbers, not physical pin numbers)
const int RED_PIN   = 25;
const int GREEN_PIN = 27;
const int BLUE_PIN  = 26; //Can be ignored

void setup()
{
  // Starts the Communication
  Serial.begin(115200);
  // Initialize pins as outputs
  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);
}

void drawMode()
{
  setRGB(0, 255, 0); // Sets led to the color green to represent drawing in-progress
  Serial.println("STATUS: DRAWING IN-PROGRESS(GREEN)");
  //...
  delay(3000); // Represents the execution time of drawing (3-seconds for example)
  //...
  setRGB(0,0,0); // Turns off LED when drawing is done
}

void emergencyStop()
{
  setRGB(255, 0, 0); // Sets led to the color red to represent emergency stop
  Serial.println("STATUS: EMERGENCY STOP IN-PROGRESS(RED)");
  //...
  delay(3000); // Represents the execution time of the emergency stop (3-seconds for example)
  //...
  setRGB(0,0,0); // Turns off LED when the emergency stop has been lifted
}


void loop()
{
  drawMode();
  Serial.println("STATUS: PAUSE(NO LIGHT)");
  delay(4000);
  emergencyStop();
  Serial.println("STATUS: PAUSE(NO LIGHT)");
  delay(4000);
}

// Helper function to write values to the pins
void setRGB(int r, int g, int b)
{
  analogWrite(RED_PIN, r);
  analogWrite(GREEN_PIN, g);
  analogWrite(BLUE_PIN, b);
}
//-------------------------------------------

/*
  Test Script for testing the emergency stop button for User Control Block
  By: Andrew Ng
  Date: 3/6/2026
*/

// GPIO pin for the Emergency Stop Switch (UC1_MC_dsig)
const int STOP_SWITCH_PIN = 34; // Update with your specific GPIO pin

void setup() {
  // Initialize Serial communication at 115200 baud
  Serial.begin(115200);
  
  // Set the switch pin as an input
  // Use INPUT_PULLUP if your switch is not externally pulled up 
  pinMode(STOP_SWITCH_PIN, INPUT);
  
  Serial.println("System Initialized. Starting spam...");
}

void loop() {
  // Read the current state of the switch 
  int switchState = digitalRead(STOP_SWITCH_PIN);

  // If the switch reads 0 (LOW), the loop enters a "paused" state 
  if (switchState == LOW) {
    Serial.println("!!! EMERGENCY STOP DETECTED - OUTPUT PAUSED !!!"); 
    
    // Stay in this while loop as long as the switch is 0 
    while(digitalRead(STOP_SWITCH_PIN) == LOW) {
      delay(100); // Small delay to prevent watchdog issues
    }
    
    Serial.println("Emergency Stop Released. Resuming..."); 
  }

  // Normal operation: spamming text 
  Serial.println("ROBOTIC ARM OPERATIONAL - SENDING DATA...");
  delay(100); // Adjust delay to control spam speed
}
//-------------------------------------------
/*
  Test Script for testing the LCD display for user feedback 2
  By: Jacob Haley
  Date: 3/6/2026
*/

#include <Arduino.h>
#include <LiquidCrystal.h>

// ---- VERIFIED PIN MAPPING (DO NOT CHANGE) ----
// LCD RS -> GPIO14
// LCD E  -> GPIO13
// LCD D4 -> GPIO33
// LCD D5 -> GPIO32
// LCD D6 -> GPIO15
// LCD D7 -> GPIO4
// LCD R/W -> GND (write-only)

LiquidCrystal lcd(14, 13, 33, 32, 15, 4);  //This is the equivalent standard to parallel communication in the liquid crystal library

// Requirement: Update Rate >= 5 Hz  -> period <= 200 ms
static const uint32_t UPDATE_PERIOD_MS = 200;
uint32_t lastUpdateMs = 0;
uint32_t updateCount = 0;
float x_mm = 0.0f;
float y_mm = 0.0f;

void printPaddedFloat(float val, uint8_t decimals) {
  // Prints float and then pads spaces to clear leftover characters.
  // (LCD doesn't auto-clear old characters.)
  lcd.print(val, decimals);
  lcd.print("     ");
}

void setup() {
  Serial.begin(115200);
  delay(200);

  lcd.begin(16, 2);
  lcd.clear();

  // Boot banner
  lcd.setCursor(0, 0);
  lcd.print("UF2 LCD TEST");
  lcd.setCursor(0, 1);
  lcd.print("BOOT OK");

  Serial.println("UF2 LCD TESTBENCH START");
  Serial.println("Expect update rate >= 5 Hz (<=200 ms/update).");

  delay(1000);
  lcd.clear();
  lastUpdateMs = millis();
}

void loop() {
  uint32_t now = millis();
  if (now - lastUpdateMs >= UPDATE_PERIOD_MS) {
    uint32_t dt = now - lastUpdateMs;   // actual update interval (ms)
    lastUpdateMs = now;
    updateCount++;

    // --- Generate test coordinates (replace later with real arm coords) ---
    x_mm += 1.7f;
    y_mm += 0.9f;
    if (x_mm > 210.0f) x_mm = 0.0f;
    if (y_mm > 297.0f) y_mm = 0.0f;

    // --- LCD update ---
    lcd.setCursor(0, 0);
    lcd.print("X:");
    printPaddedFloat(x_mm, 1);

    lcd.setCursor(0, 1);
    lcd.print("Y:");
    printPaddedFloat(y_mm, 1);

    // --- Serial verification output ---
    // Hz = 1000 / dt
    float hz = 1000.0f / (float)dt;

    Serial.print("Update ");
    Serial.print(updateCount);
    Serial.print(" @ ");
    Serial.print(now);
    Serial.print(" ms | dt=");
    Serial.print(dt);
    Serial.print(" ms | rate=");
    Serial.print(hz, 2);
    Serial.println(" Hz");
  }
}
//-------------------------------------------
/*
  Test Script for testing the LCD display for user feedback 2
  By: Cade Myers
  Date: 3/6/2026
*/

//Pseudocode:
void loop() {
  if (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    line.trim();
    if (line.length() == 0) return;

    // parse the leading word: G0, G1, G90, etc.
    // update modes for G20/G21/G90/G91
    // for G0/G1: extract X/Y/F if present
    // compute target XY (abs/rel + units)
    // segment line into points
    // for each point:
    //   IK -> theta1, theta2
    //   theta1_servo = theta1/2  (2:1 belt)
    //   command servos (PWM)
    // update current XY
    // if M2: stop program
    Serial.println("ok");
  }
}
//-------------------------------------------