#include <string.h>
#include "stm32f1xx.h"
#include "pwm_output.h"
#include "config.h"
#include "pinning.h"
#include "led.h"
#include "ownership.h"

MODULE_OWNS_PIN(GPIOA, PIN_OUTPUT_X);
MODULE_OWNS_PIN(GPIOA, PIN_OUTPUT_Y);
MODULE_OWNS_PIN(GPIOB, PIN_PEN_STATE);
MODULE_OWNS_PIN(GPIOA, PIN_OUTPUT_ENABLE);
MODULE_OWNS_PERIPHERAL(TIM1);
MODULE_OWNS_PERIPHERAL(TIM4);

static HPGL_Movement_t Output_Buffer[CONFIG_BUFFER_MOVEMENTS];
static const HPGL_Movement_t *Output_BufferRead = Output_Buffer;
static HPGL_Movement_t *Output_BufferWrite = Output_Buffer;
//static bool Output_PenIsDown;
static unsigned int Output_PenState = OUTPUT_PEN_UP;
static unsigned int Output_DelayCounter = 0;

typedef struct
{
    unsigned int x;
    unsigned int y;
} Output_Coordinate_t;

static Output_Coordinate_t Output_CurrentLineStart = {0, 0};
static Output_Coordinate_t Output_CurrentLineEnd = {0, 0};
static unsigned int Output_CurrentLineLength;
static unsigned int Output_CurrentLinePosition;
static unsigned int Output_CurrentVelocity = OUTPUT_LENGTH_SCALE / 3;

bool Output_EnqueueMovement(HPGL_Movement_t movement)
{
    int buffer_space = Output_BufferRead - Output_BufferWrite - 1;
    if(buffer_space < 0)
    {
        buffer_space += CONFIG_BUFFER_MOVEMENTS;
    }
    if(buffer_space == 0)
    {
        return false;
    }

    if(movement.x > OUTPUT_COORDINATE_LIMIT)
    {
        movement.x = OUTPUT_COORDINATE_LIMIT;
    }
    if(movement.y > OUTPUT_COORDINATE_LIMIT)
    {
        movement.y = OUTPUT_COORDINATE_LIMIT;
    }

    memcpy(Output_BufferWrite, &movement, sizeof(HPGL_Movement_t));
    Output_BufferWrite++;
    if(Output_BufferWrite >= Output_Buffer + CONFIG_BUFFER_MOVEMENTS)
    {
        Output_BufferWrite = Output_Buffer;
    }
    return true;
}

static void Output_PenDown(void)
{
    Output_PenState = OUTPUT_PEN_START;
    TIM4->CCR1 = Output_PenState;
}

static void Output_PenUp(void)
{
    Output_PenState = OUTPUT_PEN_UP;
    TIM4->CCR1 = Output_PenState;
}

static unsigned int Output_ApproximateLength(int dx, int dy)
{
    if(dx < 0)
    {
        dx = -dx;
    }
    if(dy < 0)
    {
        dy = -dy;
    }

    // Approximate vector length with alpha max plus beta min algorithm
    if(dx < dy)
    {
        return (dy + dx) * OUTPUT_LENGTH_SCALE / 4;
    }
    else
    {
        return (dx + dy) * OUTPUT_LENGTH_SCALE / 4;
    }
}

static bool Output_FetchNextPoint(void)
{
    LED_Off(LED_Pink);
    if(Output_BufferRead == Output_BufferWrite)
    {
        LED_Off(LED_Orange);
        return false;
    }

    const HPGL_Movement_t *movement = Output_BufferRead;
    if(movement->pen_down != (Output_PenState == OUTPUT_PEN_DOWN))
    {
        if(movement->pen_down)
        {
            Output_PenDown();
            Output_DelayCounter = OUTPUT_PEN_DOWN_DELAY;
        }
        else
        {
            Output_PenUp();
            Output_DelayCounter = OUTPUT_PEN_UP_DELAY;
        }
        return false;
    }

    Output_CurrentLineStart = Output_CurrentLineEnd;
    Output_CurrentLineEnd.x = movement->x;
    Output_CurrentLineEnd.y = movement->y;
    Output_CurrentLineLength = Output_ApproximateLength(
        Output_CurrentLineEnd.x - Output_CurrentLineStart.x,
        Output_CurrentLineEnd.y - Output_CurrentLineStart.y);
    Output_CurrentLinePosition = 0;

    Output_BufferRead++;
    if(Output_BufferRead >= Output_Buffer + CONFIG_BUFFER_MOVEMENTS)
    {
        Output_BufferRead = Output_Buffer;
    }
    return true;
}

