#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>
#include <math.h>

Servo Servo; 

RF24 radio(7, 8); // CE, CSN

const byte addresses[][6] = {"00001", "00002"};

int accelerator;
int steering;
int highBeam;
int mode;
int headlamp;
int fogLamp;
int flashlamp;
int hazardLamp = 0;;

byte receivedText[10];
byte reply[3];

const float R1 = 30000.0;
const float R2 = 7500.0;
const float referenceVoltage = 5.0;
const float nominalResistance = 10000.0; // Updated to 10k for thermistor

float voltage;
int temperature;

void setup() {
  pinMode(A1, OUTPUT); 
  pinMode(A5, OUTPUT);
  Servo.attach(9);
  pinMode(2, INPUT);
  Serial.begin(9600);
  radio.begin();
  radio.openWritingPipe(addresses[1]); // Transmit to 00002
  radio.openReadingPipe(1, addresses[0]); // Listen on 00001
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening(); // Start in listening mode
}

void loop() {
  // Voltage Measurement
  voltage = analogRead(A0);
  voltage = (voltage * referenceVoltage) / 1023.0;
  voltage = voltage * ((R1 + R2) / R2);

  // Thermistor Temperature Measurement
  float tSensor = analogRead(A7);
  float tVoltage = (tSensor * referenceVoltage) / 1023.0;
  float resistance = nominalResistance * ((referenceVoltage / tVoltage) - 1.0);
  float tKelvin = 1.0 / (1.0 / 300 + (log(resistance / nominalResistance) / 3950.0));
  temperature = tKelvin - 273.15;

  

  // NRF24 Communication
  if (radio.available()) {
    radio.read(&receivedText, sizeof(receivedText));
    accelerator = receivedText[0];
    steering = receivedText[1];
    mode = receivedText[2];
    headlamp = receivedText[4];
    highBeam = receivedText[5];
    fogLamp = receivedText[6];
    flashlamp = receivedText[7];

    if(mode == 0){
      digitalWrite(A2, LOW);
      digitalWrite(A3, HIGH);
      analogWrite(5, accelerator);
    }
    if(mode == 1){
      digitalWrite(A2, HIGH);
      digitalWrite(A3, LOW);
      analogWrite(5, accelerator);
    }
    Serial.println(accelerator);
    Servo.write(steering);
    //Serial.println(steering);

    int k = analogRead(6);
    //Serial.println(k);
    if(headlamp == 0){
      if(k < 100){
        if(highBeam == 0){
        digitalWrite(A1, HIGH);
      }if(highBeam == 1){
        digitalWrite(A1, LOW);
      }}

      if(k > 140){
        digitalWrite(A1, LOW);
      }
    }

    if(headlamp == 1){
      digitalWrite(A1, LOW);
    }
    
    if(headlamp == 2){
      digitalWrite(A1, HIGH);
    }
    if(highBeam == 1){
      digitalWrite(A5, HIGH);
      digitalWrite(A1, LOW);
    }
    if(highBeam == 0){
      digitalWrite(A5, LOW);
    }

    if(fogLamp == 1){
      digitalWrite(A4, HIGH);
    }
    if(fogLamp == 0){
      digitalWrite(A4, LOW);
    }
    if(flashlamp == 1){
      digitalWrite(2, HIGH);
    }
    if(flashlamp == 2){
      digitalWrite(4, HIGH);
    }
    if(flashlamp == 0){
      if(hazardLamp == 0){
      digitalWrite(2, LOW);
      digitalWrite(4, LOW);
    }}



    radio.stopListening();

    voltage = voltage*10;

    reply[0] = voltage; // Example scaling for transmission
    reply[1] = temperature; // Example scaling for transmission

    bool success = radio.write(&reply, sizeof(reply));
    radio.startListening();
  }
  else{
    //Servo.write(90);
    //digitalWrite(A2, LOW);
   // analogWrite(3, 0);
  }
}