/**********************************************************************
  Minimal RF24 Remote Car with Movement, Sonar, and Servo Control
  2025/07/11 - Cleaned and Fixed
**********************************************************************/

#include <RF24.h>
#include <Servo.h>
#include <Arduino.h>
#include <stdint.h>

// === Types ===
typedef unsigned long u32;
typedef uint8_t u8;

// === Pins ===
#define PIN_DIRECTION_LEFT 4
#define PIN_DIRECTION_RIGHT 3
#define PIN_MOTOR_PWM_LEFT 6
#define PIN_MOTOR_PWM_RIGHT 5
#define PIN_SONIC_TRIG 7
#define PIN_SONIC_ECHO 8
#define PIN_SERVO 2

// === NRF24L01 ===
#define PIN_SPI_CE 9
#define PIN_SPI_CSN 10
RF24 radio(PIN_SPI_CE, PIN_SPI_CSN);
const byte address[6] = "Free1";

#define NRF_UPDATE_TIMEOUT 1000
u32 lastNrfUpdateTime = 0;
int nrfDataRead[8];
bool nrfComplete = false;

// === Car Modes ===
#define MODE_REMOTE_CONTROL 7
#define MODE_OBSTACLE_AVOID 3

u8 nrfCarMode = MODE_REMOTE_CONTROL, lastNrfCarMode = MODE_REMOTE_CONTROL;

// === Servo ===
Servo servo;
int servoOffset = 0;

// === Obstacle Avoidance Settings ===
#define SONIC_TIMEOUT 20000
#define OBSTACLE_DISTANCE 25
#define OBSTACLE_DISTANCE_LOW 18
int speedOffset = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("Minimal RF24 Remote Car Starting...");

  pinMode(PIN_DIRECTION_LEFT, OUTPUT);
  pinMode(PIN_DIRECTION_RIGHT, OUTPUT);
  pinMode(PIN_MOTOR_PWM_LEFT, OUTPUT);
  pinMode(PIN_MOTOR_PWM_RIGHT, OUTPUT);
  pinMode(PIN_SONIC_TRIG, OUTPUT);
  pinMode(PIN_SONIC_ECHO, INPUT);

  servo.attach(PIN_SERVO);
  writeServo(90);

  if (!nrf24L01Setup()) {
    Serial.println("NRF24L01 setup failed!");
    while (1);
  }
  Serial.println("NRF24L01 setup OK.");
}

void loop() {
  if (getNrf24L01Data()) {
    clearNrfFlag();
    nrfCarMode = updateNrfCarMode();

    if (nrfCarMode != MODE_REMOTE_CONTROL && nrfCarMode != MODE_OBSTACLE_AVOID) {
      Serial.println("Unknown mode — forcing REMOTE_CONTROL.");
      nrfCarMode = MODE_REMOTE_CONTROL;
    }

    if (nrfCarMode != lastNrfCarMode) {
      Serial.println("Mode changed — stopping motors.");
      motorRun(0, 0);
      lastNrfCarMode = nrfCarMode;
    }

    switch (nrfCarMode) {
      case MODE_REMOTE_CONTROL:
        updateCarActionByNrfRemote();
        break;
      case MODE_OBSTACLE_AVOID:
        updateObstacleAvoidance();
        break;
    }

    lastNrfUpdateTime = millis();
  }

  if (millis() - lastNrfUpdateTime > NRF_UPDATE_TIMEOUT) {
    Serial.println("NRF timeout — stopping motors.");
    motorRun(0, 0);
    resetNrfDataBuf();
    nrfCarMode = lastNrfCarMode = MODE_REMOTE_CONTROL;
    lastNrfUpdateTime = millis();
  }
}

bool nrf24L01Setup() {
  if (radio.begin()) {
    radio.setPALevel(RF24_PA_MAX);
    radio.setDataRate(RF24_1MBPS);
    radio.openReadingPipe(1, address);
    radio.startListening();
    return true;
  }
  return false;
}

bool getNrf24L01Data() {
  if (radio.available()) {
    radio.read(nrfDataRead, sizeof(nrfDataRead));
    nrfComplete = true;
  }
  return nrfComplete;
}


void clearNrfFlag() {
  nrfComplete = false;
}

u8 updateNrfCarMode() {
  return ((nrfDataRead[5] ? 1 : 0) << 2) | ((nrfDataRead[6] ? 1 : 0) << 1) | (nrfDataRead[7] ? 1 : 0);
}

void updateCarActionByNrfRemote() {
  int x = nrfDataRead[2] - 512;
  int y = nrfDataRead[3] - 512;
  const int DEADZONE = 30;

  if (abs(x) < DEADZONE) x = 0;
  if (abs(y) < DEADZONE) y = 0;

  int pwmL = (y - x) / 2;
  int pwmR = (y + x) / 2;

  motorRun(pwmL, pwmR);

  int servoAngle = map(nrfDataRead[1], 0, 1023, 0, 180);
  writeServo(servoAngle);
}

void updateObstacleAvoidance() {
  int distance[3] = {0}, sumDistance = 0;
  static u8 leftToRight = 0;
  static u8 RightToLeft = 0;
  const u8 scanAngle[2][3] = { {150, 90, 30}, {30, 90, 150} };

  for (int i = 0; i < 3; i++) {
    u8 angle = scanAngle[RightToLeft][i];
    writeServo(angle);
    delay(130); // Wait servo to settle

    sumDistance = 0;
    for (int j = 0; j < 5; j++) {
      sumDistance += getSonar();
    }
    int avg = sumDistance / 5;
    distance[RightToLeft == 0 ? i : (2 - i)] = avg;
  }
  RightToLeft = (RightToLeft - 1) % 2;

  if (distance[1] < OBSTACLE_DISTANCE) {
    motorRun(-150, -150);
    delay(100);
    if (distance[0] > distance[2]) {
      motorRun(-150, 150);
    } else {
      motorRun(150, -150);
    }
  } else {
    if (distance[0] < OBSTACLE_DISTANCE_LOW) {
      motorRun(-150, -150);
      delay(100);
      motorRun(180, 50);
    } else if (distance[2] < OBSTACLE_DISTANCE_LOW) {
      motorRun(-150, -150);
      delay(100);
      motorRun(50, 180);
    } else {
      motorRun(80, 80);
    }
  }
}

void motorRun(int speedL, int speedR) {
  bool dirL = speedL >= 0;
  bool dirR = speedR >= 0;

  speedL = constrain(abs(speedL), 0, 255);
  speedR = constrain(abs(speedR), 0, 255);

  digitalWrite(PIN_DIRECTION_LEFT, dirL);
  digitalWrite(PIN_DIRECTION_RIGHT, dirR);

  analogWrite(PIN_MOTOR_PWM_LEFT, speedL);
  analogWrite(PIN_MOTOR_PWM_RIGHT, speedR);
}

void writeServo(int pos) {
  pos = constrain(pos + servoOffset, 0, 180);
  servo.write(pos);
}

float getSonar() {
  digitalWrite(PIN_SONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_SONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_SONIC_TRIG, LOW);

  long duration = pulseIn(PIN_SONIC_ECHO, HIGH, SONIC_TIMEOUT);
  return (duration / 2.0) * 0.0343;
}

void resetNrfDataBuf() {
  for (int i = 0; i < 8; i++)
    nrfDataRead[i] = (i < 2) ? 0 : 512;
}