#include "stm32g0b1xx.h"
#include "stepper.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "SEGGER_SYSVIEW.h"
#include "uart_dmx.h"

stepper_T steppers[] = {
    {.en_port=GPIOC, .en_pin=11, .dir_port=GPIOB, .dir_pin=4, .step_port=GPIOE, .step_pin=2, .home_port=GPIOF, .home_pin=3},
    {.en_port=GPIOB, .en_pin=3, .dir_port=GPIOF, .dir_pin=11, .step_port=GPIOF, .step_pin=12, .home_port=GPIOF, .home_pin=4}
};

void set_pins(stepper_T* stp){
    // Set direction (00: Input, 01: Output, 10: Alternate function, 11: Analog)

    // enable
    stp->en_port->MODER &= ~(0x3 << (stp->en_pin * 2));
    stp->en_port->MODER |=  (0x1 << (stp->en_pin * 2));

    // direction
    stp->dir_port->MODER &= ~(0x3 << (stp->dir_pin * 2));
    stp->dir_port->MODER |=  (0x1 << (stp->dir_pin * 2));

    // step
    stp->step_port->MODER &= ~(0x3 << (stp->step_pin * 2));
    stp->step_port->MODER |=  (0x1 << (stp->step_pin * 2));

    // home switch
    stp->home_port->MODER &= ~(0x3 << (stp->home_pin * 2));

    // make shure stepper is enabled (active low)
    stp->en_port->BSRR |= (0x1 << (stp->en_pin + 16));
}

void stepper_gpio_init(){
    RCC->IOPENR |= RCC_IOPENR_GPIOBEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOCEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOEEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOFEN;

    for(uint8_t i=0; i<(sizeof(steppers)/sizeof(stepper_T)); i++){
        set_pins(&steppers[i]);
    }
}

void Timer1_Init(void) {
    // Step 1: Enable the clock for Timer 1
    RCC->APBENR2 |= RCC_APBENR2_TIM1EN;  // Enable TIM1 clock

    // Step 2: Configure Timer 1
    TIM1->PSC = 64;
    TIM1->ARR = 100;
    TIM1->DIER |= TIM_DIER_UIE; // Enable update interrupt

    // Step 3: Enable Timer 1
    TIM1->CR1 |= TIM_CR1_CEN; // Enable the timer

    // Step 4: Enable the interrupt in NVIC
    NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn); // Enable Timer 1 interrupt in NVIC
}

// Timer 1 Update Interrupt Handler
void TIM1_BRK_UP_TRG_COM_IRQHandler(void) {
    SEGGER_SYSVIEW_RecordEnterISR();
    if (TIM1->SR & TIM_SR_UIF) { // Check if the update interrupt flag is set
        TIM1->SR &= ~TIM_SR_UIF; // Clear the update interrupt flag

        //SEGGER_SYSVIEW_PrintfHost("do step");
        do_steps();
    }
    SEGGER_SYSVIEW_RecordExitISR();
}

void do_steps(){
    static uint32_t timer = 0;
    timer++;

    for(uint8_t i=0; i<(sizeof(steppers)/sizeof(stepper_T)); i++){
        if((steppers[i].ramp_to_speed != 0 && steppers[i].next_step == timer) || steppers[i].trigger_step){
            /* reset trigger */
            steppers[i].trigger_step = 0;

            /* ramp speed */
            if(steppers[i].speed < steppers[i].ramp_to_speed)
               steppers[i].speed++;
            if(steppers[i].speed > steppers[i].ramp_to_speed)
               steppers[i].speed--;

            /* limit max speed */
            if(steppers[i].speed >= STEPPERS_MAX_SPEED)
               steppers[i].speed=STEPPERS_MAX_SPEED;
            if(steppers[i].speed <= -STEPPERS_MAX_SPEED)
               steppers[i].speed=-STEPPERS_MAX_SPEED;

            /* get direction from sign */
            if(steppers[i].speed>0)
                steppers[i].dir_port->BSRR |= 1 << steppers[i].dir_pin;
            if(steppers[i].speed<0)
                steppers[i].dir_port->BSRR |= 1 << (steppers[i].dir_pin + 16);

            // avoid division by zero
            if(steppers[i].speed != 0){
                /* calculate next step time 
                 *
                 * should be safe for 1 overflow; speeds which
                 * require longer wait times than uint32_max not supported
                 * */
                steppers[i].next_step = timer + (1000/abs(steppers[i].speed));

                /* step and count position */
                steppers[i].step_port->ODR ^= 1 << steppers[i].step_pin;
                if(steppers[i].speed>0)
                    steppers[i].pos++;
                if(steppers[i].speed<0)
                    steppers[i].pos--;

            }
            else{
                // dont send step at speed == 0
                steppers[i].next_step = timer + 1;
            }

            printf("motor[%d]: pos %ld\tspeed %d\n", i, steppers[i].pos, steppers[i].speed);
        }

        if(steppers[i].homed){
            static uint16_t old_ramp_to_speed;
            old_ramp_to_speed = steppers[i].ramp_to_speed;

            int32_t swapped_val = rxBuffer[2*i+1]<<8 | (rxBuffer[2*i+2]&0xFF);
            steppers[i].ramp_to_speed = swapped_val - steppers[i].pos;

            if(old_ramp_to_speed == 0 && steppers[i].ramp_to_speed != 0)
                steppers[i].trigger_step = 1;
        }
        else{
            if(~(steppers[i].home_port->IDR) & (1 << steppers[i].home_pin)){
                steppers[i].pos = 0;
                steppers[i].homed = 1;
                steppers[i].speed = 0;
                steppers[i].ramp_to_speed = 0;
                printf("homed %d\n", i);
            }
            else{
                if(steppers[i].ramp_to_speed == 0){
                    steppers[i].ramp_to_speed=10;
                    steppers[i].trigger_step = 1; // trigger first step
                }
            }
        }
    }
}