#include "include/controller.h"

int controller_init(controller_t *control)
{
    gpio_init(BTN0_PIN,BTN0_MODE);
    controller_home(control);
    control->flags = control->stepper_a->flags | control->stepper_b->flags;
    return 0;
}

int controller_loop(controller_t *control){
    path_node_t *setpoint = (path_node_t*)control->path->current.next;
    //printf("start controller loop\n");
    while(setpoint->list.next != 0x0){
        if ((setpoint->point.x > 127) | (setpoint->point.x < 0) | (setpoint->point.y > 127) | (setpoint->point.y < 0 ) ) {
            printf("Controller Error, x and y value are out of range x: %i, y: %i\n", setpoint->point.x, setpoint->point.y);
            return CONTROLLER_ERR;
        }
        if (setpoint->modus == PATH_MOVE) {
            controller_raise_marker(control);
        } else {
            controller_lower_marker(control);
        }
        //printf("Controller: look up x: %i, y: %i\n", setpoint->point.x, setpoint->point.y);
        int16_t angle1 = lookup_table_angle1[setpoint->point.y][setpoint->point.x];
        int16_t angle2 = lookup_table_angle2[setpoint->point.y][setpoint->point.x];
        //printf("Controller: next setpoint A: %i, B: %i\n",angle1, angle2);
        stepper_set_setpoint(control->stepper_a, angle1, control->time_step);
        stepper_set_setpoint(control->stepper_b, angle2, control->time_step);
        xtimer_usleep(10000);
        thread_flags_wait_all(control->flags);
        setpoint = path_next(setpoint);
    }
    printf("controller loop done\n");
    return CONTROLLER_OK;
}

int controller_home(controller_t *control){
    stepper_disable(control->stepper_b);
    stepper_disable(control->stepper_a);
    while(gpio_read(BTN0_PIN)!=0)
    {
    }
    xtimer_usleep(1000000);
    stepper_enable(control->stepper_b);
    stepper_set_direction(control->stepper_b,STEPPER_FORWARD);
    while(gpio_read(BTN0_PIN)!=0){
        stepper_step(control->stepper_b);
        xtimer_usleep(3000);
    }
    stepper_enable(control->stepper_a);
    stepper_set_direction(control->stepper_a,STEPPER_REVERSE);
    stepper_set_direction(control->stepper_b,STEPPER_REVERSE);
    uint32_t homing_b_steps = stepper_get_count(control->stepper_a,1.92);
    uint32_t homing_a_steps= stepper_get_count(control->stepper_b,0.75);
    uint32_t homing_combined = homing_b_steps - homing_a_steps;
    for(uint32_t i = 0; i < homing_combined; i++)
    {
        stepper_step(control->stepper_b);
        xtimer_usleep(1000);
    }
    stepper_enable(control->stepper_b);
    for(uint32_t i = 0; i < homing_a_steps; i++)
    {
        stepper_step(control->stepper_b);
        stepper_step(control->stepper_a);
        xtimer_usleep(1000);
    }
    control->stepper_a->step_count = stepper_get_count(control->stepper_a,GB_MATH_PI/2.0);
    control->stepper_b->step_count = 0;
    float angle_a;
    float angle_b;
    stepper_get_angle(control->stepper_a, &angle_a);
    stepper_get_angle(control->stepper_b, &angle_b);
    //printf("angle1: %f, angle2 %f\n", angle1, angle2);
    printf("Angle A: %f, B: %f\n", angle_a, angle_b);
    while(gpio_read(BTN0_PIN)!=0)
    {
    }
    xtimer_usleep(500000);
    return 0;
}

void controller_raise_marker(controller_t *control)
{
    servo_set(control->servo,SERVO_RAISED);
    if(!control->lifted){
        xtimer_usleep(100000);
        control->lifted = true;
    }
}
    
void controller_lower_marker(controller_t *control)
{
    servo_set(control->servo,SERVO_LOWERED);
    if(control->lifted){
        xtimer_usleep(100000);
        control->lifted = false;
    }
}