#include <SoftwareSerial.h>

// Pin definitions
#define STATUS_LED 4
#define ALERT_LED 5
#define RX_PIN 2  // Explicitly define RX pin for software serial
#define TX_PIN 3  // Explicitly define TX pin for software serial

// Create software serial object
SoftwareSerial vehicleSerial(RX_PIN, TX_PIN);

// Define vehicle data structure
struct VehicleData {
  int vehicleId;
  float speed;
  float latitude;
  float longitude;
  int alertType;
  bool isActive;
  unsigned long lastUpdate;
};

VehicleData myData;
VehicleData receivedData;
bool isVehicle1 = true; // Toggle this for each vehicle

// Timing variables
unsigned long lastTransmitTime = 0;
unsigned long lastDisplayTime = 0;
const unsigned long TRANSMIT_INTERVAL = 1000;  // Increased to 1 second for more stable communication
const unsigned long DISPLAY_INTERVAL = 2000;
const unsigned long VEHICLE_TIMEOUT = 3000;

void setup() {
  // Initialize both hardware and software serial
  Serial.begin(9600);
  vehicleSerial.begin(9600);
  
  // Initialize pins
  pinMode(STATUS_LED, OUTPUT);
  pinMode(ALERT_LED, OUTPUT);
  
  // Initialize my vehicle data
  myData.vehicleId = isVehicle1 ? 1 : 2;
  myData.speed = 60.0;  // Starting speed
  myData.latitude = isVehicle1 ? 37.7749 : 37.7751;  // Different starting positions
  myData.longitude = isVehicle1 ? -122.4194 : -122.4192;
  myData.alertType = 0;
  myData.isActive = true;
  
  // Clear any initial garbage data
  while(vehicleSerial.available()) vehicleSerial.read();
  
  Serial.print("Vehicle ");
  Serial.print(myData.vehicleId);
  Serial.println(" initialized");
  delay(100);  // Short delay for stability
}

void loop() {
  // Handle serial communication
  if (vehicleSerial.available()) {
    processIncomingData();
  }
  
  // Regular updates
  unsigned long currentTime = millis();
  
  // Transmit data
  if (currentTime - lastTransmitTime >= TRANSMIT_INTERVAL) {
    updateAndTransmitData();
    lastTransmitTime = currentTime;
  }
  
  // Display status
  if (currentTime - lastDisplayTime >= DISPLAY_INTERVAL) {
    displayStatus();
    lastDisplayTime = currentTime;
  }
  
  // Check vehicle timeout
  checkVehicleTimeout();
  
  // Update indicators
  updateIndicators();
}

void updateAndTransmitData() {
  // Update vehicle data
  myData.speed += random(-5, 6);
  myData.speed = constrain(myData.speed, 0, 120);
  
  // Small position changes
  myData.latitude += random(-10, 11) / 100000.0;
  myData.longitude += random(-10, 11) / 100000.0;
  
  // Random alerts (5% chance)
  if (random(100) < 5) {
    myData.alertType = random(1, 4);
  } else {
    myData.alertType = 0;
  }
  
  // Send data via software serial
  vehicleSerial.print("V2V,");
  vehicleSerial.print(myData.vehicleId);
  vehicleSerial.print(",");
  vehicleSerial.print(myData.speed);
  vehicleSerial.print(",");
  vehicleSerial.print(myData.latitude, 6);
  vehicleSerial.print(",");
  vehicleSerial.print(myData.longitude, 6);
  vehicleSerial.print(",");
  vehicleSerial.println(myData.alertType);
  
  // Also print to hardware serial for debugging
  Serial.print("Transmitted: V2V,");
  Serial.print(myData.vehicleId);
  Serial.print(",");
  Serial.print(myData.speed);
  Serial.print(",");
  Serial.print(myData.latitude, 6);
  Serial.print(",");
  Serial.print(myData.longitude, 6);
  Serial.print(",");
  Serial.println(myData.alertType);
}

