#define joy_X1_pin A6
#define joy_Y1_pin A7
#define joy_sel1_pin 53
#define joy_X2_pin A8
#define joy_Y2_pin A9
#define joy_sel2_pin 52

int joy_X1_value = 512, joy_Y1_value = 512, joy_sel1_value = 1, joy_X2_value = 512, joy_Y2_value = 512, joy_sel2_value = 1, joy_sel1_state = 1, joy_sel2_state = 1;
int escValue1 = 1500, escValue2 = 1500, escValue3 = 1500, escValue4 = 1500, escValue5 = 1500;
float pressure = 0, temperature = 0, acceleration_x = 0, acceleration_y = 0, acceleration_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
int hours = 0, minutes = 0, seconds = 0;
char timeString[9];

void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);
  Serial.println(" ");
  Serial1.println(" ");
  pinMode(joy_sel1_pin, INPUT_PULLUP);
  pinMode(joy_sel2_pin, INPUT_PULLUP);
}

void loop() {
  joy_X1_value = analogRead(joy_X1_pin);
  joy_Y1_value = analogRead(joy_Y1_pin);
  joy_sel1_value = digitalRead(joy_sel1_pin);
  joy_X2_value = analogRead(joy_X2_pin);
  joy_Y2_value = analogRead(joy_Y2_pin);
  joy_sel2_value = digitalRead(joy_sel2_pin);

  if( (joy_X1_value < 200) || (joy_X1_value > 850) || (joy_X2_value < 200) || (joy_X2_value > 850) || (joy_Y1_value < 200) || (joy_Y1_value > 850) || (joy_Y2_value < 200) || (joy_Y2_value > 850) || (digitalRead(joy_sel1_pin)==0) || (digitalRead(joy_sel2_pin)==0) )
  {
    if(digitalRead(joy_sel1_pin)==0)
    {
      while(digitalRead(joy_sel1_pin)==0);
      joy_sel1_state = !joy_sel1_state;
    }
    if(digitalRead(joy_sel2_pin)==0)
    {
      while(digitalRead(joy_sel2_pin)==0);
      joy_sel2_state = !joy_sel2_state;
    }
    if( (joy_X1_value < 200) || (joy_X1_value > 850) )
    {
      escValue1 = map(joy_X1_value, 0, 1023, 1100, 1900);
      escValue2 = map(joy_X1_value, 0, 1023, 1100, 1900);
      escValue3 = map(joy_X1_value, 0, 1023, 1900, 1100);
      escValue4 = 1500;
      escValue5 = 1500;
    }
    else if( (joy_X2_value < 200) || (joy_X2_value > 850) )
    {
      escValue1 = map(joy_X2_value, 0, 1023, 1100, 1900);
      escValue2 = map(joy_X2_value, 0, 1023, 1100, 1900);
      escValue3 = map(joy_X2_value, 0, 1023, 1100, 1900);
      escValue4 = 1500;
      escValue5 = 1500;
    }
    else if( (joy_Y1_value < 200) || (joy_Y1_value > 850) )
    {
      escValue4 = map(joy_Y1_value, 0, 1023, 1100, 1900);
      escValue1 = 1500;
      escValue2 = 1500;
      escValue3 = 1500;
      escValue5 = 1500;
    }
    else if( (joy_Y2_value < 200) || (joy_Y2_value > 850) )
    {
      escValue5 = map(joy_Y2_value, 0, 1023, 1100, 1900);
      escValue1 = 1500;
      escValue2 = 1500;
      escValue3 = 1500;
      escValue4 = 1500;
    }

    // Convert data to a string to send via RS485
    String data = String("<") + 
                  String(escValue1) + "," + 
                  String(escValue2) + "," + 
                  String(escValue3) + "," + 
                  String(escValue4) + "," + 
                  String(escValue5) + "," + 
                  String(joy_sel1_state) + "," + 
                  String(joy_sel2_state) +
                  String(">");
    // Send data over RS485
    Serial.println(data);
  }
  else
  {
    escValue1 = 1500;
    escValue2 = 1500;
    escValue3 = 1500;
    escValue4 = 1500;
    escValue5 = 1500;
  }

  // Check if data is available to read
  if (Serial1.available()) {
    // Read the data from RS485
    String data = readStringUntilDelimiter(); // Read the string from Arduino2

    if (data.length() > 0) {
      Serial.print("Received data: ");
      Serial.println(data);

      // Process the received data (split and convert if needed)
    // Example of splitting the data (assuming it's in the format "temperature,pressure,acceleration_x,acceleration_y,acceleration_z,gyro_x,gyro_y,gyro_z,hours,minutes,seconds")
    int index = 0;
    String values[11];
    while (index < 11) {
      int commaIndex = data.indexOf(',');
      if (commaIndex == -1) {
        values[index] = data;
        break;
      }
      values[index] = data.substring(0, commaIndex);
      data = data.substring(commaIndex + 1);
      index++;
    }

    // Convert strings to float or int as appropriate
    temperature = values[0].toFloat();
    pressure = values[1].toFloat();
    acceleration_x = values[2].toFloat();
    acceleration_y = values[3].toFloat();
    acceleration_z = values[4].toFloat();
    gyro_x = values[5].toFloat();
    gyro_y = values[6].toFloat();
    gyro_z = values[7].toFloat();
    hours = values[8].toInt();
    minutes = values[9].toInt();
    seconds = values[10].toInt();

    // Print the received values
    Serial.print("Received temperature: ");
    Serial.println(temperature);
    Serial.print("Received pressure: ");
    Serial.println(pressure);
    Serial.print("Received acceleration_x: ");
    Serial.print(acceleration_x);
    Serial.print(", Received acceleration_y: ");
    Serial.print(acceleration_y);
    Serial.print(", Received acceleration_z: ");
    Serial.println(acceleration_z);
    Serial.print("Received gyro_x: ");
    Serial.print(gyro_x);
    Serial.print(", Received gyro_y: ");
    Serial.print(gyro_y);
    Serial.print(", Received gyro_z: ");
    Serial.println(gyro_z);
    sprintf(timeString, "%02d:%02d:%02d", hours, minutes, seconds);
    Serial.print("received time: ");
    Serial.println(timeString);
    Serial.println("*************************");
    }
  }
  delay(50);
}

// Function to read a string until the end delimiter
String readStringUntilDelimiter() {
  String receivedString = "";
  char incomingByte;
  bool startReadingFlag = 0;
  
  while (Serial1.available() > 0) {
    incomingByte = Serial1.read();

    // Check for start delimiter
    if (incomingByte == '<') {
      receivedString = "";
      startReadingFlag = 1;
    }
    else if(startReadingFlag == 1) 
    {
      if (incomingByte == '>') {
        return receivedString;
      }
      else {
        receivedString += incomingByte;
      }
    }
  }
  return ""; // Return empty string if no valid message is found
}