/*
 * ============================================================
 *  ARMx — Servo Tuner Utility
 * ============================================================
 * 
 *  This program is used to safely find the mechanical minimum 
 *  and maximum angles for each servo in your prosthetic hand.
 * 
 *  INSTRUCTIONS:
 *  1. Upload this sketch to your Teensy 4.1.
 *  2. Open the Arduino Serial Monitor (Baud: 115200).
 *  3. Type commands in the format: <Finger><Angle>
 *     Fingers: T (Thumb), I (Index), M (Middle), R (Ring), P (Pinky)
 *     Angle: 0 to 180
 * 
 *  EXAMPLES:
 *    T90  -> Moves Thumb to 90 degrees (center)
 *    I20  -> Moves Index to 20 degrees
 *    M160 -> Moves Middle to 160 degrees
 * 
 *  WARNING: Start near 90 degrees and slowly increase/decrease
 *  by 5-10 degrees at a time. Stop immediately if you hear the 
 *  servo buzzing loudly (which means it's stalling and could burn out).
 * 
 * ============================================================
 */

#include <Servo.h>

// Servo pin definitions (must match your hardware)
#define PIN_THUMB   2
#define PIN_INDEX   3
#define PIN_MIDDLE  4
#define PIN_RING    5
#define PIN_PINKY   6

Servo thumb;
Servo index;
Servo middle;
Servo ring;
Servo pinky;

void setup() {
    Serial.begin(115200);
    
    // Attach servos
    thumb.attach(PIN_THUMB);
    index.attach(PIN_INDEX);
    middle.attach(PIN_MIDDLE);
    ring.attach(PIN_RING);
    pinky.attach(PIN_PINKY);
    
    // Initialize to safe middle position
    thumb.write(90);
    index.write(90);
    middle.write(90);
    ring.write(90);
    pinky.write(90);
    
    Serial.println("===============================");
    Serial.println("  ARMx Servo Tuner Ready!");
    Serial.println("  Commands: T90, I45, M120, etc.");
    Serial.println("===============================");
}

void loop() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        input.toUpperCase();
        
        if (input.length() >= 2) {
            char finger = input.charAt(0);
            int angle = input.substring(1).toInt();
            
            // Constrain to physical servo limits to prevent complete destruction
            angle = constrain(angle, 0, 180);
            
            switch (finger) {
                case 'T':
                    thumb.write(angle);
                    Serial.print("Thumb moved to: ");
                    break;
                case 'I':
                    index.write(angle);
                    Serial.print("Index moved to: ");
                    break;
                case 'M':
                    middle.write(angle);
                    Serial.print("Middle moved to: ");
                    break;
                case 'R':
                    ring.write(angle);
                    Serial.print("Ring moved to: ");
                    break;
                case 'P':
                    pinky.write(angle);
                    Serial.print("Pinky moved to: ");
                    break;
                default:
                    Serial.println("Invalid finger! Use T, I, M, R, or P.");
                    return;
            }
            Serial.println(angle);
        }
    }
}