/*
 * egts.c
 *
 *  Created on: Jun 4, 2021
 *      Author: zemon
 */


#include "egts.h"
#include "egts_crc.h"
#include "string.h"
#include "stdint.h"
#include "math.h"
#include "egtsWorker.h"

#define ADD_TO_RESULT(DATA, LEN) memcpy(out + offset, (uint8_t * ) & DATA, LEN); offset+=LEN;


//flags
const uint8_t flags = 0b10000001;
//const uint16_t SPD = 50;
const uint8_t DIR = 0;
const uint32_t ODR = 0x000000;
const uint8_t DIN = 0;
const uint8_t SRC = 16;
const uint32_t ALT = 0x00;

//uint16_t vEgtsPackPosDataGen(uint8_t *out, void *args) {
//    tEgtsPosDataArgs *posDataArgs = (tEgtsPosDataArgs *) args;
//    return vEgtsPackPosData(out, posDataArgs);
//}

uint16_t vEgtsPackPosData(uint8_t *out, tEgtsPosDataArgs *args) {

    uint32_t NTM = toEgtsTimestamp(args->epochTimestamp);

    double lat = (fabs(args->latitude) / 90) * 0xFFFFFFFF;
    double lon = (fabs(args->longitude) / 180) * 0xFFFFFFFF;
    uint32_t LAT = (uint32_t) lat;
    uint32_t LONG = (uint32_t) lon;

    uint16_t offset = 0;


    ADD_TO_RESULT(NTM, 4);
    ADD_TO_RESULT(LAT, 4);
    ADD_TO_RESULT(LONG, 4);
    ADD_TO_RESULT(flags, 1);
    ADD_TO_RESULT(args->velocity, 2);
    ADD_TO_RESULT(DIR, 1);
    ADD_TO_RESULT(ODR, 3);
    ADD_TO_RESULT(DIN, 1);
    ADD_TO_RESULT(SRC, 1);
    ADD_TO_RESULT(ALT, 3);

    return offset;

}

uint16_t vEgtsPackSensorsData(uint8_t *out, tEgtsSensorsDataArgs *args) {

    uint16_t offset = 0;

    ADD_TO_RESULT(args->digitalInPresent, 1);
    ADD_TO_RESULT(args->digitalOutPresent, 1);
    ADD_TO_RESULT(args->analogInPresent, 1);

    for (uint16_t i = 0; i < 8; ++i) {
        uint8_t mask = 0x1 << i;
        if (args->digitalInPresent & mask) {
            ADD_TO_RESULT(args->digitalIn[i], 1);
        }
    }

    for (uint16_t i = 0; i < 8; ++i) {
        uint8_t mask = 0x1 << i;
        if (args->analogInPresent & mask) {
            ADD_TO_RESULT(args->analogIn[i], 3);
        }
    }
    return offset;

}

//uint32_t TID = 4168809385;
uint8_t TermIdentityFlags = 0b01000010;
//unit16_t TermIdentityHDID = ;
//unit32_t TermIdentityNID = ;
uint16_t TermIdentityBS = 512;


//uint16_t vEgtsPackTermIdentityGen(uint8_t *out, void *args) {
//    tEgtsTermIdentityArgs *termIdentityArgs = (tEgtsTermIdentityArgs *) args;
//    return vEgtsPackTermIdentity(out, termIdentityArgs);
//}

uint16_t vEgtsPackTermIdentity(uint8_t *out, tEgtsTermIdentityArgs *args) {

    uint16_t offset = 0;

    ADD_TO_RESULT(args->TerminalID, 4);
    ADD_TO_RESULT(TermIdentityFlags, 1);
    ADD_TO_RESULT((*args->IMEI), args->IMEI_len);
    ADD_TO_RESULT(TermIdentityBS, 2);

    return offset;

}

uint16_t vEgtsPackResponse(uint8_t *out, tEgtsRecordResponseData *args) {

    uint16_t offset = 0;

    ADD_TO_RESULT(args->CRN, 2);
    ADD_TO_RESULT(args->RST, 1);

    return offset;

}

uint16_t vEgtsPacSrCommandDataResponse(uint8_t *out, tEgtsSrCmdConfirmation *args) {

    uint16_t offset = 0;

    uint8_t flag = (args->CT << 0x4) | (args->CCT & 0xF);
    ADD_TO_RESULT(flag, 1);

    ADD_TO_RESULT(args->CID, 4);
    ADD_TO_RESULT(args->SID, 4);

    uint8_t hasFlags = (args->ACFE << 0x1) | (args->CHSFE & 0x1);
    ADD_TO_RESULT(hasFlags, 1);

    if (args->CHSFE) {
        ADD_TO_RESULT(args->CHS, 1);
    }
    if (args->ACFE) {
        ADD_TO_RESULT(args->ACL, 1);
        ADD_TO_RESULT(args->AC, 4);
    }

    if ((args->CT == CT_COMCONF) && (args->CCT == CC_OK)) {
        ADD_TO_RESULT(args->CmdData.ADR, 2);
        uint8_t flagComData = (args->CmdData.SZ << 0x4) | (args->CmdData.ACT & 0xF);
        ADD_TO_RESULT(flagComData, 1);
        ADD_TO_RESULT(args->CmdData.CCD, 2);
    }
    if (args->CmdData.DT) {
        //todo?
    }

    return offset;
}

