#include <SoftwareSerial.h>

#define RX_PIN 2
#define TX_PIN 3

SoftwareSerial zt2Serial(RX_PIN, TX_PIN); // RX, TX

const int dataLength = 17; // Total number of bytes in the frame
uint8_t buffer[dataLength * 2]; // Circular buffer for incoming data
int head = 0; // Index for writing data to the buffer
int tail = 0; // Index for reading data from the buffer

// Global variables to store specific bytes
uint8_t afr = 0;
uint8_t map_value = 0;
uint8_t user1 = 0;
uint8_t user2 = 0;

// Function to validate the data frame (basic sanity check)
bool isValidFrame(uint8_t* data) {
  // Implement any specific validation rules for your frame here
  // For example, ensure AFR (byte 4) and other values are within expected ranges
  // Return false if the frame is invalid
  if (data[3] > 255 || data[8] > 255 || data[11] > 255 || data[14] > 255) {
    return false; // Example range check, adjust as necessary
  }
  return true;
}

/*void readZeitronix() {
  // Read incoming bytes and store them in the buffer
  while (zt2Serial.available()) {
    buffer[head] = zt2Serial.read();
    head = (head + 1) % (dataLength * 2); // Wrap around the buffer

    // Check if there's a complete frame available
    if ((head + (dataLength * 2) - tail) % (dataLength * 2) >= dataLength) {
      // Check for the start of a new frame: 0 1 2
      if (buffer[tail] == 0 && buffer[(tail + 1) % (dataLength * 2)] == 1 && buffer[(tail + 2) % (dataLength * 2)] == 2) {
        // Read the frame into a temporary array
        uint8_t data[dataLength];
        for (int i = 0; i < dataLength; i++) {
          data[i] = buffer[(tail + i) % (dataLength * 2)];
        }

        // Move the tail to the end of the processed frame
        tail = (tail + dataLength) % (dataLength * 2);

        // Validate the frame
        if (isValidFrame(data)) {
          // Store the specific bytes in global variables
          afr = data[3];     // byte 4 (AFR)
          map_value = data[8]; // byte 9 (MAP)
          user1 = data[11];  // byte 12 (USER1)
          user2 = data[14];  // byte 15 (USER2)
        }
      } else {
        // If the start of the frame is not found, discard the first byte and move the tail
        tail = (tail + 1) % (dataLength * 2);
      }
    }
  }
} */
void readZeitronix() {
  // Read incoming bytes and store them in the buffer
  while (zt2Serial.available()) {
    buffer[head] = zt2Serial.read();
    head = (head + 1) % (dataLength * 2); // Wrap around the buffer

    // Check if there's a complete frame available
    if ((head + (dataLength * 2) - tail) % (dataLength * 2) >= dataLength) {
      // Check for the start of a new frame: 0 1 2
      if (buffer[tail] == 0 && buffer[(tail + 1) % (dataLength * 2)] == 1 && buffer[(tail + 2) % (dataLength * 2)] == 2) {
        // Read the frame into a temporary array
        uint8_t data[dataLength];
        for (int i = 0; i < dataLength; i++) {
          data[i] = buffer[(tail + i) % (dataLength * 2)];
        }

        // Move the tail to the end of the processed frame
        tail = (tail + dataLength) % (dataLength * 2);

        // Validate the frame
        if (isValidFrame(data)) {
          // Print the entire frame for debugging
          Serial.print("Frame: ");
          for (int i = 0; i < dataLength; i++) {
            Serial.print(data[i]);
            Serial.print(" ");
          }
          Serial.println();

          // Store the specific bytes in global variables
          afr = data[3];     // byte 4 (AFR)
          map_value = data[8]; // byte 9 (MAP)
          user1 = data[11];  // byte 12 (USER1)
          user2 = data[14];  // byte 15 (USER2)
        }
      } else {
        // If the start of the frame is not found, discard the first byte and move the tail
        tail = (tail + 1) % (dataLength * 2);
      }
    }
  }
}

void setup() {
  Serial.begin(9600);
  zt2Serial.begin(9600);
}

void loop() {
  readZeitronix();

  // Use the global variables to update the TFT display or for other purposes
  Serial.print("AFR: ");
  Serial.print(afr); // AFR value
  Serial.print(" MAP: ");
  Serial.print(map_value); // MAP value
  Serial.print(" USER1: ");
  Serial.print(user1); // USER1 value
  Serial.print(" USER2: ");
  Serial.println(user2); // USER2 value

  // Update the TFT display here using the values of afr, map_value, user1, user2
  // Example:
  // tft.setCursor(0, 0);
  // tft.print("AFR: " + String(afr));
  // tft.setCursor(0, 10);
  // tft.print("MAP: " + String(map_value));
  // tft.setCursor(0, 20);
  // tft.print("USER1: " + String(user1));
  // tft.setCursor(0, 30);
  // tft.print("USER2: " + String(user2));

  delay(100); // Adjust delay as needed for the desired refresh rate
}