#include <esp_now.h>
#include <WiFi.h>

// Motor driver pins
#define IN1 8   // Left motor forward
#define IN2 7   // Left motor backward
#define IN3 10  // Right motor forward
#define IN4 9   // Right motor backward
#define WEAPON_MOTOR 6  // MOSFET SIG control

const int threshold = 30;

typedef struct {
  int x;
  int y;
  bool weapon_on;
} JoystickData;

JoystickData incomingData;

void stopMotors() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
}

void driveMotors(int x, int y) {
  int dx = x - 512;
  int dy = y - 512;

  if (abs(dx) < threshold && abs(dy) < threshold) {
    stopMotors();
    return;
  }

  if (abs(dy) > abs(dx)) {
    if (dy > threshold) {  // Forward
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
    } else if (dy < -threshold) {  // Backward
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
      digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
    }
  } else {
    if (dx > threshold) {  // Turn right
      digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
      digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
    } else if (dx < -threshold) {  // Turn left
      digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
      digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
    }
  }
}

void controlWeapon(bool on) {
  digitalWrite(WEAPON_MOTOR, on ? HIGH : LOW);
}

void onReceiveData(const esp_now_recv_info_t *info, const uint8_t *data, int len) {
  if (len == sizeof(JoystickData)) {
    memcpy(&incomingData, data, sizeof(JoystickData));
    driveMotors(incomingData.x, incomingData.y);
    controlWeapon(incomingData.weapon_on);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  pinMode(WEAPON_MOTOR, OUTPUT);

  stopMotors();
  digitalWrite(WEAPON_MOTOR, LOW);  // Start weapon OFF

  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW init failed!");
    return;
  }

  esp_now_register_recv_cb(onReceiveData);
  Serial.println("✅ Receiver Ready");
}

void loop() {
  // Handled by ESP-NOW callback
}