#include "include/controller.h"


void print_point(gbVec2 *point)
{
    printf("position x: %.4f, y: %.4f\n", point->x, point->y);
}

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;
        }
        //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;
}

/*void controller_update(event_t *event)
{
    controller_t *control = container_of(event, controller_t, event);
    controller_calculate_velocity(control);
    event_timeout_set(&(control->event_timeout), control->time_step);
}


int controller_setpoint(controller_t *control, gbVec2 *point)
{
    control->setpoint = *point;
    controller_inverse_kinematics(control, point, &control->angle1_setpoint , &control->angle2_setpoint);
    return 0;
}

void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2)
{
    float phi = gb_arctan2(point->y, point->x);
    float length_a2 = gb_square(control->arm_a);
    float length_b2 = gb_square(control->arm_b);
    float length_c2 = gb_vec2_mag2(*point);
    float length_c = gb_sqrt(length_c2);
    float b = gb_arccos((length_a2 + length_c2 - length_b2) / (2 * control->arm_a * length_c));
    float c = gb_arccos((length_a2 + length_b2 - length_c2) / (2 * control->arm_a * control->arm_b));
    *angle1 = b + phi;
    *angle2 = *angle1 - GB_MATH_PI + c;
}

void controller_current_position(controller_t *control, gbVec2 *point)
{
    float angle_a;
    stepper_get_angle(control->stepper_a, &angle_a);
    float angle_b;
    stepper_get_angle(control->stepper_b, &angle_b);
    gbVec2 joint1 = {
        .x = control->arm_a * gb_cos(angle_a),
        .y = control->arm_a * gb_sin(angle_a)
    };
    gbVec2 joint2 = {
        .x = control->arm_b * gb_cos(angle_b),
        .y = control->arm_b * gb_sin(angle_b)
    };
    gb_vec2_add(point, joint1, joint2);
}


int controller_calculate_velocity(controller_t *control)
{
    uint32_t setpoint_time;
    uint32_t ik_time;
    uint32_t angle_time;
    uint32_t velocity_time;
    uint32_t start = xtimer_now_usec();
    gbVec2 next_setpoint;
    int status = controller_sub_setpoint(control, &next_setpoint, 0.005);
    setpoint_time = xtimer_now_usec();
    float angle1;
    float angle2;
    controller_inverse_kinematics(control, &next_setpoint, &angle1, &angle2);
    ik_time = xtimer_now_usec();
    float angle_a;
    stepper_get_angle(control->stepper_a, &angle_a);
    float angle_b;
    stepper_get_angle(control->stepper_b, &angle_b);
    angle_time = xtimer_now_usec();
    //printf("angle1: %f, angle2 %f\n", angle1, angle2);
    //printf("anglea: %f, angleb %f\n", angle_a, angle_b);
    float omega1 = (angle1-angle_a)*25;
    float omega2 = (angle2-angle_b)*25;
    if (status == CONTROLLER_STATISFIED)
    {
        omega1 = 0.0;
        omega2 = 0.0;
    }
   // printf("omega1: %f, omega2 %f\n", omega1, omega2);
    stepper_set_velocity(control->stepper_a, omega1);
    stepper_set_velocity(control->stepper_b, omega2);
    velocity_time = xtimer_now_usec();
    printf("Setpoint: %lu\n", setpoint_time - start);
    printf("IK-time: %lu\n", ik_time - setpoint_time);
    printf("Angles: %lu\n", angle_time - ik_time);
    printf("velocity: %lu\n", velocity_time - angle_time);
    return 0;
}

int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance)
{
    // TODO: if the length is shorten than the distance, do not scale it up
    // TODO: if the length is zero. Do nothing.
    // get current position.
    gbVec2 current_position;

    controller_current_position(control, &current_position);
    // print_point(&current_position);

    // compare current position with setpoint.
    //
    while(1){
        gbVec2 position_error;
        path_node_t *next_point = path_next_point(control->path);
        //gb_vec2_sub(&position_error, next_point->point, current_position);
        float error_magnitude = gb_vec2_mag(position_error);
        gbVec2 scaled_position_error;
        //printf("Magnitude: %f Error ", error_magnitude);
        //print_point(&position_error);
        if (error_magnitude > distance)
        {
            gb_vec2_div(&scaled_position_error, position_error, error_magnitude/distance);
            gb_vec2_add(setpoint, current_position, scaled_position_error);
            //printf("Next ");
            //print_point(setpoint);
            return CONTROLLER_OK;
        }
        else if (next_point->list.next == NULL)
        {
            scaled_position_error = current_position;
            gb_vec2_add(setpoint, current_position, scaled_position_error);
            //printf("No new point\n");
            if (error_magnitude < distance/8)
            {
                return CONTROLLER_STATISFIED;
            }
            return CONTROLLER_NO_PATH;
        }
        else 
        {
            path_increment(control->path);
            printf("Increment Path\n");
        }
    }
    gbVec2 position_error;
      gb_vec2_sub(&position_error, control->setpoint, current_position);
    // scale the length of the error.
    // to limit the length.
    control->error = gb_vec2_mag(position_error);
    gbVec2 scaled_position_error;
    if (control->error > distance) {
    gb_vec2_div(&scaled_position_error, position_error, control->error/distance);
    } else {
    scaled_position_error = position_error;
    }
    gb_vec2_add(setpoint, current_position, scaled_position_error);
    return 0;
}
*/
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;
}


int controller_lift_marker(controller_t *control, bool lift){
    (void)lift;
    (void)control;
    return 0;
}