/*
 * msd.c
 *
 *  Created on: OCT 21, 2020
 *      Author: FICOM-IT LTD
 */

#include "EraGlonassMsd.h"
#include "BitBuffer.h"
#include <memory.h>


void EraGlonassMsdInit(tEraGlonassMsd *env) {

//    env->lastPos = (tEraGlonassMsd_GpsPos) {512, 512};

    //init not changeable fields
    env->MSD_Data.msgId = 0;

    env->MSD_Data.msdId = MSD_V_2;                            // MSD format version

    env->MSD_Data.MSD_Control.extraData = 0x00;              // Additional information not included

    // aGpsStatus
    env->MSD_Data.MSD_Control.recentPos1 = 0x01;             // Recent vehicle position 1 available
    env->MSD_Data.MSD_Control.recentPos2 = 0x01;             // Recent vehicle position 2 available

    env->MSD_Data.additionalData.coordSystem = ERA_GLONASS_MSD_COORDINATE_SYSTEM_TYPE_WGS84;
    env->MSD_Data.additionalData.diagnosticResult = NULL;
}

void EraGlonassMsd_ClearDiagnostic(tMSD_DiagnosticResult *env) {
    memset(env, 0, sizeof(tMSD_DiagnosticResult));
}

// In case we don't have valid GPS data set defaults as described in EN15722
void EraGlonassMsdNoGnssSetDefaults(tEraGlonassMsd *env) {
    env->MSD_Data.timestamp = 0x00;
    env->MSD_Data.pos.lat = 0xFFFFFFFF;
    env->MSD_Data.pos.lon = 0xFFFFFFFF;

    env->MSD_Data.direction = 0xFF;

    env->MSD_Data.MSD_PosDelta[0].lat = 512;
    env->MSD_Data.MSD_PosDelta[0].lon = 512;

    env->MSD_Data.MSD_PosDelta[1].lat = 512;
    env->MSD_Data.MSD_PosDelta[1].lon = 512;

    // aGpsStatus
    env->MSD_Data.MSD_Control.recentPos1 = 0x00;             // Recent vehicle position 1 available
    env->MSD_Data.MSD_Control.recentPos2 = 0x00;             // Recent vehicle position 2 available
}

void EraGlonassMsdSetDataEmergencySituationFlags(
        tEraGlonassMsd *env,
        int MSD_MSG_ID,
        eEcallActivationType eCallType,
        eEcallTestMode eCallTestMode
) {
    // Message ID
    env->MSD_Data.msgId = MSD_MSG_ID;

    // Automatic or manual eCall activation
    if (eCallType == AUTOMATIC_ACTIVATION) {
        env->MSD_Data.MSD_Control.activation = 1;
    }
    if (eCallType == MANUAL_ACTIVATION) {
        env->MSD_Data.MSD_Control.activation = 0;
    }

    // Test eCall
    if (eCallTestMode == TEST_CALL) {
        env->MSD_Data.MSD_Control.testCall = 1;
    }
    if (eCallTestMode == EMERGENCY_CALL) {
        env->MSD_Data.MSD_Control.testCall = 0;
    }

}

void EraGlonassMsdCalcPosDelta(tEraGlonassMsd *env, tEraGlonassMsd_GpsPos lastPos) {
    int16_t deltaLat = 0, deltaLon = 0;

    deltaLat = (lastPos.lat - env->MSD_Data.pos.lat) / 100;
    deltaLon = (lastPos.lon - env->MSD_Data.pos.lon) / 100;

    if (deltaLat <= -512) {
        deltaLat = -512;
    } else if (deltaLat >= 511) {
        deltaLat = 511;
    }
    deltaLat += 512;                    // Make offset

    if (deltaLon <= -512) {
        deltaLon = -512;
    } else if (deltaLon >= 511) {
        deltaLon = 511;
    }
    deltaLon += 512;


    env->MSD_Data.MSD_PosDelta[1].lat = env->MSD_Data.MSD_PosDelta[0].lat;
    env->MSD_Data.MSD_PosDelta[1].lon = env->MSD_Data.MSD_PosDelta[0].lon;
    env->MSD_Data.MSD_Control.recentPos2 = env->MSD_Data.MSD_Control.recentPos1;

    env->MSD_Data.MSD_PosDelta[0].lat = deltaLat;
    env->MSD_Data.MSD_PosDelta[0].lon = deltaLon;
    env->MSD_Data.MSD_Control.recentPos1 = 0x01;

    //Update to the latest position in order to calculate
    //correctly the deltas on the next iteration
//    env->lastPos.lat = env->MSD_Data.pos.lat;
//    env->lastPos.lon = env->MSD_Data.pos.lon;
}

