#include <stdlib.h>
#include <stdio.h>
#define GB_MATH_IMPLEMENTATION
#include "include/gb_math.h"

#include "include/stepper.h"
#include "include/path.h"
#include "include/stepper_params.h"
#include "include/sensor.h"
#include "include/controller.h"
#include "xtimer.h"
#include "thread.h"
#include "servo.h"
#include "thread_flags.h"

#define STACKSIZE               THREAD_STACKSIZE_DEFAULT
#define PRIO                    (THREAD_PRIORITY_MAIN - 1)


static char stack[STACKSIZE];
//static event_queue_t queue;

void test_velocity(stepper_t *stepper, int steps){
    //float angle;
    stepper_set_direction(stepper, STEPPER_FORWARD);
    for ( int i = 0; i < steps; i++)
    {
        stepper_step(stepper);
        xtimer_usleep(400);
        //stepper_get_angle(stepper, &angle);
        //printf("Angle: %f | Steps: %i\n",angle,i);
    }
    printf("reverse\n");
    stepper_set_direction(stepper, STEPPER_REVERSE);
    for ( int i = 0; i < steps; i++)
    {
        stepper_step(stepper);
        xtimer_usleep(400);
        //stepper_get_angle(stepper, &angle);
        //printf("Angle: %f | Steps: %i\n",angle,i);
    }
}

typedef struct{
    event_t event;
    thread_t *thread;
    thread_flags_t flags;
} test_t;

static void *claiming_thread(void *arg)
{
    event_queue_t *dqs = arg;

    event_queue_claim(dqs);

    event_loop(dqs);

    return NULL;
}

int main(void)
{
    event_queue_t queue = EVENT_QUEUE_INIT_DETACHED;
    event_queue_init_detached(&queue);
    thread_create(stack, sizeof(stack), PRIO, 0, claiming_thread, &queue, "ct");

    thread_t* main_thread = thread_get_active();

    servo_t servo;
    servo_init(&servo, PWM_DEV(0), 2, 0, 1000);


    path_t path;
    path_init(&path);
    path_add_char(&path, 'S');
    path_add_char(&path, 'Q');
    path_add_char(&path, 'X');
    point_t point1= {.x = 72, .y = 89};
    path_add_vec(&path, &point1, PATH_MOVE);
    point_t point2= {.x = 72, .y = 89};
    path_add_vec(&path, &point2, PATH_MOVE);
    path_reverse(&path);
    path_reset(&path);
    
    // Set stepper A
    stepper_t stepper_a;
    stepper_a.queue = &queue;
    stepper_a.thread = main_thread;
    stepper_a.flags = 0x1;
    stepper_a.id = 'A';
    stepper_init(&stepper_a, &stepper_params[0]);

    // Set stepper B
    stepper_t stepper_b;
    stepper_b.queue = &queue;
    stepper_b.thread = main_thread;
    stepper_b.flags = 0x2;
    stepper_b.id = 'B';
    stepper_params_t stepper_params_b;
    memcpy(&stepper_params_b, &stepper_params[0], sizeof(stepper_params_t));
    stepper_params_b.dir_pin = GPIO_PIN(1,1);
    stepper_params_b.step_pin = GPIO_PIN(1,2);
    stepper_params_b.enable_pin = GPIO_PIN(1,12);
    stepper_init(&stepper_b, &stepper_params_b);

    controller_t controller_scara;
    controller_scara.stepper_a = &stepper_a;
    controller_scara.stepper_b = &stepper_b;
    controller_scara.servo = &servo;
    controller_scara.time_step = 4000;
    controller_scara.path = &path;
    controller_init(&controller_scara);
    controller_loop(&controller_scara);
    
    return 0;
}


int test_registers(stepper_t *dev) {
    xtimer_usleep(10000);
    stepper_read(dev, 0x0);
    xtimer_usleep(10000);
    stepper_read(dev, 0x6C);
    xtimer_usleep(10000);
    stepper_read(dev, 0x70);
    xtimer_usleep(10000);
    return STEPPER_OK;
}