// ===============================================
// FINAL CODE: STABLE SELF-CYCLING WITH STRICT DEAD ZONE
// ===============================================

#include "ble_functions.h" 
const int PLAYER_NUMBER = 18; 

// --- 1. MOVEMENT STATE ---
int cycleDirection = 1;
unsigned long lastCycleTime = 0;

// --- 2. PIN & THRESHOLD DEFINITIONS ---
const int UP_SENSOR_PIN = A2;     
const int DOWN_SENSOR_PIN = A3;   
const int BUZZER_PIN = 9;       
const int LED_PIN = LED_BUILTIN; 

// --- CONTROL CONSTANTS (Stable Flow) ---
const int ACTIVATION_THRESHOLD = 600; 
const int DIRECTION_DOMINANCE_MARGIN = 150; 
const int CYCLE_TIME_MS = 75; 
const int DOWN_BIAS_CORRECTION = 150; 


// --- 3. BUZZER FUNCTIONS (LIGHT FEEDBACK) ---
void playBEEP() { tone(BUZZER_PIN, 2000, 20); } 
void playBOOP() { tone(BUZZER_PIN, 500, 30); }  

// --- 4. SETUP FUNCTION ---
void setup() {
  Serial.begin(115200); 
  pinMode(BUZZER_PIN, OUTPUT); 
  
  Serial.println("Stable Self-Cycling Override Ready.");
  setupBLE("DF-Pong", PLAYER_NUMBER, LED_BUILTIN); 
}

// --- 5. MAIN LOOP (The Control Logic) ---
void loop() {
  updateBLE(); 
  
  // B. Read the pressure and APPLY BIAS CORRECTION
  int upValue = analogRead(UP_SENSOR_PIN);
  int downValueRaw = analogRead(DOWN_SENSOR_PIN);
  
  // Apply correction to the DOWN sensor (A3)
  int downValue = downValueRaw - DOWN_BIAS_CORRECTION; 
  int rawDifference = upValue - downValue;
  
  int finalCommand; 

  // C. Implement Pressure Override Logic
  
  // 1. Check for Override: UP
  if (upValue >= ACTIVATION_THRESHOLD && rawDifference > DIRECTION_DOMINANCE_MARGIN) {
    finalCommand = 1; 
    cycleDirection = 1; 
    playBEEP();
  }
  
  // 2. Check for Override: DOWN
  else if (downValue >= ACTIVATION_THRESHOLD && rawDifference < -DIRECTION_DOMINANCE_MARGIN) {
    finalCommand = 2; 
    cycleDirection = 2; 
    playBOOP();
  }
  
  // D. Implement Self-Cycling Flow (Default Behavior)
  else {
    // Check if the cycle time has expired
    if (millis() > lastCycleTime + CYCLE_TIME_MS) {
      // Toggle the direction
      cycleDirection = (cycleDirection == 1) ? 2 : 1; 
      lastCycleTime = millis();
    }
    finalCommand = cycleDirection; 
  }

  // E. DEBUGGING: Print values 
  Serial.print("UP: ");
  Serial.print(upValue);
  Serial.print(" | DOWN(Adj): ");
  Serial.print(downValue);
  Serial.print(" | FINAL COMMAND: ");
  Serial.println(finalCommand);
  
  // F. Send the command wirelessly via BLE 
  sendMovement(finalCommand); 
  
  // G. Small delay to prevent runaway serial output
  delay(10); 
}