void EraGlonassMsdSetTimeStamp(tEraGlonassMsd *env, uint32_t timestamp) {
    env->MSD_Data.timestamp = timestamp;
}

void EraGlonassMsdSetPositionValue(
        tEraGlonassMsd *env,
        int32_t lonArc,
        int32_t latArc,
        uint16_t track,
        uint8_t valid
) {

    tEraGlonassMsd_GpsPos lastPos = env->MSD_Data.pos;

    env->MSD_Data.pos.lat = latArc;
    env->MSD_Data.pos.lon = lonArc;

    if (valid == 1){
        EraGlonassMsdCalcPosDelta(env, lastPos);
        env->MSD_Data.MSD_Control.posStatus = 0x00;
        env->MSD_Data.direction = 0xFF;
    } else if (valid == 2){
        EraGlonassMsdCalcPosDelta(env, lastPos);
        env->MSD_Data.MSD_Control.posStatus = 0x01;
        env->MSD_Data.direction = track/2;
    }

    env->MSD_Data.pos.lat += 0x80000000;                   // Make offset
    env->MSD_Data.pos.lon += 0x80000000;

}


void EraGlonassMsdSetPassengersNumber(tEraGlonassMsd *env, uint16_t value) {
    // Check passengers number
    if (value >= 1) {
        env->MSD_Data.MSD_Control.passengers = value;    // Passengers number available
    } else {
        env->MSD_Data.MSD_Control.passengers = 0x00;     // Passengers number not available
    }
}

void EraGlonassMsdSetVehicleType(tEraGlonassMsd *env, eUveosGostVehicleType value) {
    env->MSD_Data.MSD_Control.vehicleType = value;
}

void EraGlonassMsdSetVIN(tEraGlonassMsd *env, char *vin, uint8_t vinLen) {
    unsigned char ch = 0, i = 0;
    // Set VIN
    for (i = 0; i < 17; i++) {

        if (i < vinLen) {
            ch = vin[i];
        } else {
            ch = '0';
        }

        if (ch >= '0' && ch <= '9') {
            ch -= 0x30;     // Numbers are encoded with valuese 0x00 to 0x09
        } else if (ch >= 'A' && ch <= 'H') {
            ch -= 0x37;
        } else if (ch >= 'J' && ch <= 'N') {
            ch -= 0x38;
        } else if (ch == 'P') {
            ch = 0x17;
        } else if (ch >= 'R' && ch <= 'Z') {
            ch -= 0x3A;
        } else {
            ch = '0' - 0x30;
        }

        env->MSD_Data.vin[i] = ch;
    }
}

void EraGlonassMsdSetPropulsionStorageType(
        tEraGlonassMsd *env,
        eUveosGostVehiclePropulsionStorageType value
) {
    env->MSD_Data.MSD_Control.includedTanks = 0b01111111;          // Enable all tank type to be transmitted

    // Set propulsion system storage
    env->MSD_Data.propulsion = value;
}


uint16_t sEraGlonassMsdEncodeOfControl(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    lastBit += vBitBufferAppendByte(raw, lastBit, data->msgId, 8);

    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.activation ? 0x01 : 0x00, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.testCall ? 0x01 : 0x00, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.posStatus ? 0x01 : 0x00, 1);

    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.vehicleType, 5);

    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfVin(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    for (int i = 0; i < 17; ++i) {
        lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t) data->vin[i]), 6);
    }

    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfPropulsion(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    uint8_t maxCountOfTanks = 0;

    if (data->msdId == MSD_V_1) {
        maxCountOfTanks = 6;
    } else if (data->msdId == MSD_V_2) {
        maxCountOfTanks = 7;
    }


    uint8_t tanksInfoIncluded = !(data->MSD_Control.includedTanks >> maxCountOfTanks) && 0x1;

    if (tanksInfoIncluded) {
        lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.includedTanks, maxCountOfTanks + 1);

        for (int i = 0; i < maxCountOfTanks; ++i) {
            if ((data->MSD_Control.includedTanks >> i) && 0x1) {
                lastBit += vBitBufferAppendByte(raw, lastBit, (data->propulsion >> i), 1);
            }
        }
    } else {
        lastBit += vBitBufferAppendByte(raw, lastBit, 0x1, 1);
    }


    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfTimestamp(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->timestamp, 32);

    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfLocation(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->pos.lat, 32);
    lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->pos.lon, 32);

    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfDirection(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    \
     lastBit += vBitBufferAppendByte(raw, lastBit, data->direction, 8);

    return lastBit;
}