void processIncomingData() {
  String data = vehicleSerial.readStringUntil('\n');
  
  Serial.print("Received: "); // Debug print
  Serial.println(data);
  
  if (data.startsWith("V2V,")) {
    // Split the data
    int values[5];  // To store parsed values
    int valueIndex = 0;
    int startIndex = 4;  // Start after "V2V,"
    
    // Parse the comma-separated values
    while (valueIndex < 5 && startIndex < data.length()) {
      int nextComma = data.indexOf(',', startIndex);
      if (nextComma == -1) nextComma = data.length();
      
      String value = data.substring(startIndex, nextComma);
      if (valueIndex == 0) {  // VehicleId
        values[valueIndex] = value.toInt();
      }
      startIndex = nextComma + 1;
      valueIndex++;
    }
    
    // Only process if it's from the other vehicle
    if (values[0] != myData.vehicleId) {
      receivedData.vehicleId = values[0];
      receivedData.speed = data.substring(data.indexOf(',', 4) + 1).toFloat();
      receivedData.latitude = data.substring(data.indexOf(',', data.indexOf(',', 4) + 1) + 1).toFloat();
      receivedData.longitude = data.substring(data.indexOf(',', data.indexOf(',', data.indexOf(',', 4) + 1) + 1) + 1).toFloat();
      receivedData.alertType = data.substring(data.lastIndexOf(',') + 1).toInt();
      receivedData.isActive = true;
      receivedData.lastUpdate = millis();
      
      Serial.println("Vehicle data updated successfully!");
    }
  }
}

void checkVehicleTimeout() {
  if (receivedData.isActive && (millis() - receivedData.lastUpdate > VEHICLE_TIMEOUT)) {
    receivedData.isActive = false;
    digitalWrite(STATUS_LED, LOW);
    digitalWrite(ALERT_LED, LOW);
    Serial.println("Vehicle connection timed out");
  }
}

void updateIndicators() {
  if (receivedData.isActive) {
    // Calculate distance
    float distance = calculateDistance(
      myData.latitude, myData.longitude,
      receivedData.latitude, receivedData.longitude
    );
    
    // Update status LED based on proximity
    digitalWrite(STATUS_LED, distance < 0.001 ? HIGH : LOW);
    
    // Update alert LED based on received alerts
    digitalWrite(ALERT_LED, receivedData.alertType > 0 ? HIGH : LOW);
  }
}

void displayStatus() {
  Serial.println("\n=== Vehicle Status ===");
  Serial.print("My Vehicle (ID: ");
  Serial.print(myData.vehicleId);
  Serial.println(")");
  Serial.print("Speed: ");
  Serial.print(myData.speed, 1);
  Serial.println(" km/h");
  
  if (receivedData.isActive) {
    Serial.print("\nNearby Vehicle (ID: ");
    Serial.print(receivedData.vehicleId);
    Serial.println(")");
    Serial.print("Speed: ");
    Serial.print(receivedData.speed, 1);
    Serial.println(" km/h");
    
    float distance = calculateDistance(
      myData.latitude, myData.longitude,
      receivedData.latitude, receivedData.longitude
    );
    Serial.print("Distance: ");
    Serial.print(distance * 1000, 0);
    Serial.println(" meters");
    
    if (receivedData.alertType > 0) {
      Serial.print("ALERT: ");
      switch (receivedData.alertType) {
        case 1: Serial.println("Emergency Brake!"); break;
        case 2: Serial.println("Accident Ahead!"); break;
        case 3: Serial.println("Heavy Traffic!"); break;
      }
    }
  } else {
    Serial.println("\nSearching for nearby vehicles...");
  }
  Serial.println("=====================");
}

float calculateDistance(float lat1, float lon1, float lat2, float lon2) {
  return sqrt(pow(lat2 - lat1, 2) + pow(lon2 - lon1, 2));
}