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

#include "obd2_elm327.h"

#define KM_IN_MILES 1.6093440
#define RPM_MAX 7000
#define THROTTLE_MAX 100

typedef enum elm327_current_value_e
{
    RPM_V,
    THROTTLE_V,
    COOLANT_TEMP_V,
    INTAKE_AIR_TEMP_V,
    AMBIENT_AIR_TEMP_V,
    OIL_TEMP_V,
    ENGINE_LOAD_V,
    BATTERY_VOLTAGE_V,
    FUEL_PRESSURE_V,
    SPEED_V,
    MANIFOLD_PRESSURE_V

} elm327_current_value;

ELM327 elm327_obj;
elm327_current_value current_value;

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

char obd2_elm327_init(obd2_elm327_t *elm327)
{
#if (DEBUG == 1)
    Serial.println("initting elm327");
#endif
    ELM327_SERIAL.begin(ELM327_BAUD);

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

    if (!elm327->elm327->begin(ELM327_SERIAL, false, 2000))
    {
        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_check_connection(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();
        }
    }
}

void obd2_elm327_process(obd2_elm327_t *elm327)
{

    switch (current_value)
    {
    case RPM_V:
    {
        float rpm = elm327->elm327->rpm();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
          if (rpm > 0 && rpm < RPM_MAX)
            elm327->rpm = (uint16_t)rpm;
            elm327->value_updates |= (1 << UPDATE_RPM_POS);
        }
        current_value = THROTTLE_V;
        break;
    }
    case THROTTLE_V:
    {
        float throttle = elm327->elm327->throttle();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
          if (throttle > 0 && throttle <= THROTTLE_MAX)
          {
            elm327->throttle_percent = (uint8_t)throttle;
            elm327->value_updates |= (1 << UPDATE_THROTTLE_POS);
          }
        }
        // current_value = COOLANT_TEMP_V;
        //TODO remove
        current_value = RPM_V;
        break;
    }
    case COOLANT_TEMP_V:
    {
        float coolant_temp = elm327->elm327->engineCoolantTemp();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->engine_coolant_temp = (uint8_t)coolant_temp;
            elm327->value_updates |= (1 << UPDATE_COOLANT_TEMP_POS);
        }
        current_value = INTAKE_AIR_TEMP_V;
        break;
    }
    case INTAKE_AIR_TEMP_V:
    {
        float intake_air_temp = elm327->elm327->intakeAirTemp();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->intake_air_temp = (uint8_t)intake_air_temp;
            elm327->value_updates |= (1 << UPDATE_INTAKE_AIR_TEMP_POS);
        }
        current_value = AMBIENT_AIR_TEMP_V;
        break;
    }
    case AMBIENT_AIR_TEMP_V:
    {
        float ambient_air_temp = elm327->elm327->ambientAirTemp();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->ambient_air_temp = (uint8_t)ambient_air_temp;
            elm327->value_updates |= (1 << UPDATE_AMBIENT_AIR_TEMP_POS);
        }
        current_value = OIL_TEMP_V;
        break;
    }
    case OIL_TEMP_V:
    {
        float engine_oil_temp = elm327->elm327->oilTemp();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->engine_oil_temp = engine_oil_temp;
            elm327->value_updates |= (1 << UPDATE_OIL_TEMP_POS);
        }
        current_value = ENGINE_LOAD_V;
        break;
    }
    case ENGINE_LOAD_V:
    {
        float engine_load = elm327->elm327->engineLoad();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->engine_load = (uint8_t)engine_load;
            elm327->value_updates |= (1 << UPDATE_ENGINE_LOAD_POS);
        }
        current_value = BATTERY_VOLTAGE_V;
        break;
    }
    case BATTERY_VOLTAGE_V:
    {
        float battery_voltage = elm327->elm327->batteryVoltage();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->battery_voltage = battery_voltage;
            elm327->value_updates |= (1 << UPDATE_BATTERY_VOLTAGE_POS);
        }
        current_value = FUEL_PRESSURE_V;
        break;
    }
    case FUEL_PRESSURE_V:
    {
        float fuel_pressure = elm327->elm327->fuelPressure();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->fuel_pressure = fuel_pressure;
            elm327->value_updates |= (1 << UPDATE_FUEL_PRESSURE_POS);
        }
        current_value = SPEED_V;
        break;
    }
    case SPEED_V:
    {
        float speed = elm327->elm327->mph();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->speed = (uint8_t)(speed * KM_IN_MILES);
            elm327->value_updates |= (1 << UPDATE_SPEED_POS);
        }
        current_value = MANIFOLD_PRESSURE_V;
        break;
    }
    case MANIFOLD_PRESSURE_V:
    {
        float manifold_pressure = elm327->elm327->manifoldPressure();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->manifold_pressure = (uint8_t)manifold_pressure;
            elm327->value_updates |= (1 << UPDATE_MANIFOLD_PRESSURE_POS);
        }
        current_value = RPM_V;
        break;
    }
    default:
        break;
    }
}