#include "FirmwareLoader.h"
#include "EgtsProcessing.h"
#include "ext_telematica.h"
#include "ld_adr.h"
#include "math.h"
#include "SystemDelayInterface.h"
#include "Accel_QMA6100P.h"
#include "EgtsTeledataPoint.h"
#include "VariablesTable.h"

#define FIRMWARE_MAIN_ADDR (0x08000000 + BOOT_AREA_LENGTH)
#define FIRMWARE_MAIN_RECOVERY_ADDR (FIRMWARE_MAIN_ADDR + FIRMWARE_MAIN_AREA_LENGTH)

#define FIRMWARE_TELE_MAIN_ADDR (FIRMWARE_MAIN_RECOVERY_ADDR + FIRMWARE_MAIN_UPDATE_AREA_LENGTH)
#define FIRMWARE_TELE_RECOVERY_ADDR (FIRMWARE_MAIN_ADDR + FIRMWARE_MAIN_AREA_LENGTH)

//tFirmwareLoader FIRMWARE_UVEOS_LOADER;
tFirmwareLoader FIRMWARE_TELE_LOADER;
tFirmwareLoader FIRMWARE_LOADER;

// Переполнение стека
void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) {
    for (;;) {

    }
}

tMmaTele EXT_ENV_TELE;
tEgtsProcessing egtsProcessing;
tGsmWithGnss gsmWithGnss;
tLittleFileFs fs;
eDeviceTestingCode deviceTestingCodeCan;
/*
vector3 vecOld;
vector3 vec;
vector3 vecDiff;

float vecTotal;
float vecTotalOld;
float koefAcc;
*/

/*
void DataAccel_Vector(vector3 *current) {

}
*/

#define LOG_SIGN "EGTS"
#define LOGGER &egtsProcessing.slog->logger

bool cal;

#define ACCEL_TRESHOLD 20


//double maxA;

void DataAccel_Vector(tAccelCoor *xyz) {
/*
    if (egtsProcessing.timeAccelUpdateUds < SystemGetMs()) {

        egtsProcessing.timeAccelUpdateUds = SystemGetMs() + 500;
        //начало---------------------------------Рассчет для UDS сканера----------------------------------------------------
        vector3 gravity = (vector3) {0.f, 0.f, 1.f};
        vector3 currentValue = (vector3) {(float) xyz->x / 1000000, (float) xyz->y / 1000000, (float) xyz->z / 1000000};

        egtsProcessing.rawAccel.x = (int16_t) (currentValue.x * 1333);
        egtsProcessing.rawAccel.y = (int16_t) (currentValue.y * 1333);
        egtsProcessing.rawAccel.z = (int16_t) (currentValue.z * 1333);

        float angleCos = vector3AngleCos(gravity, currentValue);;
        float angle = acosf(angleCos);
        egtsProcessing.rawAccel.angle = 180.0 / 3.141592 * angle;

        //конец---------------------------------Рассчет для UDS сканера-----------------------------------------------------
    }
*/
    if (egtsProcessing.timeAccelUpdate < SystemGetMs()) {

        egtsProcessing.timeAccelUpdate = SystemGetMs() + 100;

        xyz->x = xyz->x / 1000;
        xyz->y = xyz->y / 1000;
        xyz->z = xyz->z / 1000;

        egtsProcessing.x1 = xyz->x;
        egtsProcessing.y1 = xyz->y;
        egtsProcessing.z1 = xyz->z;

        if (cal) {
            egtsProcessing.x2 = egtsProcessing.x1;
            egtsProcessing.y2 = egtsProcessing.y1;
            egtsProcessing.z2 = egtsProcessing.z1;
            cal = false;
            return;
        }

        double vect_vert_len = (egtsProcessing.x1 * egtsProcessing.x2 + egtsProcessing.y1 * egtsProcessing.y2 +
                                egtsProcessing.z1 * egtsProcessing.z2) /
                               (sqrt(egtsProcessing.x2 * egtsProcessing.x2 + egtsProcessing.y2 * egtsProcessing.y2 +
                                     egtsProcessing.z2 * egtsProcessing.z2) + 0.001);

        double a = sqrt(fabs((egtsProcessing.x1 * egtsProcessing.x1 + egtsProcessing.y1 * egtsProcessing.y1 +
                              egtsProcessing.z1 * egtsProcessing.z1) -
                             (vect_vert_len * vect_vert_len)));


        egtsProcessing.count_shot = (a > ACCEL_TRESHOLD) ? egtsProcessing.count_shot + 1 : 0;

//        if (a > maxA) {
//            maxA = a;
//        }

        ++egtsProcessing.count_free;

        if (egtsProcessing.count_shot == 0) {
            if (egtsProcessing.count_free >= 100) {
                // OFF
                if (egtsProcessing.carEventPosition.carPosition == CAR_POSITION_MOVE) {
                    if (egtsProcessing.totalSpeed <
                        egtsProcessing.deviceTeledataStorageData->telematica.EGTS_GNSS_COURSE_SPEED) {

                        egtsProcessing.carEventPosition.carPosition = CAR_POSITION_FIX;
                        egtsProcessing.count_free = 0;

                    } else {
                        egtsProcessing.count_free = 90;
                    }
                } else {
                    egtsProcessing.count_free = 0;
                }
            }
        }

        if (egtsProcessing.count_shot > 3) {

//            egtsProcessing.x1 = egtsProcessing.x2;
//            egtsProcessing.y1 = egtsProcessing.y2;
//            egtsProcessing.z1 = egtsProcessing.z2;
            egtsProcessing.x2 = egtsProcessing.x1;
            egtsProcessing.y2 = egtsProcessing.y1;
            egtsProcessing.z2 = egtsProcessing.z1;

            egtsProcessing.count_free = 0;

            //ON
            egtsProcessing.carEventPosition.carPosition = CAR_POSITION_MOVE;

        }

    }

}

