//
// Created by fctom on 2020/2/13.
//

#include <pthread.h>
#include "car_sensor.h"
#include "../driver_test.h"
#include "../defs.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../rtk_platform/platform.h"

#define DEBUG(fmt, args...)     LOGD("<car_sensor> <%s>: " fmt, __func__, ##args)

#define MAX_SENSOR_NUM          32

static uint16_t gpioStore;
static bool left_turn_signal, right_turn_signal;

enum {
    SENSOR_SEATBELT,
    SENSOR_LEFT_TURN_SIGNAL,
    SENSOR_RIGHT_TURN_SIGNAL,
    SENSOR_HANDBREAK,
    SENSOR_BREAK,
    SENSOR_DIPPED_BEAM_LIGHT,
    SENSOR_MAIN_BEAM_LIGHT,
    SENSOR_DOOR,
    SENSOR_SHIFT_N,
    SENSOR_SHIFT_1,
    SENSOR_SHIFT_2,
    SENSOR_SHIFT_3,
    SENSOR_SHIFT_4,
    SENSOR_SHIFT_5,
    SENSOR_SHIFT_R,
    SENSOR_ENGINE_START,
    SENSOR_BREAK2,
    SENSOR_SURROUND_CAR_1,
    SENSOR_SURROUND_CAR_2,
    SENSOR_SURROUND_CAR_3,
    SENSOR_SURROUND_CAR_4,
    SENSOR_FOG_LIGHT,
    SENSOR_CLEARANCE_LIGHT,
    SENSOR_ENGINE_RPM,
    SENSOR_SPEED
};



static int CarStatus[CAR_STATUS_END];

static struct sensor_cfg {
    int gpioId;
    int funId;
    int validLvl;
} SensorConfig[MAX_SENSOR_NUM];
static int SensorNum = 0;

static car_sensor_t Sensor;
static pthread_mutex_t sonser_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t status_rw_mutex = PTHREAD_MUTEX_INITIALIZER;

inline static int BX(int value);
static void WriteCarStatus(uint16_t id, int value);

static void ConfirmTurnSigalLater(union sigval sig);
static void flashBeamLightClose(union sigval sig);

static void SensorChanged(int id, int value);

void CarSensorInit(void)
{
    gpioStore = 0;
    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    memset(CarStatus, 0, sizeof(CarStatus));
    CarStatus[DOOR] = DOOR_CLOSE;
    CarStatus[GEAR] = GEAR_N;

    memset(&Sensor, 0, sizeof(Sensor));

    left_turn_signal = right_turn_signal = false;

    pthread_mutex_init(&sonser_mutex, NULL);
    pthread_mutex_init(&status_rw_mutex, NULL);
}

void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
//    DEBUG("加入传感器配置 sensorNum %d", sensorNum);
//
//    pthread_mutex_lock(&sonser_mutex);
//
//    SensorNum = sensorNum;
//    for (int i = 0; i < sensorNum; ++i) {
//        DEBUG("    配置<%d>: id %d - fun %d - lvl %d", i, sensor[i][0], sensor[i][1], sensor[i][2]);
//
//        SensorConfig[i].gpioId = sensor[i][0];
//        SensorConfig[i].funId = sensor[i][1];
//        if (sensor[i][2] > 0) {
//            SensorConfig[i].validLvl = 1;
//        } else {
//            SensorConfig[i].validLvl = 0;
//        }
//
//        int level = 0;
//
//        if (gpioStore & BV(SensorConfig[i].gpioId)) {
//            level = 1;
//        }
//
//        if (level == SensorConfig[i].validLvl) {
//            SensorChanged(SensorConfig[i].funId, 1);
//        } else {
//            SensorChanged(SensorConfig[i].funId, 0);
//        }
//    }
//
//    pthread_mutex_unlock(&sonser_mutex);
}

void UpdateSensor(uint16_t gpio, uint16_t speed, uint16_t rpm)
{
    uint16_t chg = 0;

    pthread_mutex_lock(&sonser_mutex);
    chg = gpioStore^gpio;
    gpioStore = gpio;
    pthread_mutex_unlock(&sonser_mutex);

    WriteCarStatus(OBD_SPEED, speed);
    WriteCarStatus(ENGINE_RPM, rpm);

    for (int i = 0; i < SensorNum; ++i) {
        if (chg & BV(SensorConfig[i].gpioId)) {
            int level = 0;

            if (gpio & BV(SensorConfig[i].gpioId)) {
                level = 1;
            }

            if (level == SensorConfig[i].validLvl) {
                SensorChanged(SensorConfig[i].funId, 1);
            } else {
                SensorChanged(SensorConfig[i].funId, 0);
            }
        }
    }
}

