#include "ELMduino.h"


#define ELM_PORT Serial2


ELM327 myELM327;

typedef enum 
{
  ENG_COOLANT_TEMP,
  ENG_FUEL_PRESSURE,
  ENG_MANIFOLD_PRESSURE,
  RPM
} obd2_state;

float rpm = 0;
float cool_temp = 0;
uint8_t man_pres = 0;
float fuel_pres = 0;

obd2_state current_state = ENG_COOLANT_TEMP;

void setup()
{
#if LED_BUILTIN
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
#endif

  Serial.begin(115200);
  ELM_PORT.begin(115200);

  
  // int bt_state = digitalRead(11);
  // while (bt_state == 0)
  // {
  //   bt_state = digitalRead(11);
  //   Serial.println(bt_state);
  //   Serial.println("bt not connected");
  //   delay(500);
  // }
  // Serial.println("bt connected");

  Serial.println("Attempting to connect to ELM327...");

  if (!myELM327.begin(ELM_PORT, false, 2000))
  {
    Serial.println("Couldn't connect to OBD scanner");
    while (1);
  }
  Serial.println(myELM327.connected);

  Serial.println("Connected to ELM327");
}


void loop()
{

  switch (current_state)
  {
    case ENG_COOLANT_TEMP:
    {
      cool_temp = myELM327.engineCoolantTemp();

      if (myELM327.nb_rx_state == ELM_SUCCESS)
      {
        Serial.print("coolant temp: ");
        Serial.println(cool_temp);
        current_state = ENG_FUEL_PRESSURE;
      }
      else if (myELM327.nb_rx_state != ELM_GETTING_MSG)
      {
        myELM327.printError();
        //current_state = ENG_FUEL_PRESSURE;
      }
      break;
    }
    case ENG_FUEL_PRESSURE:
    {
      fuel_pres = myELM327.fuelPressure();

      if (myELM327.nb_rx_state == ELM_SUCCESS)
      {
        Serial.print("fuel pressure: ");
        Serial.println(fuel_pres);
        current_state = ENG_MANIFOLD_PRESSURE;
      }
      else if (myELM327.nb_rx_state != ELM_GETTING_MSG)
      {
        myELM327.printError();
        //current_state = ENG_MANIFOLD_PRESSURE;
      }
      break;
    }
    case ENG_MANIFOLD_PRESSURE:
        {
      man_pres = myELM327.manifoldPressure();

      if (myELM327.nb_rx_state == ELM_SUCCESS)
      {
        Serial.print("manifold pressure: ");
        Serial.println(man_pres);
        current_state = RPM;
      }
      else if (myELM327.nb_rx_state != ELM_GETTING_MSG)
      {
        myELM327.printError();
        //current_state = RPM;
      }
      break;
    }
    case RPM:
    {
      rpm = myELM327.rpm();

      if (myELM327.nb_rx_state == ELM_SUCCESS)
      {
        Serial.print("rpm: ");
        Serial.println(rpm);
        current_state = ENG_COOLANT_TEMP;
      }
      else if (myELM327.nb_rx_state != ELM_GETTING_MSG)
      {
        myELM327.printError();
        //current_state = ENG_COOLANT_TEMP;
      }
      break;
    }
  }


}