#include <Arduino.h>

#include "DS2.h"

boolean compareString(char a[], char b[]);
void printRps(float rps);
void printMessage(uint8_t array[], uint8_t length);
void printData(uint8_t command[]);

// We keep data there, 255 is reccomended for full compatibility, you can use void setMaxDataLength(uint8_t dataLength) if bugs happen
uint8_t data[255];

// Format for commands is always same, see DS3.h for more info
uint8_t ecuId[] = {0x12, 0x04, 0x00, 0x16};
uint8_t generalValues[] = {0x12, 0x05, 0x0B, 0x03, 0x1F};

// We store our ECU list and byte offset for our battery voltage
struct Ecu
{
  char *ecuName;
  uint8_t batteryOffset;
};

uint8_t batteryOffset;

#define ECUS_NUMBER 9

const struct Ecu ecuList[ECUS_NUMBER] PROGMEM = {
    {"default", 16},
    {"1429764", 20},
    {"1430844", 20},
    {"7526753", 20},
    {"7500255", 20},
    {"7511570", 22},
    {"7519308", 22},
    {"7545150", 22},
    {"7551615", 22},
};

// Simple function to match ECU to what we read
uint8_t matchEcu(char *id)
{
  Ecu ecu;
  for (uint8_t i = 0; i < ECUS_NUMBER; i++)
  {
    memcpy_P(&ecu, &ecuList[i], sizeof ecu);
    if (compareString(id, ecu.ecuName))
    {
      return ecu.batteryOffset;
    }
  }
  memcpy_P(&ecu, &ecuList[0], sizeof ecu);
  return ecu.batteryOffset;
}

boolean compareString(char a[], char b[])
{
  boolean match = true;
  for (uint8_t i = 0; i < 255; i++)
  {
    if (a[i] == 0 || b[i] == 0)
      break;
    if (a[i] != b[i])
    {
      match = false;
      break;
    }
  }
  return match;
}

DS2 DS3(Serial2);

void setup()
{
  Serial.begin(9600);
  // Serial2.begin(9600, SERIAL_8E1);
  Serial2.setTimeout(ISO_TIMEOUT);

  // You can set blocking if you want to test how it works.
  // DS3.setBlocking(true);

  // Loading and displaying ECU ID

  char loading[][3] = {"|", "/", "-", "\\"};
  Serial.println("CONNECTING");
  for (uint8_t i = 0; !DS3.obtainValues(ecuId, data); i++)
  {
    if (i > 3)
    {
      i = 0;
    }

    Serial.print(loading[i]);
  }

  // Displaying Ecu ID
  char ecuIdString[8];
  DS3.getString(data, ecuIdString, 0, 7);
  Serial.print(ecuIdString);

  // Matching battery voltage offset depending on our ECU
  batteryOffset = matchEcu(ecuIdString);
}

// Loop variables
uint32_t startTime;
float fps, lowestFps = 0, highestFps = 0;
boolean print = false;

void loop(void)
{
  startTime = micros();

  // You can compare performance using two types of commands

  // Send command
  if (DS3.sendCommand(generalValues) != 0) // do stuff if data sent

    /**
     * You can put some code in between while waiting for data for higher performance
     **/
    if (print)
      printData(generalValues);

  // Receive command
  if ((DS3.receiveData(data)) == RECEIVE_OK)
    print = true; // do stuff if data received

  // Blocked .obtainValues - generally slower but easier to use and always laids some response
  //	if(DS3.obtainValues(generalValues, data)) print = true;
}

// Prints data, gets batteryVoltage from data, you need to specify which command was sent
void printData(uint8_t command[])
{
  float batteryVoltage = 0.1 * DS3.getByte(data, batteryOffset);
  Serial.print(batteryVoltage);
  Serial.println(" V  ");

  //	Takes a lot of performance due to how .print is managed. Those screens doesn't like reprints

  /*
  printMessage(command, DS3.getEcho());
  tft.setCursor(0,20);
  printMessage(data, DS3.getResponseLength());
  */

  printRps(DS3.getRespondsPerSecond());
  print = false;
}

// Very slow way of printing message, good for debugging though
void printMessage(uint8_t array[], uint8_t length)
{
  for (uint8_t i = 0; i < 255; i++)
  {
    if (i < length)
      Serial.print((array[i]), HEX);
    Serial.print(" ");
  }
}

// Prints reesponses per second
void printRps(float rps)
{

  Serial.print(rps);
  Serial.println(" rps  ");
}

// Prints Fps on the screen with low, high and current. If touch pressed min/max fps are resetted