#include "hpgl.h"
#include "usb_cdc.h"
#include "pwm_output.h"
#include "led.h"
#include "system.h"

unsigned int HPGL_ParseErrorCounter = 0;

typedef enum
{
    // Last command finished, waiting for more
    HPGL_Parser_Idle,

    // First byte of instruction has been received, waiting for second one
    HPGL_Parser_ReceivingInstruction,
    HPGL_Parser_ReceivingDeviceControl,

    // Receiving the pen number (currently ignored)
    HPGL_Parser_ReceivingPenNumber,

    // Receiving coordinates after movement/drawing command
    HPGL_Parser_ReceivingX,
    HPGL_Parser_ReceivingY,

    HPGL_Parser_Error,
} HPGL_ParserState_t;

// Adds a movement to the queue. Returns false when the queue is currently full
static bool HPGL_EnqueueMovement(HPGL_Movement_t movement)
{
    return Output_EnqueueMovement(movement);
}

// Helper macro to combine two characters into one integer
#define INST(a,b)               (((unsigned int)a) << 8 | (unsigned int)b)

void HPGL_Poll(void)
{
    static HPGL_ParserState_t state = HPGL_Parser_Idle;
    static char last_character = 0;
    // Movement which is currently built from received characters
    static HPGL_Movement_t current_movement;
    // Movemenet which is waiting to be enqueued if the queue is full
    static HPGL_Movement_t waiting_movement;

    static bool waiting_for_movement_queue = false;

    if(waiting_for_movement_queue)
    {
        if(!HPGL_EnqueueMovement(waiting_movement))
        {
            // Still not enough space
            return;
        }
        waiting_for_movement_queue = false;
    }

    int received = USBCDC_ReceiveByte();
    if(received == -1)
    {
        // Nothing received
        return;
    }
    LED_On(LED_Pink);
    LED_On(LED_Orange);
    char character = received;

    switch(state)
    {
        case HPGL_Parser_Idle:
            if(character == '\r' || character == '\n'
                || character == ';' || character == ' ')
            {
                // Ignore delimiters
                break;
            }
            if(character >= 'A' && character <= 'Z')
            {
                // First character of all supported instructions
                state = HPGL_Parser_ReceivingInstruction;
            }
            else if(character == 27)
            {
                // Found a device control instruction, which always starts with
                // ESC
                state = HPGL_Parser_ReceivingDeviceControl;
            }
            else
            {
                // Unsupported command
                HPGL_ParseErrorCounter++;
                state = HPGL_Parser_Error;
            }
            break;

        case HPGL_Parser_ReceivingInstruction:
            switch(INST(last_character, character))
            {
                case INST('I', 'N'):
                    // Initialise
                    HPGL_ParseErrorCounter = 0;
                    // No parameters expected for this one
                    state = HPGL_Parser_Idle;
                    break;

                case INST('S', 'P'):
                    // Set pen
                    state = HPGL_Parser_ReceivingPenNumber;
                    break;

                case INST('P', 'U'):
                    // Movement with pen up
                    current_movement.pen_down = false;
                    current_movement.x = 0;
                    current_movement.y = 0;
                    state = HPGL_Parser_ReceivingX;
                    break;

                case INST('P', 'D'):
                    // Movement with pen down
                    current_movement.pen_down = true;
                    current_movement.x = 0;
                    current_movement.y = 0;
                    state = HPGL_Parser_ReceivingX;
                    break;

                default:
                    // Unsupported instruction
                    HPGL_ParseErrorCounter++;
                    state = HPGL_Parser_Error;
                    break;
            }
            break;

        case HPGL_Parser_ReceivingDeviceControl:
            switch(character)
            {
                case 'B':
                    // Reset into bootloader
                    LED_On(LED_Orange);
                    LED_On(LED_Pink);
                    System_EnterBootloader();
                    break;

                default:
                    // Unsupported instruction
                    HPGL_ParseErrorCounter++;
                    state = HPGL_Parser_Error;
                    break;
            }
            break;

        case HPGL_Parser_ReceivingX:
        case HPGL_Parser_ReceivingY:
            if(character == ';' || character == '\n')
            {
                // Command finished
                if(!HPGL_EnqueueMovement(current_movement))
                {
                    // Not enough space in queue
                    waiting_movement = current_movement;
                    waiting_for_movement_queue = true;
                }
                state = HPGL_Parser_Idle;
            }
            else if(character == ',')
            {
                // Next coordinate
                if(state == HPGL_Parser_ReceivingX)
                {
                    state = HPGL_Parser_ReceivingY;
                }
                else
                {
                    // Next movement
                    if(!HPGL_EnqueueMovement(current_movement))
                    {
                        // Not enough space in queue
                        waiting_movement = current_movement;
                        waiting_for_movement_queue = true;
                    }
                    // Reset coordinates, pen position stays unmodified
                    current_movement.x = 0;
                    current_movement.y = 0;
                    // Receive an X coordinate again
                    state = HPGL_Parser_ReceivingX;
                }
            }
            else if(character >= '0' && character <= '9')
            {
                unsigned int digit = character - '0';
                if(state == HPGL_Parser_ReceivingX)
                {
                    current_movement.x = current_movement.x * 10 + digit;
                }
                else
                {
                    current_movement.y = current_movement.y * 10 + digit;
                }
            }
            else
            {
                // No decimal support so far. Sorry.
                state = HPGL_Parser_Error;
            }
            break;

        case HPGL_Parser_ReceivingPenNumber:
            // Ignore everything except for end of command
            if(character == ';' || character == '\n')
            {
                state = HPGL_Parser_Idle;
            }
            break;

        case HPGL_Parser_Error:
            // Exit error state only after end of command
            if(character == '\n' || character == ';')
            {
                state = HPGL_Parser_Idle;
            }
            break;
    }

    last_character = character;
}