volatile uint32_t valueCounter1;
volatile uint32_t valueCounter2;
volatile uint32_t valueCounter3;

void main_init() {
//    maxA = 0;

    valueCounter1 = 0;
    valueCounter2 = 0;
    valueCounter3 = 0;

    cal = true;
    memset(&egtsProcessing, 0, sizeof(tEgtsProcessing));
    memset(&gsmWithGnss, 0, sizeof(tGsmWithGnss));
    memset(&fs, 0, sizeof(tLittleFileFs));

    deviceTestingCodeCan = DEVICE_TESTING_CODE_IN_PROCESS;
    EXT_ENV_TELE.deviceTestingCodeCan = &deviceTestingCodeCan;

//    FirmwareLoader_Init(&FIRMWARE_UVEOS_LOADER, FIRMWARE_MAIN_AREA_LENGTH, FIRMWARE_MAIN_ADDR,
//                        FIRMWARE_MAIN_RECOVERY_ADDR);

    FirmwareLoader_Init(&FIRMWARE_TELE_LOADER, FIRMWARE_TELE_AREA_LENGTH, FIRMWARE_TELE_MAIN_ADDR,
                        FIRMWARE_TELE_RECOVERY_ADDR);

    FirmwareLoader_LoadMetadata(&FIRMWARE_TELE_LOADER, &FIRMWARE_TELE_LOADER.main);

    FirmwareLoader_Init(&FIRMWARE_LOADER, FIRMWARE_MAIN_AREA_LENGTH, FIRMWARE_MAIN_ADDR,
                        FIRMWARE_MAIN_RECOVERY_ADDR);

    FirmwareLoader_LoadMetadata(&FIRMWARE_LOADER, &FIRMWARE_LOADER.main);

//    LittleFileFsInit(&fs, EXT_ENV_TELE.deviceStorage->dumpObserver.accessDumper);
    LittleFileFsInit(&fs);

    GsmWithGnssInit(&gsmWithGnss);

    EgtsProcessing_Init(
            &egtsProcessing,
            &gsmWithGnss,
            EXT_ENV_TELE.rtcIO,
            &fs,
            EXT_ENV_TELE.loggerToSerialPort
    );

}

void *tele_func(void *env, eEventTelematica event);

int main(void) {
    XfcProtMethodsInit_Certs(&egtsProcessing.certs);
    EgtsProcessing_Start(&egtsProcessing);
}

void GsmWithGnssTelematica_Urc(tAtBuffer *buff);

void ComIntTelematica(tXfcProtTable *protTab);

void ComIntTelematicaDefault();

void GsmWithGnssTelematica_Urc(tAtBuffer *buff);


void setNullParm() {
    EXT_ENV_TELE.SrandStdLib = NULL;
    EXT_ENV_TELE.RandStdLib = NULL;
    EXT_ENV_TELE.DataToCan = NULL;
}