uint16_t sEraGlonassMsdEncodeOfRecentPositions(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    \
    if (data->MSD_Control.recentPos1) {
        lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[0].lat, 10);
        lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[0].lon, 10);
    }

    if (data->MSD_Control.recentPos2) {
        lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[1].lat, 10);
        lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[1].lon, 10);
    }
    return lastBit;
}


uint16_t sEraGlonassMsdEncodeOfPassengersCount(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    if (data->MSD_Control.passengers) {
        lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_Control.passengers, 8);
    }
    return lastBit;
}

uint16_t sEraGlonassMsdEncodeOfV1FreeSpaceFix(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    if (data->msdId == MSD_V_1) {
        lastBit += vBitBufferAppendByte(raw, lastBit, 0x00, 8);
    }
    return lastBit;
}


uint16_t
sEraGlonassMsdEncodeOfAdditionalDataOID(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
    lastBit += vBitBufferAppendByte(raw, lastBit, 0x03, 8);//lent of OID

    lastBit += vBitBufferAppendByte(raw, lastBit, 0x01, 8);//OID 1
    lastBit += vBitBufferAppendByte(raw, lastBit, 0x04, 8);//OID 2

    if (data->msdId == MSD_V_1) {
        lastBit += vBitBufferAppendByte(raw, lastBit, 0x01, 8);//OID 3
    } else if (data->msdId == MSD_V_2) {
        lastBit += vBitBufferAppendByte(raw, lastBit, 0x02, 8);//OID 3
    }

    return lastBit;
}

#define MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(FLAG) lastBit += vBitBufferAppendByte(raw, lastBit, (FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_ABSENT ? 0b0 : 0b1, 1);

#define MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(FLAG) \
if((FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_PRESENT_TRUE) { lastBit += vBitBufferAppendByte(raw, lastBit, 0b1 , 1);} \
else if((FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_PRESENT_FALSE) { lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);}

uint16_t sEraGlonassMsdEncodeOfAdditionalDataPayload(
        tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit
) {

    lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1); // enable extendable
    //enable what present
    lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.asi15Present ? 0b1 : 0b0, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.diagnosticResult ? 0b1 : 0b0, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.crashInfoPresent ? 0b1 : 0b0, 1);

    if (data->msdId == MSD_V_2) {
        lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.coordSystem ? 0b1 : 0b0,
                                        1); // enable extendable
    }

    if (data->additionalData.asi15Present) {
        lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t *) &data->additionalData.asi15)[1], 3);
        lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t *) &data->additionalData.asi15)[0], 8);
    }

    if (data->additionalData.diagnosticResult) {
        tMSD_DiagnosticResult *diagRes = data->additionalData.diagnosticResult;

        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->micConnectionFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->micFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->rightSpeakerFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->leftSpeakerFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->speakersFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->ignitionLineFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->uimFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->statusIndicatorFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->batteryFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->batteryVoltageLow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->crashSensorFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->firmwareImageCorruption);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->commModuleInterfaceFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->gnssReceiverFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->raimProblem);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->gnssAntennaFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->commModuleFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->eventsMemoryOverflow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->crashProfileMemoryOverflow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->otherCriticalFailures);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->otherNotCriticalFailures);


        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->micConnectionFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->micFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->rightSpeakerFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->leftSpeakerFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->speakersFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->ignitionLineFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->uimFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->statusIndicatorFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->batteryFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->batteryVoltageLow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->crashSensorFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->firmwareImageCorruption);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->commModuleInterfaceFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->gnssReceiverFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->raimProblem);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->gnssAntennaFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->commModuleFailure);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->eventsMemoryOverflow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->crashProfileMemoryOverflow);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->otherCriticalFailures);
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->otherNotCriticalFailures);

    }

    if (data->additionalData.crashInfoPresent) {

        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashFront)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashLeftSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashRightSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashRear)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashWithRollover)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashFrontOrSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashOtherType)

        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashFront)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashLeftSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashRightSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashRear)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashWithRollover)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashFrontOrSide)
        MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashOtherType)

    }

    if (data->msdId == MSD_V_2) {
        lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.coordSystem, 2);
    }

    return lastBit;
}

