#include <Arduino.h>
#include <Servo.h>

int array_size;
bool receive_done;
int *input_array;
int servo_array[6];
bool receive_start;

bool DEBUG = true;

void setup()
{
    Serial.begin(9600);
    for (int i = 0; i < 6; i++)
    pinMode(i + 2, OUTPUT);
}

void reset_servos()
{
    for (int i = 0; i < 6; i++) {
        digitalWrite(i + 2, LOW);
        servo_array[i] = 0;
    }
    delay(100); /* artificial "transition" delay */
}

void process_servo_action()
{
    static int cur_pos = 0;
    static bool arrays_set = false;
    static bool debounce_hold_right = false;
    static bool debounce_hold_left = false;

    if (!arrays_set) {
        reset_servos();

        if (input_array[cur_pos] & 0b000001)
        servo_array[0] = 1;
        if (input_array[cur_pos] & 0b000010)
        servo_array[1] = 1;
        if (input_array[cur_pos] & 0b000100)
        servo_array[2] = 1;
        if (input_array[cur_pos] & 0b001000)
        servo_array[3] = 1;
        if (input_array[cur_pos] & 0b010000)
        servo_array[4] = 1;
        if (input_array[cur_pos] & 0b100000)
        servo_array[5] = 1;

        if (DEBUG) {
            Serial.println("Showing value: ");
            Serial.println(input_array[cur_pos]);
            Serial.println("At pos: ");
            Serial.println(cur_pos);
        }

        for (int i = 0; i < 6; i++) {
            if (servo_array[i] == 1)
            digitalWrite(i + 2, HIGH);
        }
        arrays_set = true;
    }

    if (Serial.available()) {
        int serial_data = Serial.read();

        if (serial_data == 0xF1) {
            cur_pos--;
            } else if (serial_data == 0xF2) {
            cur_pos++;
            } else if (serial_data == 0xF0) { /* new string input */
            receive_done = false;
            receive_start = true;
            cur_pos = 0;
            if (DEBUG) Serial.println("Received new string");
            delete[] input_array;
        }
        arrays_set = false;
    }
}

void process_serial_data()
{
    static bool array_created = false;
    static int data_collect = 0;

    if (Serial.available()) {
        int incoming_data = Serial.read();

        Serial.println(incoming_data, HEX);

        if (array_created && receive_start) {
            if (data_collect < array_size) {
                input_array[data_collect] = incoming_data;
                data_collect++;
                if (data_collect == array_size) {
                    receive_done = true;
                    receive_start = false;
                    array_created = false;
                    return;
                }
            }
        }

        if (receive_start && !array_created) {
            /* heap fragmentation issues with dynamic mem allocation? */
            input_array = new int[incoming_data];
            if (input_array == NULL)
                Serial.println("Failed to alloc mem for input_array");

            array_size = incoming_data;
            data_collect = 0;
            array_created = true;
            if (DEBUG) {
                Serial.print("Created array of size: ");
                Serial.print(incoming_data);
                Serial.println("");
            }
        }

        if (incoming_data == 0xF0) {
            if (DEBUG)
            Serial.println("Received start send signal");
            receive_start = true;
        }
    }
}

void loop()
{
    if (!receive_done)
        process_serial_data();
    else if (receive_done)
        process_servo_action();
}