#include "include/path.h"


void path_init(path_t *path)
{
    path->current = path->start;
    path->char_count = 0;
    point_t origin = {.x = 0, .y = 0};
    path_add_vec(path, &origin, PATH_MOVE);
}

void path_add(path_t *path, path_node_t *point){
    list_add(&path->start, &point->list);
}

void path_add_vec(path_t *node, point_t *point, path_modus_t modus){
    path_node_t* new_point_ptr = (path_node_t*) malloc(sizeof(path_node_t));
    new_point_ptr->point = *point;
    new_point_ptr->modus = modus;
    path_add(node, new_point_ptr);
}

void path_add_interpolated(path_t *path, point_t *point, path_modus_t modus)
{
    if(modus == PATH_MOVE){
        path_add_vec(path,point,modus);
        return;
    }
    path_node_t* current_node = (path_node_t*)path->start.next;
    point_t point_diff = point_sub(point, &current_node->point);
    while(point_mag2(&point_diff) > 64 )
    {
        point_t interval = point_interval(&current_node->point, point);
        path_add_interpolated(path, &(interval), modus);
        current_node = (path_node_t*)path->start.next;
        point_diff = point_sub(point, &current_node->point);
    }
    path_add_vec(path, point, modus);
}

void path_add_char(path_t *path, char character)
{
    path_modus_t modus = PATH_MOVE;
    asteroids_char_t char_path;
    if (character >= 'a' && character <= 'z')
    {
        char_path = asteroids_font[(uint8_t)character - 0x40];
    } else {
        char_path = asteroids_font[(uint8_t)character - 0x20];
    }
    for(int i = 0; i<8; i++)
    {
        uint8_t char_point = char_path.points[i];
        if (char_point == FONT_LAST){
            path->char_count++;
            return;
        }
        if (char_point == FONT_UP){
            modus = PATH_MOVE;
            continue;
        }
        point_t point = {
            .x = ((char_point >> 4) & 0xF) * STEPMULTI_X + CHAR_OFFSET_X * path->char_count,
            .y = ((char_point >> 0) & 0xF) * STEPMULTI_Y + CHAR_OFFSET_Y
        };
        path_add_interpolated(path, &point, modus);
        modus = PATH_DRAW;
    }
    path->char_count++;
}

void path_reset(path_t *path)
{
    path->current = path->start;
}

/*void path_add_circle(path_t *node, gbVec2 *point, float radius, uint8_t division)
{
    
}

void path_add_arc(path_t *node, gbVec2 *point, float radius, float angle_start, float angle_end, uint8_t division)
{

}
*/

void path_reverse(path_t *path){
    if (path->start.next == 0x0)
    {
        return;
    }
    list_node_t* curr_node = path->start.next;
    list_node_t* next_node = NULL;
    list_node_t* prev_node = NULL;
    while(curr_node){
        next_node = curr_node->next;
        curr_node->next = prev_node;
        prev_node = curr_node;
        curr_node = next_node;
    }
    path->start.next = prev_node;
}

void path_increment(path_t *path)
{
    if (path->current.next != NULL){
        path_node_t* point = path_next_point(path);
        path->current = point->list;
    }
}

path_node_t* path_next_point(path_t *path)
{
    return (path_node_t*) path->current.next;
}

path_node_t* path_next(path_node_t *point){
    return (path_node_t*) point->list.next;
}
    
path_node_t* path_pop(path_node_t *point){
    return (path_node_t*) list_remove_head(&point->list);
}

/*gbVec2 path_next_setpoint(path_t *path, gbVec2 *current_position, float distance){
    gbVec2 position_error;
    gb_vec2_sub(&position_error, control->setpoint, current_position);
    float error_magnitude = gb_vec2_mag(&position_error);
}*/

point_t point_sub(point_t *a, point_t *b)
{
    point_t point = {
        .x = a->x - b->x,
        .y = a->y - b->y
    };
    return point;
}

point_t point_add(point_t *a, point_t *b)
{
    point_t point = {
        .x = a->x + b->x,
        .y = a->y + b->y
    };
    return point;
}

point_t point_interval(point_t *a, point_t*b)
{
    point_t point = {
        .x = a->x + (b->x - a->x)/2,
        .y = a->y + (b->y - a->y)/2
    };
    return point;
}

int16_t point_mag2(point_t *a)
{
    return a->x * a->x + a->y * a->y;
}