void UpdateSensor(const car_sensor_t *s)
{
    WriteCarStatus(OBD_SPEED, s->speed);
    WriteCarStatus(ENGINE_RPM, s->engine);

    if (s->leftTurnLamp != Sensor.leftTurnLamp && s->leftTurnLamp != '#') {
        SensorChanged(SENSOR_LEFT_TURN_SIGNAL, BX(s->leftTurnLamp));
    }
    if (s->rightTurnLamp != Sensor.rightTurnLamp && s->rightTurnLamp != '#') {
        SensorChanged(SENSOR_RIGHT_TURN_SIGNAL, BX(s->rightTurnLamp));
    }
    if (s->fogLamp != Sensor.fogLamp && s->fogLamp != '#') {
        SensorChanged(SENSOR_FOG_LIGHT, BX(s->fogLamp));
    }
    if (s->clearanceLamp != Sensor.clearanceLamp && s->clearanceLamp != '#') {
        SensorChanged(SENSOR_CLEARANCE_LIGHT, BX(s->clearanceLamp));
    }
    if (s->dippedBeamLamp != Sensor.dippedBeamLamp && s->dippedBeamLamp != '#') {
        SensorChanged(SENSOR_DIPPED_BEAM_LIGHT, BX(s->dippedBeamLamp));
    }
    if (s->mainBeamLamp != Sensor.mainBeamLamp && s->mainBeamLamp != '#') {
        SensorChanged(SENSOR_MAIN_BEAM_LIGHT, BX(s->mainBeamLamp));
    }
    if (s->seatBelt != Sensor.seatBelt && s->seatBelt != '#') {
        SensorChanged(SENSOR_SEATBELT, BX(s->seatBelt));
    }
    if (s->key != Sensor.key && s->key != '#') {
        SensorChanged(SENSOR_ENGINE_START, s->key == 3? 1 : 0);
    }
    if (s->mainBreak != Sensor.mainBreak && s->mainBreak != '#') {
        SensorChanged(SENSOR_BREAK, BX(s->mainBreak));
    }
    if (s->assBreak != Sensor.assBreak && s->assBreak != '#') {
        SensorChanged(SENSOR_BREAK2, BX(s->assBreak));
    }
    if (s->handBreak != Sensor.handBreak && s->handBreak != '#') {
        SensorChanged(SENSOR_HANDBREAK, BX(s->handBreak));
    }
    if (s->door != Sensor.door && s->door != '#') {
        SensorChanged(SENSOR_DOOR, BX(s->door));
    }
    if (s->surround1 != Sensor.surround1 && s->surround1 != '#') {
        SensorChanged(SENSOR_SURROUND_CAR_1, BX(s->surround1));
    }
    if (s->surround2 != Sensor.surround2 && s->surround2 != '#') {
        SensorChanged(SENSOR_SURROUND_CAR_2, BX(s->surround2));
    }
    if (s->surround3 != Sensor.surround3 && s->surround3 != '#') {
        SensorChanged(SENSOR_SURROUND_CAR_3, BX(s->surround3));
    }
    if (s->surround4 != Sensor.surround4 && s->surround4 != '#') {
        SensorChanged(SENSOR_SURROUND_CAR_4, BX(s->surround4));
    }
    if (s->gear != Sensor.gear && s->gear != '#') {
//        SensorChanged(GEAR, GEAR_N + s->gear);
        DEBUG("状态改变 挡位 %d", s->gear);
        WriteCarStatus(GEAR, s->gear + GEAR_N);
    }

    Sensor = *s;
}

int ReadCarStatus(uint16_t id)
{
    int value;

    pthread_mutex_lock(&status_rw_mutex);
    value = CarStatus[id];
    pthread_mutex_unlock(&status_rw_mutex);

    return value;
}

static void WriteCarStatus(uint16_t id, int value)
{
    int old_value;
    pthread_mutex_lock(&status_rw_mutex);
    old_value = CarStatus[id];
    CarStatus[id] = value;
    pthread_mutex_unlock(&status_rw_mutex);

    if (id != OBD_SPEED && id != ENGINE_RPM && old_value != value) {
        uint8_t buffer[6];

        buffer[0] = HI_UINT16(id);
        buffer[1] = LO_UINT16(id);
        buffer[2] = BREAK_UINT32(value, 3);
        buffer[3] = BREAK_UINT32(value, 2);
        buffer[4] = BREAK_UINT32(value, 1);
        buffer[5] = BREAK_UINT32(value, 0);

        PlatformStatusChanged(SENSOR_CHANGE_EVT, buffer, 6);
    }
}

