// Example program
#include <iostream>
#include <string>
#include <array>
#include <stdint.h>
#include <iomanip>

#define PACKET_N_BYTES 16

using ui8_t = uint8_t;

using packet_t = std::array<ui8_t, PACKET_N_BYTES>;
using payload_t = std::array<ui8_t, 12>;

const std::array<ui8_t,2> head = {0xAE, 0xAE};
const std::array<ui8_t,2> tail = {0xAE, 0xAE};

enum packetParts
{
    Head,
    Payload,
    Tail
};

void outputPayload(payload_t payload)
{
    
    for (int i = 0; i < 12; ++i)
        std::cout << std::hex << std::setfill('0') << std::setw(2) << payload[i] << " ";
    std::cout << std::endl;
}

void parsePacket(const packet_t& newPacket)
{
    bool done = false;
    payload_t payload;
    packetParts packetIndex = Head;
    ui8_t i = 0;

    do {
        switch(packetIndex) {
            case Head:
                std::cout << "checking packet head..." << std::endl;
                // check head
                for(i = 0; i < 2; i++) {
                    if(head[i] != newPacket[i]) {
                        std::cout << "...failed!" << std::endl;   
                        return;
                    }
                }
                std::cout << "...head valid!" << std::endl;
                packetIndex = Payload;
                break;
            case Payload:
                // copy payload
                std::cout << "copy payload..." << std::endl;
                for(;i < 14; i++) {
                    payload[i - 2] = newPacket[i];
                }
                std::cout << "...done!" << std::endl;
                packetIndex = Tail;
                break;
            case Tail:
                // check tail
                std::cout << "checking packet tail..." << std::endl;
                for(;i < 16; i++) {
                    if(tail[(i - 14)] != newPacket[i]) {
                        std::cout << "...failed!" << std::endl;   
                        return;
                    }
                }
                std::cout << "...tail valid!" << std::endl;
                std::cout << "forwarding payload!" << std::endl;
                outputPayload(payload);
                done = true;
                break;
            default:
                return;
        };
    } while(!done);
}




int main()
{
    packet_t receivedPacket = {0xAE, 0xAE, 0x00, 0x00,
                               0x00, 0x00, 0x00, 0x00,
                               0x00, 0x00, 0x00, 0x00,
                               0x00, 0x00, 0xAE, 0xAE};
                               
    packet_t corruptedPacket = {0xAE, 0xAE, 0x00, 0x00,
                               0x00, 0x00, 0x00, 0x00,
                               0x00, 0x00, 0x00, 0x00,
                               0x00, 0x00, 0xAE, 0xFE};         
             
    parsePacket(receivedPacket);
    parsePacket(corruptedPacket);
}