#include <map>
#include "Servo.h"

std::map<uint8_t, Servo> servos;

void setup()
{
    Serial.begin(115200);
}

void setupServos(uint8_t *calibrationValues, uint8_t size)
{
    // We assume there are 3 bytes per servo. Ignore if there aren't.
    if (size % 3 == 0)
    {
        for (int i = 0; i < size; i += 3)
        {
            servos.insert(std::make_pair(calibrationValues[i], new Servo()))
        }
    }
}

void loop()
{
    uint8_t *header = new uint8_t[2];
    Serial.readBytes(header, 2);

    if (header[0] == 0)
    {
        uint8_t *calibration = new uint8_t[3 * header[1]];
        Serial.readBytes(calibration, header[1]);
        setupServos();
        delete calibration;
    }

    delete header;
}