//
// 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"

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

#define MAX_SENSOR_NUM          32

static uint16_t gpioStore;
static int left_turn_signal, right_turn_signal;
static int flashMainBeamCnt;

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 LRLightTimeout(union sigval sig);
static void ChangeLRLight(int light);
static void ConfirmTurnSigalLater(union sigval sig);
static void flashBeamLightClose(union sigval sig);
static void confirmFlashBeamLightLater(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));

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

    left_turn_signal = right_turn_signal = 0;
    flashMainBeamCnt = 0;

    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->lock != Sensor.lock && s->lock != '#') {
        SensorChanged(SENSOR_DOOR, BX(s->lock));
    }
    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)
{
    pthread_mutex_lock(&status_rw_mutex);
    CarStatus[id] = value;
    pthread_mutex_unlock(&status_rw_mutex);
}

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

    WriteCarStatus(TURN_SIGNAL_LAMP, OFF_LIGHT);
}

static void ChangeLRLight(int light)
{
    WriteCarStatus(TURN_SIGNAL_LAMP, light);

    AppTimer_delete(LRLightTimeout);
    AppTimer_add(LRLightTimeout, 1500);
}

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

    ChangeLRLight(sig.sival_int);
}

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

static void confirmFlashBeamLightLater(union sigval sig)
{
    AppTimer_delete(confirmFlashBeamLightLater);
    flashMainBeamCnt = 0;
}

static void SensorChanged(int id, int value)
{
    DEBUG("状态改变 %d = %d", id, value);

    switch (id) {
        case SENSOR_LEFT_TURN_SIGNAL: {
            left_turn_signal = value;

            if (left_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (right_turn_signal) {
                    // 判定为双闪
                    ChangeLRLight(HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, LEFT_TURN_LIGHT);
                }
            }
            break;
        }
        case SENSOR_RIGHT_TURN_SIGNAL: {
            right_turn_signal = value;

            if (right_turn_signal) {
                AppTimer_delete(ConfirmTurnSigalLater);
                if (left_turn_signal) {
                    // 判定为双闪
                    ChangeLRLight(HAZARD_LIGHTS);
                } else {
                    AppTimer_add(ConfirmTurnSigalLater, 200, RIGHT_TURN_LIGHT);
                }
            }
            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: {
            if (value == 0) {
                WriteCarStatus(MAIN_BEAM_LAMP, OFF_LIGHT);
            } else {
                WriteCarStatus(MAIN_BEAM_LAMP, MAIN_BEAM_LIGHT);
            }

            if (++flashMainBeamCnt > 3) {         // 亮-灭-亮-灭
                WriteCarStatus(FLASH_BEAM_LAMP, FLASH_BEAM_LIGHT);

                AppTimer_delete(flashBeamLightClose);
                AppTimer_add(flashBeamLightClose, D_SEC(2));
            }

            AppTimer_delete(confirmFlashBeamLightLater);
            AppTimer_add(confirmFlashBeamLightLater, D_SEC(3));
            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:
            break;
    }
}

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