#include "ELMduino.h"
#include <string.h>


#include "obd2_elm327.h"

#define KM_IN_MILES 1.6093440

ELM327 elm327_obj;

char bt_states[2][BT_STATE_LENGTH] = {"Initializing", "Connected   "};

char obd2_elm327_init(obd2_elm327_t *elm327)
{
    ELM327_SERIAL.begin(ELM327_BAUD);

    elm327->bt_state = BT_INITIALISING;
    elm327->elm327 = &elm327_obj;

    if (!elm327_obj.begin(ELM327_SERIAL, false, 5000))
    {
      return 0;
    }
    return 1;
}

void obd2_elm327_get_state(obd2_elm327_t *elm327, char *state)
{
    strcpy(state, bt_states[elm327->bt_state]);
}

void obd2_elm327_process(obd2_elm327_t *elm327)
{
    if (elm327->bt_state != BT_CONNECTED)
    {
        __UINT8_TYPE__ bt_state = elm327->elm327->connected;

        if (bt_state != elm327->bt_state)
        {
          // 0 for initialising, 1 for connected
            elm327->bt_state = bt_state == 1 ? BT_CONNECTED : BT_INITIALISING; 
            elm327->on_state_change();
        }
    }

    /* TODO maybe change to switch statement */

    float rpm = elm327->elm327->rpm();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->rpm = (uint16_t)rpm;
    }

    float throttle = elm327->elm327->throttle();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->throttle_percent = (uint8_t)throttle;
    }

    float coolant_temp = elm327->elm327->engineCoolantTemp();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->engine_coolant_temp = (uint8_t)coolant_temp;
    }

    float intake_air_temp = elm327->elm327->intakeAirTemp();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->intake_air_temp = (uint8_t)intake_air_temp;
    }

    float ambient_air_temp = elm327->elm327->ambientAirTemp();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->ambient_air_temp = (uint8_t) ambient_air_temp;
    }

    float engine_oil_temp = elm327->elm327->oilTemp();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->engine_oil_temp = engine_oil_temp;
    }

    float engine_load = elm327->elm327->engineLoad();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->engine_load = (uint8_t)engine_load;
    }

    float battery_voltage = elm327->elm327->batteryVoltage();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->battery_voltage = battery_voltage;
    }

    float fuel_pressure = elm327->elm327->fuelPressure();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->fuel_pressure = fuel_pressure;
    }

    float speed = elm327->elm327->mph();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->speed = (uint8_t)(speed * KM_IN_MILES);
    }

    float manifold_pressure = elm327->elm327->manifoldPressure();
    if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
    {
      elm327->manifold_pressure = (uint8_t)manifold_pressure;
    }
}