// Arduino Nano FLEX SENSOR - INDUSTRIAL BEND CONTROLLER
// A0 + 10kΩ divider | Angle calc + Servo + 5-LED bar + Gestures

const int flexPin = A0;
const int servoPin = 9;
const int ledPins[] = {3,4,5,6,7};  // 5-LED bar
int straightVal, bentVal;

void setup() {
  Serial.begin(9600);
  
  // LED bar setup
  for(int i=0; i<5; i++) {
    pinMode(ledPins[i], OUTPUT);
  }
  
  // Servo setup
  pinMode(servoPin, OUTPUT);
  
  // Auto-calibration
  calibrateFlex();
  
  Serial.println("=== FLEX SENSOR PROFESSIONAL CONTROLLER ===");
  Serial.println("Raw  Bend°  | LEDs | Servo | Gesture");
}

void loop() {
  int rawVal = analogRead(flexPin);
  float bendAngle = mapAngle(rawVal);
  int ledCount = map(bendAngle, 0, 90, 0, 5);
  int servoPos = map(bendAngle, 0, 90, 0, 180);
  
  // LED bar graph
  updateLedBar(ledCount);
  
  // Servo position
  servoWrite(servoPin, servoPos);
  
  // Gesture detection
  String gesture = detectGesture(bendAngle);
  
  // Live display
  Serial.printf("%4d %5.1f | %2d/5 | %3d° | %s\n", 
                rawVal, bendAngle, ledCount, servoPos, gesture.c_str());
  
  delay(10);  // 100Hz
}

void calibrateFlex() {
  Serial.println("CALIBRATE: Hold STRAIGHT 3s...");
  delay(3000);
  straightVal = analogRead(flexPin);
  
  Serial.println("CALIBRATE: Bend FULL 3s...");
  delay(3000);
  bentVal = analogRead(flexPin);
  
  Serial.printf("Straight: %d, Bent: %d (90° range)\n", straightVal, bentVal);
}

float mapAngle(int raw) {
  // Linear interpolation straight(0°) → bent(90°)
  if(straightVal == bentVal) return 0;
  return constrain(map(raw, straightVal, bentVal, 0, 90), 0, 90);
}

void updateLedBar(int count) {
  for(int i=0; i<5; i++) {
    digitalWrite(ledPins[i], i < count ? HIGH : LOW);
  }
}

void servoWrite(int pin, int pos) {
  // Simple servo pulse (50Hz)
  int pulse = map(pos, 0, 180, 1000, 2000);
  digitalWrite(pin, HIGH);
  delayMicroseconds(pulse);
  digitalWrite(pin, LOW);
  delay(20 - pulse/1000);  // 20ms period
}

String detectGesture(float angle) {
  if(angle < 10) return "STRAIGHT";
  if(angle < 30) return "SLIGHT";
  if(angle < 60) return "MEDIUM";
  if(angle > 80) return "FULL BEND";
  return "BENDING";
}

// Add to loop() after angle calc
if(angle > 45) {
  // Send 'GRIP' command
  radio.write(&command, sizeof(command));
}