// ESP32 Front (Child) Code
// Reads the right brake temperature and suspension heights and sends the data to the parent ESP32.

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

// ESP-NOW
uint8_t parentAddress[] = {0xE4, 0x65, 0xB8, 0xDA, 0x10, 0x50};

// Brake temperature sensor
#define D6T_addr 0x0A
#define D6T_cmd 0x4C
byte rbuf[19];
float temp_R[9]; // amb + 8 temp_readings for Right brake

// Suspension height sensor
unsigned short lenth_val_L = 0;
unsigned short lenth_val_R = 0;
unsigned char i2c_rx_buf[16];

// Struct for sending sensor data
typedef struct sensor_data {
  int device_id;
  float brake_temp_R;
  int susp_height_L;
  int susp_height_R;
} sensor_data;

sensor_data myData;
bool dataRequestFlag = false; // Flag to indicate data request
bool startDataCollection = false; // Flag to start data collection

// Visual Feedback LED
const int ledPin = 2; // Set to the correct pin for your ESP32

// Callback function for when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

// Callback function for when data is received
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  Serial.print("Received data request from Parent\n");

  // Visual feedback
  digitalWrite(ledPin, HIGH);
  delay(100);
  digitalWrite(ledPin, LOW);

  // Set flags to start data collection and indicate data request
  startDataCollection = true;
  dataRequestFlag = true;
}

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

  // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);

  // Init ESP-NOW
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  // Register peer
  esp_now_peer_info_t peerInfo;
  memcpy(peerInfo.peer_addr, parentAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  // Add peer
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }

  // Register for a callback function that will be called when data is received
  esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv));
  esp_now_register_send_cb(OnDataSent);

  myData.device_id = 0; // 0 for front child

  pinMode(ledPin, OUTPUT); // Initialize LED pin
}

void loop() {
  if (startDataCollection) {
    // Continuously collect data
    readBrakeTemp(false); // Right brake only
    myData.susp_height_L = ReadDistance(0x50); // Assuming left sensor is at 0x50
    myData.susp_height_R = ReadDistance(0x52); // Assuming right sensor is at 0x52

    // Use the average of the 8 temperature readings for the right brake
    float avg_temp_R = 0;
    for (int i = 1; i < 9; i++) {
      avg_temp_R += temp_R[i];
    }
    myData.brake_temp_R = avg_temp_R / 8;

    // If data request flag is set, send data to parent ESP32
    if (dataRequestFlag) {
      // Serialize data into bytes
      char dataToSend[100];
      snprintf(dataToSend, sizeof(dataToSend), "0,%.1f,%d,%d,%.1f,%.1f", myData.brake_temp_R, myData.susp_height_L, myData.susp_height_R, 0.0, 0.0);
      esp_err_t result = esp_now_send(parentAddress, (uint8_t *) dataToSend, strlen(dataToSend));
      if (result == ESP_OK) {
        Serial.println("Sent with success");
      } else {
        Serial.println("Error sending the data");
      }
      dataRequestFlag = false; // Reset the flag
    }

    delay(250); // Collect data 4 times per second
  }
}

void readBrakeTemp(bool isLeft) {
  Wire.beginTransmission(D6T_addr);
  Wire.write(D6T_cmd);
  Wire.endTransmission();
  Wire.requestFrom(D6T_addr, 19);
  delay(100);

  for (int i = 0; i < 19; i++) {
    rbuf[i] = Wire.read();
  }

  // Read only the right brake temperatures
  if (!isLeft) {
    for (int i = 0; i < 9; i++) {
      temp_R[i] = (rbuf[i * 2] + (rbuf[i * 2 + 1] << 8)) * 0.1;
    }

    if (temp_R[0] > 0) {
      Serial.print("Right Brake Temperatures: ");
      for (int i = 0; i < 9; i++) {
        Serial.print(temp_R[i]);
        Serial.print(i < 8 ? ", " : "\n");
      }
    }
  }
}

void SensorRead(unsigned char addr, unsigned char* datbuf, unsigned char cnt) {
  Wire.beginTransmission(addr);
  Wire.write(byte(0x00));
  Wire.endTransmission();

  delay(1);

  Wire.requestFrom(addr, cnt);
  if (cnt <= Wire.available()) {
    *datbuf++ = Wire.read();
    *datbuf++ = Wire.read();
  }
}

int ReadDistance(int sensorAddr) {
  SensorRead(sensorAddr, i2c_rx_buf, 2);
  lenth_val_L = i2c_rx_buf[0];
  lenth_val_L = lenth_val_L << 8;
  lenth_val_L |= i2c_rx_buf[1];
  delay(100);
  return lenth_val_L;
}