uint16_t sEraGlonassMsdEncodeOfAdditionalDataOctetString(
        tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit
) {

    uint16_t octetStringOffset = lastBit + 8;

    uint16_t octetStringBitsLength =
            sEraGlonassMsdEncodeOfAdditionalDataPayload(data, raw, octetStringOffset) - (octetStringOffset);

    uint8_t octetsCount = octetStringBitsLength / 8 + (octetStringBitsLength % 8 ? 1 : 0);

    lastBit += vBitBufferAppendByte(raw, lastBit, octetsCount, 8);
    lastBit += octetsCount * 8;

    return lastBit;
}

uint16_t sEraGlonassMsdEncodeOfAdditionalData(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    lastBit = sEraGlonassMsdEncodeOfAdditionalDataOID(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfAdditionalDataOctetString(data, raw, lastBit);

    return lastBit;
}

uint16_t sEraGlonassMsdEncodeOfMsdMainBody(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalDataPresent ? 0b1 : 0b0, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);
//    lastBit+= vBitBufferAppendByte(raw,lastBit,data->MSD_Control.extraData,3);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.recentPos1 ? 0x01 : 0x00, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.recentPos2 ? 0x01 : 0x00, 1);
    lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.passengers ? 0x01 : 0x00, 1);

    lastBit = sEraGlonassMsdEncodeOfControl(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfVin(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfPropulsion(data, raw, lastBit);

    lastBit = sEraGlonassMsdEncodeOfV1FreeSpaceFix(data, raw, lastBit);

    lastBit = sEraGlonassMsdEncodeOfTimestamp(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfLocation(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfDirection(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfRecentPositions(data, raw, lastBit);
    lastBit = sEraGlonassMsdEncodeOfPassengersCount(data, raw, lastBit);

    if (data->additionalDataPresent) {
        lastBit = sEraGlonassMsdEncodeOfAdditionalData(data, raw, lastBit);
    }

    return lastBit;
}

uint16_t sEraGlonassMsdEncodeV2BodyOctetString(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {

    //length of msd block
    uint16_t octetStringOffset = lastBit + 8;
    uint16_t octetStringBitsLength =
            sEraGlonassMsdEncodeOfMsdMainBody(data, raw, octetStringOffset) - (octetStringOffset);
    uint8_t octetsCount = octetStringBitsLength / 8 + (octetStringBitsLength % 8 ? 1 : 0);

    lastBit += vBitBufferAppendByte(raw, lastBit, octetsCount, 8);

    lastBit += octetsCount * 8;

    return lastBit;
}


uint16_t EraGlonassMsdEncode(tEraGlonassMsd *env, eEraGlonassMsd_Version msdVersion, uint8_t *encoded) {
    env->MSD_Data.msdId = msdVersion;

    memset(encoded, '\0', ERA_GLONASS_MSD_ENCODED_BUFFER_LENGTH);    // Clear

    uint16_t lastBit = 0;

    lastBit += vBitBufferAppendByte(encoded, lastBit, env->MSD_Data.msdId, 8);

    if (env->MSD_Data.msdId == MSD_V_1) {
        lastBit = sEraGlonassMsdEncodeOfMsdMainBody(&env->MSD_Data, encoded, lastBit);
    } else if (env->MSD_Data.msdId == MSD_V_2) {
        lastBit = sEraGlonassMsdEncodeV2BodyOctetString(&env->MSD_Data, encoded, lastBit);
    }

    size_t byteLength = lastBit / 8 + (lastBit % 8 ? 1 : 0);
    return byteLength;
}