#include <stdio.h>
#include <stdint.h>

/*
 * |   0   |   1   |   2   |   3   |   4   |   5   |   6   |   7   |   8   |   9   |
 * 11111111111111112222222233333333333333334444444444444455555555555555666677777777
 */

#pragma pack(1)
typedef struct DataStruct {
    uint16_t senderID;
    uint8_t packetType;
    int16_t altitude;
    uint32_t latLonFlags;
    uint8_t ext;
} dataStruct_t;

typedef struct Packet {
    uint32_t senderID;
    uint32_t packetType;
    int32_t altitude;
    int32_t longitude;
    int32_t latitude;
    uint8_t flags;
    uint8_t ext;
} packet_t;

void marshal(packet_t *p, const uint8_t *data) {
    ((dataStruct_t *) data)->senderID = p->senderID;
    ((dataStruct_t *) data)->packetType = p->packetType;
    ((dataStruct_t *) data)->altitude = p->altitude;
    {   // Latitude longitude and flags
        uint32_t latLonFlags;
        int32_t latitude = p->latitude;
        int32_t longitude = p->longitude;
        uint32_t flags = p->flags;
        // Saturation
        if (latitude < -8192) latitude = -8192;
        if (latitude > 8191) latitude = 8191;
        if (longitude < -8192) longitude = -8192;
        if (longitude > 8191) longitude = 8191;
        if (flags > 15) flags = 15;
        latitude += 8192;
        longitude += 8192;

        latLonFlags = (latitude << 18) | (longitude << 4) | flags;

        ((dataStruct_t *) data)->latLonFlags = latLonFlags;
    }
    ((dataStruct_t *) data)->ext = p->ext;
}

void unmarshal(const uint8_t *data, packet_t *p) {
    p->senderID = ((dataStruct_t *) data)->senderID;
    p->packetType = ((dataStruct_t *) data)->packetType;
    p->altitude = ((dataStruct_t *) data)->altitude;
    p->ext = ((dataStruct_t *) data)->ext;

    uint32_t latLonFlags = ((dataStruct_t *) data)->latLonFlags;

    p->latitude = (int32_t) ((latLonFlags & 0b11111111111111000000000000000000) >> 18) - 8192;
    p->longitude = (int32_t) ((latLonFlags & 0b00000000000000111111111111110000) >> 4) - 8192;
    p->flags = (uint8_t) (latLonFlags & 0b00000000000000000000000000001111);
}

int main() {
    packet_t packetSrc;
    packet_t packetDst;
    uint8_t data[10];

    // Inserisco dati in packetSrc
    packetSrc.senderID = 1;
    packetSrc.packetType = 2;
    packetSrc.altitude = 3;
    packetSrc.longitude = 4;
    packetSrc.latitude = 5;
    packetSrc.flags = 6;
    packetSrc.ext = 7;

    // Marshalling e Unmarshalling
    marshal(&packetSrc, data);
    unmarshal(data, &packetDst);


    // Confronto packetSrc con packetDst
    int errors = 0;

    if (packetSrc.senderID != packetDst.senderID) {
        errors++;
        printf("Sender ID: %i != %i\n", packetSrc.senderID, packetDst.senderID);
    }
    if (packetSrc.packetType != packetDst.packetType) {
        errors++;
        printf("Packet Type: %i != %i\n", packetSrc.packetType, packetDst.packetType);
    }
    if (packetSrc.altitude != packetDst.altitude) {
        errors++;
        printf("Packet Type: %i != %i\n", packetSrc.altitude, packetDst.altitude);
    }
    if (packetSrc.longitude != packetDst.longitude) {
        errors++;
        printf("Longitude: %i != %i\n", packetSrc.longitude, packetDst.longitude);
    }
    if (packetSrc.latitude != packetDst.latitude) {
        errors++;
        printf("Latitude: %i != %i\n", packetSrc.latitude, packetDst.latitude);
    }
    if (packetSrc.flags != packetDst.flags) {
        errors++;
        printf("Flags: %i != %i\n", packetSrc.flags, packetDst.flags);
    }
    if (packetSrc.ext != packetDst.ext) {
        errors++;
        printf("Ext: %i != %i\n", packetSrc.ext, packetDst.ext);
    }

    if (errors == 0) {
        printf("\nMarshalling e Unmarshalling avvenuto correttamente.\n");
    } else {
        printf("\nCi sono stati problemi durante il Marshalling o Unmarshalling.\n");
    }

    return 0;
}