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

#include "obd2_elm327.h"
#include "defines.h"

#define DEBUG_VALUES 1

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

typedef enum elm327_current_value_e
{
    RPM_V, // fast
    // THROTTLE_V,
    COOLANT_TEMP_V,     // slow
    INTAKE_AIR_TEMP_V,  // slow
    AMBIENT_AIR_TEMP_V, // slow
    OIL_TEMP_V,         // slow
    ENGINE_LOAD_V,      // fast
    // BATTERY_VOLTAGE_V,
    FUEL_PRESSURE_V, // fast?
    // SPEED_V,
    MANIFOLD_PRESSURE_V, // fast?
    FUEL_LEVEL_V         // slow

} elm327_current_value;

ELM327 elm327_obj;
elm327_current_value current_value_fast;
elm327_current_value current_value_slow;

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
    current_value_slow = COOLANT_TEMP_V;
    current_value_fast = ENGINE_LOAD_V;

    ELM327_SERIAL.begin(ELM327_BAUD);

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

#if (SKIP_BT_CHECK == 0)
    if (!elm327->elm327->begin(ELM327_SERIAL, false, 2000))
    {
        return 0;
    }
#endif // !SKIP_BT_CHECK

    return 1;
}

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

char obd2_el327_is_bit_set(obd2_elm327_t *elm327, uint16_t pos)
{
    return elm327->value_updates & (1 << pos);
}

void obd2_el327_set_bit(obd2_elm327_t *elm327, uint16_t pos)
{
    elm327->value_updates |= (1 << pos);
}

void obd2_el327_clear_bit(obd2_elm327_t *elm327, uint16_t pos)
{
    elm327->value_updates &= ~(1 << pos);
}

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_fast(obd2_elm327_t *elm327)
{
    switch (current_value_fast)
    {
    case ENGINE_LOAD_V:
    {
#if (DEBUG_VALUES == 1)
        current_value_fast = FUEL_PRESSURE_V;
        elm327->engine_load = (elm327->engine_load + 1) % 100;
        elm327->value_updates |= (1 << UPDATE_ENGINE_LOAD_POS);
#else
        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_fast = FUEL_PRESSURE_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_fast = FUEL_PRESSURE_V;
        }
#endif
        break;
    }
    case FUEL_PRESSURE_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->fuel_pressure = (elm327->fuel_pressure + 1) % 20;
        elm327->value_updates |= (1 << UPDATE_FUEL_PRESSURE_POS);
        current_value_fast = MANIFOLD_PRESSURE_V;
#else
        float fuel_pressure = elm327->elm327->fuelPressure();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->fuel_pressure = (uint16_t)fuel_pressure;
            elm327->value_updates |= (1 << UPDATE_FUEL_PRESSURE_POS);
            current_value_fast = MANIFOLD_PRESSURE_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_fast = MANIFOLD_PRESSURE_V;
        }
#endif
        break;
    }
    case MANIFOLD_PRESSURE_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->manifold_pressure = (elm327->manifold_pressure + 1) % 20;
        elm327->value_updates |= (1 << UPDATE_MANIFOLD_PRESSURE_POS);
        current_value_fast = ENGINE_LOAD_V;
#else
        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_fast = ENGINE_LOAD_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_fast = ENGINE_LOAD_V;
        }
#endif
        break;
    }
    }
}

void obd2_elm327_process_slow(obd2_elm327_t *elm327)
{

    switch (current_value_slow)
    {
    case COOLANT_TEMP_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->engine_coolant_temp = (elm327->engine_coolant_temp + 10) % 150;
        elm327->value_updates |= (1 << UPDATE_COOLANT_TEMP_POS);
        current_value_slow = INTAKE_AIR_TEMP_V;
#else
        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_slow = INTAKE_AIR_TEMP_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_slow = INTAKE_AIR_TEMP_V;
        }
#endif
        break;
    }
    case INTAKE_AIR_TEMP_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->intake_air_temp = (elm327->intake_air_temp + 10) % 60;
        elm327->value_updates |= (1 << UPDATE_INTAKE_AIR_TEMP_POS);
        current_value_slow = AMBIENT_AIR_TEMP_V;
#else
        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_slow = AMBIENT_AIR_TEMP_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_slow = AMBIENT_AIR_TEMP_V;
        }
#endif
        break;
    }
    case AMBIENT_AIR_TEMP_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->ambient_air_temp = (elm327->ambient_air_temp + 10) % 60;
        elm327->value_updates |= (1 << UPDATE_AMBIENT_AIR_TEMP_POS);
        current_value_slow = OIL_TEMP_V;
#else
        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_slow = OIL_TEMP_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_slow = OIL_TEMP_V;
        }
#endif
        break;
    }
    case OIL_TEMP_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->engine_oil_temp = (elm327->engine_oil_temp + 10) % 150;
        elm327->value_updates |= (1 << UPDATE_OIL_TEMP_POS);
        current_value_slow = FUEL_LEVEL_V;
#else
        float engine_oil_temp = elm327->elm327->oilTemp();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->engine_oil_temp = (uint16_t)engine_oil_temp;
            elm327->value_updates |= (1 << UPDATE_OIL_TEMP_POS);
            current_value_slow = FUEL_LEVEL_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_slow = FUEL_LEVEL_V;
        }
#endif
        break;
    }
    case FUEL_LEVEL_V:
    {
#if (DEBUG_VALUES == 1)
        elm327->fuel_level = (elm327->fuel_level + 1) % 100;
        elm327->value_updates |= (1 << UPDATE_FUEL_LEVEL_POS);
        current_value_slow = COOLANT_TEMP_V;
#else
        float fuel_level = elm327->elm327->fuelLevel();
        if (elm327->elm327->nb_rx_state == ELM_SUCCESS)
        {
            elm327->fuel_level = (uint16_t)fuel_level;
            elm327->value_updates |= (1 << UPDATE_FUEL_LEVEL_POS);
            current_value_slow = COOLANT_TEMP_V;
        }
        else if (elm327->elm327->nb_rx_state != ELM_GETTING_MSG)
        {
            // elm327->elm327->printError();
            current_value_slow = COOLANT_TEMP_V;
        }
#endif
        break;
    }
    default:
        break;
    }
}