uint16_t vEgtsPackTeledataTransport(
        uint16_t packetId,
        uint8_t *out,
        double latitude,
        double longitude,
        uint16_t velocity,
        uint32_t epochTimestamp
) {
    tEgtsPosDataArgs args = {
            .longitude = longitude,
            .latitude = latitude,
            .velocity = velocity,
            .epochTimestamp = epochTimestamp,
    };

    return vEgtsPackTransport(
            0,
            packetId,
            out,
            EGTS_SERVICE_FLAGS_POS,
            EGTS_PT_APPDATA,
            EGTS_TELEDATA_SERVICE,
            EGTS_TELEDATA_SERVICE,
            epochTimestamp,
            EGTS_SR_POS_DATA,
            vEgtsPackPosDataGen,
            &args
    );

//    return vEgtsPackPosData(out, &args);
}


uint16_t vEgtsPackSensorsTransport(
        uint16_t packetId,
        uint8_t *out,
        uint32_t epochTimestamp,
        tEgtsSensorsDataArgs *sensors
) {
    return vEgtsPackTransport(
            0,
            packetId,
            out,
            EGTS_SERVICE_FLAGS_POS,
            EGTS_PT_APPDATA,
            EGTS_TELEDATA_SERVICE,
            EGTS_TELEDATA_SERVICE,
            epochTimestamp,
            EGTS_SR_AD_SENSORS_DATA,
            vEgtsPackSensorsDataGen,
            sensors
    );

//    return vEgtsPackPosData(out, &args);
}

uint16_t vEgtsPackTermIdentityTransport(
        uint16_t packetId,
        uint8_t *out,
        uint8_t *IMEI,
        uint8_t IMEI_len,
        uint32_t TerminalID,
        uint32_t epochTimestamp
) {
    tEgtsTermIdentityArgs args = {
            .IMEI = IMEI,
            .IMEI_len = IMEI_len,
            .TerminalID = TerminalID,
//            .epochTimestamp = epochTimestamp,
    };

    return vEgtsPackTransport(
            0,
            packetId,
            out,
            EGTS_SERVICE_FLAGS_AUTH,
            EGTS_PT_APPDATA,
            EGTS_AUTH_SERVICE,
            EGTS_AUTH_SERVICE,
            epochTimestamp,
            EGTS_SR_TERM_IDENTITY,
            vEgtsPackTermIdentityGen,
            &args
    );

//    return vEgtsPackPosData(out, &args);
}

uint16_t
vEgtsPackEgtsResponse(uint16_t packetId, uint8_t *out, uint8_t rst, uint16_t crn, uint16_t cid, uint32_t timestamp) {

    tEgtsRecordResponseData args = {
            .CRN = crn,
            .RST = rst,
    };

    return vEgtsPackTransport(
            cid,
            packetId,
            out,
            EGTS_SERVICE_FLAGS_POS,
            EGTS_PT_RESPONSE,
            EGTS_ECALL_SERVICE,
            EGTS_ECALL_SERVICE,
            timestamp,
            EGTS_SR_RECORD_RESPONSE,
            vEgtsPackResponseGen,
            &args
    );
}

uint16_t vEgtsPackSrCmdConfirmation(
        uint8_t *out,
        uint16_t packetId,
        uint16_t cmdType,
        uint16_t cmdConfirType,
        uint16_t cid,
        uint16_t sid,
        uint16_t acfe,
        uint16_t chsfe,
        uint16_t adr,
        uint8_t sz,
        uint8_t act,
        uint16_t ccd,
        uint32_t dt
) {

    bool authCodeExist = false;
    bool charsetExist = false;

    tEgtsSrCmdConfirmation args = {
            .CT = cmdType,
            .CCT = cmdConfirType,
            .CID = cid,
            .SID = sid,
            .ACFE = acfe,
            .CHSFE = chsfe,
            .CmdData.ADR = adr,
            .CmdData.SZ = sz,
            .CmdData.ACT = act,
            .CmdData.CCD = ccd,
            .CmdData.DT = dt,
    };

    return vEgtsPackTransport(
            0,
            packetId,
            out,
            EGTS_SERVICE_FLAGS_POS,
            EGTS_PT_APPDATA,
            EGTS_COMMANDS_SERVICE,
            EGTS_COMMANDS_SERVICE,
            0,
            EGTS_SR_COMMAND_DATA,
            vEgtsPacSrCommandDataResponseGen,
            &args
    );
}