//
// Created by zemon on 11.02.2022.
//

#include "egts.h"
//#include "BitOps.h"
#include "BitBuffer.h"

uint16_t vEgtsPackTrackData(uint8_t *out, uint16_t bitsOffset, tEgtsTrackData *trackData) {

    bitsOffset += vBitBufferAppendByte(out, bitsOffset, trackData->LOHS ? 0b1 : 0b0, 1);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, trackData->LAHS ? 0b1 : 0b0, 1);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->speed)[1], 5);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, trackData->TrackDataExist ? 0b1 : 0b0, 1);


    bitsOffset += vBitBufferAppendBitsReverseBytes(out, bitsOffset, (uint8_t *) &trackData->Latitude, 32);
    bitsOffset += vBitBufferAppendBitsReverseBytes(out, bitsOffset, (uint8_t *) &trackData->Longitude, 32);

    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->speed)[0], 8);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->Altitude)[0], 8);


    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->Direction)[1], 1);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, trackData->AltitudeDirection ? 0b1 : 0b0, 1);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->Altitude)[1], 6);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &trackData->Direction)[0], 8);

}


uint16_t vEgtsPackSrEpTrackData(uint8_t *out, tEgtsSrEpTrackDataArgs *args) {

    uint16_t bitsOffset = 0;

    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &args->blockNumber)[0], 8);

    bitsOffset += vBitBufferAppendBitsReverseBytes(out, bitsOffset, (uint8_t *) &args->absoluteTime, 32);

    bitsOffset += vBitBufferAppendByte(out, bitsOffset, args->relativeAmount, 8);

    bitsOffset += vBitBufferAppendByte(out, bitsOffset, args->timeShift, 4);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, args->CoordinateSystem ? 0b1 : 0b0, 1);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, ((uint8_t *) &args->blockNumber)[1], 2);
    bitsOffset += vBitBufferAppendByte(out, bitsOffset, args->RTU ? 0b1 : 0b0, 1);


    bitsOffset += vEgtsPackTrackData(out, bitsOffset, &args->originPoint);


    return (bitsOffset / 8) + ((bitsOffset % 8) ? 1 : 0) + args->relativeAmount;
}