static void ConfirmTurnSigalLater(union sigval sig)
{
    AppTimer_delete(ConfirmTurnSigalLater);

    DEBUG("确认转向灯 左 %d 右 %d", left_turn_signal, right_turn_signal);

    if (!left_turn_signal && !right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
    } else if (left_turn_signal && right_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, HAZARD_LIGHTS);
    } else if (left_turn_signal) {
        WriteCarStatus(TURN_SIGNAL_LAMP, LEFT_TURN_LIGHT);
    } else {
        WriteCarStatus(TURN_SIGNAL_LAMP, RIGHT_TURN_LIGHT);
    }
}

static void flashBeamLightClose(union sigval sig)
{
    WriteCarStatus(FLASH_BEAM_LAMP, OFF_LIGHT);
}

static void SensorChanged(int id, int value)
{
    DEBUG("状态改变 %d = %d", id, value);
    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = (bool) (value > 0);

            AppTimer_delete(ConfirmTurnSigalLater);

            AppTimer_add(ConfirmTurnSigalLater, left_turn_signal? 100 : 1100);
            break;
        }
        case SENSOR_RIGHT_TURN_SIGNAL: {
            right_turn_signal = (bool) (value > 0);

            AppTimer_delete(ConfirmTurnSigalLater);

            AppTimer_add(ConfirmTurnSigalLater, right_turn_signal? 100 : 1100);
            break;
        }
        case SENSOR_FOG_LIGHT: {
            if (value == 0) {
                WriteCarStatus(FOG_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(FOG_LAMP, FOG_LIGHT);
            }
            break;
        }
        case SENSOR_CLEARANCE_LIGHT: {
            if (value == 0) {
                WriteCarStatus(CLEARANCE_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(CLEARANCE_LAMP, CLEARANCE_LIGHT);
            }
            break;
        }
        case SENSOR_DIPPED_BEAM_LIGHT: {
            if (value == 0) {
                WriteCarStatus(DIPPED_BEAM_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(DIPPED_BEAM_LAMP, DIPPED_BEAM_LIGHT);
            }
            break;
        }
        case SENSOR_MAIN_BEAM_LIGHT: {
            static uint32_t t1 = 0;

            if (value == 0) {
                WriteCarStatus(MAIN_BEAM_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(MAIN_BEAM_LAMP, MAIN_BEAM_LIGHT);
            }

            if (value != 0) {
                t1 = AppTimer_GetTickCount();
            } else if (AppTimer_GetTickCount() - t1 < D_SEC(3)) {
                // 3秒内完成亮灭，闪灯
                if (ReadCarStatus(FLASH_BEAM_LAMP) != FLASH_BEAM_LIGHT) {
                    WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);
                }
                AppTimer_delete(flashBeamLightClose);
                AppTimer_add(flashBeamLightClose, D_SEC(2));
            }
            break;
        }
        case SENSOR_SEATBELT: {
            if (value == 0) {
                WriteCarStatus(SEATBELT, EJECT_SEATBELT);
            } else {
                WriteCarStatus(SEATBELT, INSERT_SEATBELT);
            }
            break;
        }
        case SENSOR_ENGINE_START: {
            if (value == 0) {
                WriteCarStatus(ENGINE_START, ENGINE_START_INACTIVE);
            } else {
                WriteCarStatus(ENGINE_START, ENGINE_START_ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK: {
            if (value == 0) {
                WriteCarStatus(BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_BREAK2: {
            if (value == 0) {
                WriteCarStatus(SECOND_BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(SECOND_BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_HANDBREAK: {
            if (value == 0) {
                WriteCarStatus(HAND_BREAK, BREAK_INACTIVE);
            } else {
                WriteCarStatus(HAND_BREAK, BREAK_ACTIVE);
            }
            break;
        }
        case SENSOR_DOOR: {
            if (value == 0) {
                WriteCarStatus(DOOR, DOOR_CLOSE);
            } else {
                WriteCarStatus(DOOR, DOOR_OPEN);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_1: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_1, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_2: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_2, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_3: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_3, SURROUND_CAR_ACTIVE);
            }
            break;
        }
        case SENSOR_SURROUND_CAR_4: {
            if (value == 0) {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_INACTIVE);
            } else {
                WriteCarStatus(SURROUND_CAR_4, SURROUND_CAR_ACTIVE);
            }
            break;
        }
//        case SENSOR_SHIFT_N:
//        case SENSOR_SHIFT_1:
//        case SENSOR_SHIFT_2:
//        case SENSOR_SHIFT_3:
//        case SENSOR_SHIFT_4:
//        case SENSOR_SHIFT_5:
//        case SENSOR_SHIFT_R: {
//            if (value != 0) {
//                WriteCarStatus(GEAR, id - SENSOR_SHIFT_N + GEAR_N);
//            }
//            break;
//        }
        default:
            return;
    }
}

inline static int BX(int value)
{
    return (value == 1) ? 1 : 0;
}