static void Output_Position(Output_Coordinate_t position)
{
    uint16_t compare_x = OUTPUT_PWM_OFFSET + position.x;
    uint16_t compare_y = OUTPUT_PWM_OFFSET + position.y;

    TIM1->CCR1 = compare_x;
    TIM1->CCR2 = compare_y;
}

// Update the pen PWM output if the pen is currently moving, return false
// otherwise
static bool Output_UpdatePen(void)
{
    // Only the downwards motion of the pen is smoothed. Thus, the direction is
    // implied within the state variable (intermediate values only occur while
    // the pen is being lowered).
    if((Output_PenState == OUTPUT_PEN_DOWN)
        || (Output_PenState == OUTPUT_PEN_UP))
    {
        return false;
    }

    Output_PenState += OUTPUT_PEN_STEP;
    if(Output_PenState >= OUTPUT_PEN_STOP)
    {
        Output_PenState = OUTPUT_PEN_DOWN;
    }
    TIM4->CCR1 = Output_PenState;

    return true;
}

void Output_Tick(void)
{
    // Do not move while pen is lowered
    if(Output_UpdatePen())
    {
        return;
    }

    // Wait for a bit longer after each pen state change
    if(Output_DelayCounter > 0)
    {
        Output_DelayCounter--;
        return;
    }

    unsigned int step_length = Output_CurrentVelocity;
    Output_Coordinate_t position;
    while(true)
    {
        // Compute distance from current position to current line’s end
        unsigned int length_ahead = Output_CurrentLineLength
            - Output_CurrentLinePosition;
        if(step_length <= length_ahead)
        {
            // Move closer to current line end point
            Output_CurrentLinePosition += step_length;
            unsigned int remaining = Output_CurrentLineLength
                - Output_CurrentLinePosition;
            position.x = ((long long)Output_CurrentLineEnd.x
                        * Output_CurrentLinePosition
                    + (long long)Output_CurrentLineStart.x
                        * remaining)
                / Output_CurrentLineLength;
            position.y = ((long long)Output_CurrentLineEnd.y
                        * Output_CurrentLinePosition
                    + (long long)Output_CurrentLineStart.y
                        * remaining)
                / Output_CurrentLineLength;
            break;
        }

        // Current target point is not far enough away to keep velocity
        step_length -= length_ahead;
        position = Output_CurrentLineEnd;
        Output_CurrentLinePosition = Output_CurrentLineLength;

        // Replace line start with current line end and fetch new line end
        if(!Output_FetchNextPoint())
        {
            // No more points in queue or pen state change
            break;
        }
    }

    Output_Position(position);
}

void Output_Init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;

    Output_PenUp();

    GPIOA->CRL = (GPIOA->CRL
        & ~(0xf << (4 * PIN_OUTPUT_ENABLE)))
        | (0x02 << (PIN_OUTPUT_ENABLE * 4)) // 2 MHz push-pull output
        ;
    // Close relays
    GPIOA->BSRR = (1 << PIN_OUTPUT_ENABLE);

    GPIOA->CRH = (GPIOA->CRH
        & ~(0xf << (4 * PIN_OUTPUT_X - 32))
        & ~(0xf << (4 * PIN_OUTPUT_Y - 32)))
        | (0xa << (4 * PIN_OUTPUT_X - 32))  // AF output, 2 MHz
        | (0xa << (4 * PIN_OUTPUT_Y - 32))  // AF output, 2 MHz
        ;

    GPIOB->CRL = (GPIOB->CRL
        & ~(0xf << (4 * PIN_PEN_STATE)))
        | (0xe << (4 * PIN_PEN_STATE))      // AF-OD output, 2 MHz
        ;

    TIM1->CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1
        | TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1;
    TIM1->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
    TIM1->BDTR = TIM_BDTR_MOE;
    TIM1->ARR = (1 << OUTPUT_PWM_RESOLUTION) - 1;
    TIM1->DIER = TIM_DIER_UIE;
    TIM1->CR1 = TIM_CR1_CEN;
    NVIC_EnableIRQ(TIM1_UP_IRQn);

    TIM4->CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1;
    TIM4->CCER = TIM_CCER_CC1P | TIM_CCER_CC1E;
    TIM4->PSC = OUTPUT_PEN_PRESCALER - 1;
    TIM4->ARR = OUTPUT_PEN_RELOAD;
    TIM4->CCR1 = OUTPUT_PEN_UP;
    TIM4->CR1 = TIM_CR1_CEN;
}

void TIM1_UP_IRQHandler(void)
{
    Output_Tick();

    TIM1->SR &= ~TIM_SR_UIF;
}
