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

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

stepper_T steppers[] = {
    {.en_port=GPIOC, .en_pin=11, .dir_port=GPIOB, .dir_pin=4,  .step_port=GPIOE, .step_pin=2,  .home_port=GPIOB, .home_pin=14, .home_position=0, .homed = 0},
    {.en_port=GPIOB, .en_pin=3,  .dir_port=GPIOF, .dir_pin=11, .step_port=GPIOF, .step_pin=12, .home_port=GPIOB, .home_pin=13, .home_position=0, .homed = 0},
    {.en_port=GPIOF, .en_pin=10, .dir_port=GPIOD, .dir_pin=6,  .step_port=GPIOD, .step_pin=7,  .home_port=GPIOF, .home_pin=7,  .home_position=0, .homed = 0},
    {.en_port=GPIOD, .en_pin=5,  .dir_port=GPIOD, .dir_pin=2,  .step_port=GPIOD, .step_pin=3,  .home_port=GPIOB, .home_pin=12, .home_position=0, .homed = 0},
    {.en_port=GPIOD, .en_pin=1,  .dir_port=GPIOC, .dir_pin=8,  .step_port=GPIOC, .step_pin=9,  .home_port=GPIOE, .home_pin=7,  .home_position=0, .homed = 0},
    {.en_port=GPIOD, .en_pin=15, .dir_port=GPIOD, .dir_pin=9,  .step_port=GPIOD, .step_pin=11, .home_port=GPIOB, .home_pin=11, .home_position=0, .homed = 0},
    //{.en_port=GPIOA, .en_pin=15, .dir_port=GPIOA, .dir_pin=14, .step_port=GPIOA, .step_pin=10, .home_port=GPIOB, .home_pin=11, .home_position=0, .homed = 1},
};

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));
    stp->home_port->PUPDR &= ~(0x3 << (stp->home_pin * 2));
    stp->home_port->PUPDR |=  (0x1 << (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_GPIOAEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOBEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOCEN;
    RCC->IOPENR |= RCC_IOPENR_GPIODEN;
    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;
    int16_t rotation_speed = 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 >= STEPPER_MAX_SPEED)
               steppers[i].speed=STEPPER_MAX_SPEED;
            if(steppers[i].speed <= -STEPPER_MAX_SPEED)
               steppers[i].speed=-STEPPER_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\trot: %ld\tspeed %ld\n", i, steppers[i].pos, steppers[i].endless_rot_integrator, steppers[i].speed);
            //printf("rotation speed: %d\n", rotation_speed);
        }

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

            uint8_t raw_endless = rxBuffer[3*i + DMX_ADDRESS + 2]; 

            /* deadzone on top and bottom */
            if(raw_endless >= 5 && raw_endless <= 250) {
                rotation_speed = raw_endless - 127;

                /* dead zone in the middle */
                if(rotation_speed >= 5)
                    rotation_speed -= 5;
                else if(rotation_speed <= -5)
                    rotation_speed += 5;
                else
                    rotation_speed = 0;
            }
            else{
                rotation_speed = 0;
            }

            if(steppers[i].pos == STEPPER_STEPS_PER_ROTATION+1){
                steppers[i].pos = 1;
            }
            if(steppers[i].pos == -1){
                steppers[i].pos = STEPPER_STEPS_PER_ROTATION-1;
            }

            ///* handle endless rotation offset and wraparound */
            //if(rotation_speed){
            //    steppers[i].endless_rot_integrator += rotation_speed;
            //    if(steppers[i].endless_rot_integrator >= STEPPER_STEPS_PER_ROTATION)
            //    {
            //        steppers[i].endless_rot_integrator -= STEPPER_STEPS_PER_ROTATION;
            //        steppers[i].pos -= STEPPER_STEPS_PER_ROTATION;
            //    }
            //    else if(steppers[i].endless_rot_integrator <= 0)
            //    {
            //        steppers[i].endless_rot_integrator += STEPPER_STEPS_PER_ROTATION;
            //        steppers[i].pos += STEPPER_STEPS_PER_ROTATION;
            //    }
            //}
            //else
            //    steppers[i].endless_rot_integrator = 0;

            if(rotation_speed){
                steppers[i].ramp_to_speed = rotation_speed;
            }
            else{
                int32_t swapped_val = rxBuffer[3*i + DMX_ADDRESS]<<8 | (rxBuffer[3*i+DMX_ADDRESS+1]&0xFF);
                //swapped_val += steppers[i].endless_rot_integrator;
                int32_t scaled_steps = swapped_val * STEPPER_STEPS_PER_ROTATION / UINT16_MAX;
                steppers[i].ramp_to_speed = scaled_steps - steppers[i].pos;
                //static int cnt = 0;
                //if(i==5 && ++cnt==10000){
                //    printf("swapped_val[%d]: %d\n", i, swapped_val);
                //    cnt=0;
                //}
            }

            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 = steppers[i].home_position;
                steppers[i].homed = 1;
                steppers[i].speed = 0;
                steppers[i].ramp_to_speed = 0;
                printf("homed %d\n", i);
            }
            else if(steppers[i].pos == 2*STEPPER_STEPS_PER_ROTATION){
                steppers[i].pos = 0;
                steppers[i].homed = 1;
                steppers[i].speed = 0;
                steppers[i].ramp_to_speed = 0;
                printf("cant home %d, setting 0 now\n", i);
            }
            else{
                if(steppers[i].ramp_to_speed == 0){
                    steppers[i].ramp_to_speed=80;
                    steppers[i].trigger_step = 1; // trigger first step
                }
            }
        }
    }
}