#include <SoftwareSerial.h>
#include <Servo.h>

#define servoPin 9
#define escMotorPin 12
// הגדרות פינים קבועות
const int PIN_RX = 10;
const int PIN_TX = 11;
const int M0 = 7;
const int M1 = 8;

SoftwareSerial loraSerial(PIN_RX, PIN_TX);
Servo steeringServo;
Servo escMotor;

// מבנה נתונים ארוז - חייב להיות זהה לשלט
struct __attribute__((packed)) ControlData {
  byte header;    // 0xAA
  byte throttle;  // 0-255
  byte steering;  // 0-255
  byte trimT;     // 0-255
  byte trimS;     // 0-255
  byte checksum;  
};

ControlData receiverdat;
unsigned long lastPacketTime = 0; 
bool connected = false;

// ערכים סופיים לפקודות (גלובליים כדי שיהיו נגישים לכל הקוד)
int finalThrottle = 127;
int finalSteering = 127;
int escSignal = 1500;      // Ajouté ici pour être global
int steeringSignal = 1500;

void setup() {
  steeringServo.attach(servoPin);
  escMotor.attach(escMotorPin);
  // הגדרת מצבי עבודה למודול LoRa
  pinMode(M0, OUTPUT);
  pinMode(M1, OUTPUT);
  digitalWrite(M0, LOW);
  digitalWrite(M1, LOW);

  Serial.begin(115200);
  loraSerial.begin(9600);
  Serial.println("System Initialized. Waiting for Remote...");
  
  // אתחול זמן ראשוני כדי למנוע כניסה מיידית ל-Failsafe
  lastPacketTime = millis();
}

void loop() {
  // 1. ניקוי באפר במקרה של הצטברות "זבל" אלקטרוני
  if (loraSerial.available() > 30) {
    while (loraSerial.available()) loraSerial.read();
  }

  // 2. קליטת נתונים ואימות
  if (loraSerial.available() >= sizeof(ControlData)) {
    byte potentialHeader = loraSerial.read();
    
    if (potentialHeader == 0xAA) {
      byte* structPtr = (byte*)&receiverdat;
      structPtr[0] = potentialHeader;
      
      loraSerial.readBytes(&structPtr[1], sizeof(ControlData) - 1);

      // אימות מתמטי של החבילה
      byte calculatedChecksum = receiverdat.throttle + receiverdat.steering + 
                                 receiverdat.trimT + receiverdat.trimS;

      if (calculatedChecksum == receiverdat.checksum) {
        // סימון קשר תקין ועדכון זמן
        lastPacketTime = millis();
        
        if (!connected) {
          connected = true;
          Serial.println(">>> LINK ESTABLISHED <<<");
        }

        // א. חישוב ערכים סופיים כולל טרמים (מרכז ב-127)
        finalThrottle = receiverdat.throttle + (receiverdat.trimT - 127);
        finalSteering = receiverdat.steering + (receiverdat.trimS - 127);

        // ב. הגבלת הטווח ל-0-255
        finalThrottle = constrain(finalThrottle, 0, 255);
        finalSteering = constrain(finalSteering, 0, 255);

        // ג. החלת Deadzone (שטח מת) למניעת זמזום במנוחה
        if (finalThrottle > 123 && finalThrottle < 131) finalThrottle = 127;
        if (finalSteering > 123 && finalSteering < 131) finalSteering = 127;

        escSignal = map(finalThrottle, 0, 255, 1000, 2000);
        escMotor.writeMicroseconds(escSignal);

        steeringSignal = map(finalSteering, 0, 255, 1000, 2000);
        steeringServo.writeMicroseconds(steeringSignal);

        // דוגמה להדפסת הנתונים הסופיים
        printStatus();
      }
    }
  }

  // 3. מנגנון Failsafe דינמי (עצירה אם אין קשר מעל 1.5 שניות)
  if (millis() - lastPacketTime > 1500) {
    if (connected) {
      connected = false;
      Serial.println("!!! LINK LOST - FAILSAFE ACTIVATED !!!");
    }
    
    finalThrottle = 127; // עצירה
    finalSteering = 127; // יישור גלגלים
    
    // מניעת הצפה של ה-Serial Monitor
    lastPacketTime = millis() - 1400; 
  }
}

// פונקציית עזר להדפסת נתונים
void printStatus() {
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 300) { // הדפסה פעם ב-200 מילי-שניות בלבד
    // נתונים גולמיים מהשלט (Raw)
    Serial.print("RAW -> Thr: "); Serial.println(receiverdat.throttle);
    Serial.print("Str: "); Serial.println(receiverdat.steering);
    //Serial.print("TrimT: "); Serial.println(receiverdat.trimT);
    //Serial.print("TrimS: "); Serial.println(receiverdat.trimS);
    Serial.println("____________");
    //Serial.print("T_Total: "); Serial.print(finalThrottle);
    //Serial.print(" | S_Total: "); Serial.println(finalSteering);
    //Serial.println("____________");
    Serial.print("ESC_Signal: "); Serial.println(escSignal);
    Serial.print("STERRING_Signal: "); Serial.println(steeringSignal);
    Serial.println("____________");
    lastPrint = millis();
  }
}