void *tele_func(void *env, eEventTelematica event) {
//    uint16_t sent = 0;
//    //            uint32_t tm = SystemGetMs();
//    if (event == TELE_MODULE_GNSS) {
//        if (!EXT_ENV_TELE.store.device->factoryMode) {
//            //начало-----------------------------------ТИШИНА-------------------------------------------------------
//
//                CanSerialPortFrameSetId1(egtsProcessing.CanMain.CanIO->env, 0x640);
//
//                if (EXT_ENV_TELE.GNSS_General->GNSS_LC > 15) {
//                    EXT_ENV_TELE.GNSS_General->GNSS_LC = 0;
//                } else {
//                    ++EXT_ENV_TELE.GNSS_General->GNSS_LC;
//                }
//
//                sent = egtsProcessing.CanMain.CanIO->transmit1(egtsProcessing.CanMain.CanIO->env, (uint8_t *) EXT_ENV_TELE.GNSS_General, 2,
//                                            WAIT_FRAME_WRITE);
//            sent = sent;
//            //конец-----------------------------------ТИШИНА-------------------------------------------------------
//
//    }
//}

    if (event == TELE_MODULE_COUNTER_1) {
        ++valueCounter1;
    }

    if (event == TELE_MODULE_COUNTER_2) {
        ++valueCounter2;
    }

    if (event == TELE_MODULE_COUNTER_3) {
        ++valueCounter3;
    }

    if (event == TELE_MODULE_INIT) {

        bool isGeneralToCan = VariablesTable_CheckUDS_ByNameStatic(EXT_ENV_TELE.store.publicVariablesTable, "GNSS_GENERAL_TO_CAN");

        if (isGeneralToCan == false) {
            setNullParm();
        }

        main_init();
    }

    if (event == TELE_MODULE_PRE_INIT) {
        asm("bl Reset_Handler");
    }

    if (event == TELE_MODULE_START) {
        main();
    }

    if (event == TELE_MODULE_COM_INT) {
        ComIntTelematica(env);
    }

    if (event == TELE_MODULE_COM_INT_DEFAULT) {
        ComIntTelematicaDefault();
    }

    if (event == TELE_MODULE_GSM_URC) {
        GsmWithGnssTelematica_Urc(env);
    }

    if (event == TELE_MODULE_ACCEL_DATA) {
        DataAccel_Vector(env);
    }

    if (event == TELE_MODULE_ADD_EVENT) {
        addEventFromUveos(&egtsProcessing, env);
    }

    if (event == TELE_MODULE_CAN_UDS_START) {
        CanMain_Start(&egtsProcessing.CanMain);
        CanMainAdditional_Start(&egtsProcessing.CanMainAdditional);
        CanMainTest_Start(&egtsProcessing.CanMainTest);
        CanMainMute_Start(&egtsProcessing.CanMainMute);
    }

    if (event == TELE_MODULE_SHUTDOWN) {
        /** Проверка флага обновления */
        if(egtsProcessing.isTimerUpdate == true){
            /** проверка таймера обновления*/
            if(egtsProcessing.timerUpdate > SystemGetMs()){
                return &egtsProcessing.timerUpdate;
            }
        }
        egtsProcessing.timerUpdate = 0;

        EXT_ENV_TELE.store.runtime->EGTS_FLEET_ON = false;
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Зафиксировано событие: TELE_MODULE_SHUTDOWN")

        LoggerInfoStatic(LOGGER, LOG_SIGN, "Ожидание закрытия соединения")

        ///* FIX
        for (uint8_t i = 0; i < 30; i++) {
            if (EXT_ENV_TELE.store.runtime->telematicaCloseConnect == true) {
                break;
            }
            SystemDelayMs(100);

        }
//        uint32_t tm = SystemGetMs() + 3000;
//        uint32_t timer = SystemGetMs();
//
//        while ((tm > timer) && (EXT_ENV_TELE.store.runtime->telematicaCloseConnect == false)) {
//            timer = SystemGetMs();
//            SystemDelayMs(100);
//        }

        if (EXT_ENV_TELE.store.runtime->telematicaCloseConnect == true) {
            LoggerInfoStatic(LOGGER, LOG_SIGN, "Соединение успешно закрыто")
        } else {
            LoggerInfoStatic(LOGGER, LOG_SIGN, "Соединение не было закрыто")
        }


        return &egtsProcessing.timerUpdate;
    }

    if (event == TELE_MODULE_MUTE_ON) {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Зафиксировано событие: TELE_MODULE_MUTE_ON")
    }

    if (event == TELE_MODULE_MUTE_OFF) {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Зафиксировано событие: TELE_MODULE_MUTE_OFF")
    }

    if (event == TELE_MODULE_FUEL_CUT_OFF) {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Зафиксировано событие: TELE_MODULE_FUEL_CUT_OFF")
    }

    if (event == TELE_MODULE_GET_CAN_COM_IO) {
        return &egtsProcessing.serialPortCanComIntIO;
    }

    return 0;
}