#include <Servo.h>

// Pin definisi
const int servoPin1 = PA0;
const int servoPin2 = PA1;

const int flexPin1 = PA2;
const int flexPin2 = PA5;

const int switchPowerPin = PA3;     // Tombol power ON/OFF
const int switchModePin = PA6;      // HIGH = komunikasi, LOW = rehabilitasi
const int switchCalibPin = PA7;     // Tombol kalibrasi untuk komunikasi

const int buzzerPin = PA4;          // Buzzer aktif saat threshold terlewati

// Variabel kontrol
bool systemOn = false;
bool lastPowerState = HIGH; // default HIGH karena pakai INPUT_PULLUP
bool lastCalibState = HIGH;

int flex1Min = 1023, flex1Max = 0;
int flex2Min = 1023, flex2Max = 0;
int flex1Threshold, flex2Threshold;

Servo servo1;
Servo servo2;

void setup() {
  pinMode(switchPowerPin, INPUT_PULLUP);    // gunakan internal pull-up
  pinMode(switchModePin, INPUT);            // diasumsikan sinyal eksternal HIGH/LOW
  pinMode(switchCalibPin, INPUT_PULLUP);    // juga aktif-LOW
  pinMode(buzzerPin, OUTPUT);

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  Serial.begin(9600);
  Serial.println("Sistem Siap");
}

void loop() {
  // Baca status tombol power
  bool currentPowerState = digitalRead(switchPowerPin);
  if (currentPowerState == LOW && lastPowerState == HIGH) {
    systemOn = !systemOn;
    delay(200); // debounce
    Serial.println(systemOn ? "Sistem ON" : "Sistem OFF");
  }
  lastPowerState = currentPowerState;

  if (!systemOn) {
    buzzerOff();
    return;
  }

  // Mode deteksi
  bool isCommunicationMode = digitalRead(switchModePin);

  if (isCommunicationMode) {
    Serial.println("Mode: Komunikasi");

    // Kalibrasi bila tombol ditekan (aktif-LOW)
    bool calibState = digitalRead(switchCalibPin);
    if (calibState == LOW && lastCalibState == HIGH) {
      Serial.println("Kalibrasi Dimulai...");
      kalibrasiSensor();
      Serial.println("Kalibrasi Selesai");
    }
    lastCalibState = calibState;

    // Baca nilai sensor
    int val1 = analogRead(flexPin1);
    int val2 = analogRead(flexPin2);

    bool activated = false;

    if (val1 > flex1Threshold) {
      Serial.println("Gerakan Terdeteksi: Flex Sensor 1");
      activated = true;
    }

    if (val2 > flex2Threshold) {
      Serial.println("Gerakan Terdeteksi: Flex Sensor 2");
      activated = true;
    }

    if (activated) {
      buzzerOn(1000); // 1kHz
    } else {
      buzzerOff();
    }

  } else {
    Serial.println("Mode: Rehabilitasi");
    buzzerOff();
    terapiServo();
  }

  delay(300);
}

void kalibrasiSensor() {
  flex1Min = 1023; flex1Max = 0;
  flex2Min = 1023; flex2Max = 0;

  for (int i = 0; i < 10; i++) {
    int v1 = analogRead(flexPin1);
    int v2 = analogRead(flexPin2);
    if (v1 < flex1Min) flex1Min = v1;
    if (v1 > flex1Max) flex1Max = v1;
    if (v2 < flex2Min) flex2Min = v2;
    if (v2 > flex2Max) flex2Max = v2;
    delay(100);
  }

  flex1Threshold = (flex1Min + flex1Max) / 2;
  flex2Threshold = (flex2Min + flex2Max) / 2;

  Serial.print("Flex1 threshold: "); Serial.println(flex1Threshold);
  Serial.print("Flex2 threshold: "); Serial.println(flex2Threshold);
}

void terapiServo() {
  for (int pos = 0; pos <= 90; pos += 5) {
    servo1.write(pos);
    servo2.write(90 - pos);
    delay(50);
  }
  for (int pos = 90; pos >= 0; pos -= 5) {
    servo1.write(pos);
    servo2.write(90 - pos);
    delay(50);
  }
}

void buzzerOn(int freqHz) {
  int delayUs = 1000000 / (freqHz * 2);
  for (int i = 0; i < 200; i++) {
    digitalWrite(buzzerPin, HIGH);
    delayMicroseconds(delayUs);
    digitalWrite(buzzerPin, LOW);
    delayMicroseconds(delayUs);
  }
}

void buzzerOff() {
  digitalWrite(buzzerPin, LOW);
}