#ifndef UWB_H
#define UWB_H

#include <cstdint>
#include <vector>

struct UwbPosition {
    int x = 0;
    int y = 0;
    int z = 0;
    uint8_t quali = 0;
};

struct UwbDistance {
    uint16_t nodeID = 0;
    int distance = 0;
    uint8_t quali = 0;
};

struct UwbResult {
    UwbPosition pos;
    std::vector<UwbDistance> distances;
};


static void uwb_parse_pos(const unsigned char input[], UwbPosition& pos)
{
    // X,Y,Z coordinates (each 4 bytes) and quality factor (1 byte), total size: 13 bytes

    pos.x  = input[0];
    pos.x |= input[1] << 8;
    pos.x |= input[2] << 16;
    pos.x |= input[3] << 24;

    pos.y  = input[4];
    pos.y |= input[5] << 8;
    pos.y |= input[6] << 16;
    pos.y |= input[7] << 24;

    pos.z  = input[8];
    pos.z |= input[9] << 8;
    pos.z |= input[10] << 16;
    pos.z |= input[11] << 24;

    pos.quali = input[12];
}

static void uwb_parse_dist(const unsigned char input[], std::vector<UwbDistance>& result)
{
    // First byte is distance count(1 byte)
    // Sequence of node ID(2 bytes), distance in mm(4 bytes) and quality factor(1 byte)
    // Max value contains 15 elements, size: 8 - 106

    size_t numOfAnchors = input[0];

    for (size_t i = 0; i < numOfAnchors; i++)
    {
        size_t offset = 1 + i * 7;

        UwbDistance dist;

        dist.nodeID = input[offset + 1] << 8 | input[offset];
        dist.distance = input[offset + 5] << 24
                      | input[offset + 4] << 16
                      | input[offset + 3] << 8
                      | input[offset + 2];
        dist.quali = input[offset + 6];

        result.push_back(dist);
    }
}

static UwbResult uwb_parse(const unsigned char input[])
{
    UwbResult result;

    if (input[0] == 0)
    {
        // pos only
        uwb_parse_pos(&input[1], result.pos);
    }
    else if (input[0] == 1)
    {
        // dist only
        uwb_parse_dist(&input[1], result.distances);
    }
    else if (input[0] == 2)
    {
        // Encoded Position (as above, 13 bytes)
        // Encoded Distances(as above, 8 - 29 bytes).Position and distances
        // are sent by tag, with a maximum number of 4 ranging anchors
        uwb_parse_pos(&input[1], result.pos);
        uwb_parse_dist(&input[1 + 13], result.distances);
    }

    return result;
}

#endif // UWB_H