/* 
 * File:   locomotion.c
 * Author: Christian
 *
 * Created on 8 de julio de 2019, 10:59
 */

#include "main.h"

/*====Sistema de lista de espera====*/
#define WL_max 12
WL_OBJ WL_emptyObjects[WL_max];
WL_OBJ* WL_emptyFirst = NULL;
WL_OBJ* WL_emptyLast = NULL;
void WL_init(void){
    
    WL_emptyFirst = &WL_emptyObjects[0];
    WL_emptyLast = &WL_emptyObjects[WL_max-1];
    
    int i;
    for(i=1;i<WL_max;i++){
        WL_emptyObjects[i-1].next = &WL_emptyObjects[i];
        WL_emptyObjects[i].prev = NULL;
    }
}
void WL_emptyPush(WL_OBJ* newEmp){
    newEmp->next->prev = NULL;
    if(WL_emptyFirst == NULL){
        newEmp->next = NULL;
        WL_emptyFirst = WL_emptyLast = newEmp;
    }else{
        WL_emptyLast->next = newEmp;
        WL_emptyLast = newEmp;
    }
}
WL_OBJ* WL_emptyPop(void){
    WL_OBJ* ret = WL_emptyFirst;
    if(ret != NULL){
        WL_emptyFirst = WL_emptyFirst->next;
        ret->next = NULL;
    }
    return ret;
}

WL_OBJ* WL_NextMilestone;
unsigned char WL_length = 0;
bool WL_isON = true;
WL_OBJ* WL_findIndex(int i){
    if(i < WL_length){
        WL_OBJ* find = WL_NextMilestone;
        while(i--){
            find = find->next;
        }
        return find;
    }
    return 0;
}
void WL_insert(WL_OBJ *new, WL_OBJ *cur){
    new->next = cur->next;
    cur->next = new;
    new->next->prev = new;
    new->prev = cur;
}
void WL_deleteAllMilestones(){
    WL_isON = false;
    while(WL_length){
        
        WL_OBJ *aux = WL_NextMilestone->next;
        WL_emptyPush(WL_NextMilestone);
        WL_length--;
        
        WL_NextMilestone = aux;
    }
    WL_isON = true;
}
void WL_registerMileStone(uint32_t time, ACTIONLISTS al, unsigned char estado/*new milestone*/)
{
    WL_OBJ* new = WL_emptyPop();
    if(new == NULL){
        return;
    }
    new->time = tmr5_obj.count + time;
    new->al = al;
    new->estado = estado;
    
    if (!WL_length){
        WL_NextMilestone = new;
        WL_length = 1;
    }else{
        unsigned char dicotoIndex = WL_length/2;
        WL_OBJ* current = WL_findIndex(dicotoIndex);
        while(true){
            if(new->time >= current->time){
                if(new->time == current->time){
                    WL_insert(new, current);
                    WL_length++;
                    break;
                }else{
                    if(current->next != NULL){
                        if(new->time < current->next->time){
                            WL_insert(new, current);
                            WL_length++;
                            break;
                        }else{
                            current = current->next;
                        }
                    }else{
                        /* insert especial (por la izquierda) */
                        current->next = new;
                        new->prev = current;
                        WL_length++;
                        break;
                    }
                }
            }else{
                if(current->prev != NULL){
                    current = WL_findIndex((dicotoIndex/=2));
                }else{
                    /* insert especial (por la izquierda) */
                    WL_NextMilestone = new;
                    new->next = current;
                    current->prev = new;
                    WL_length++;
                    break;
                }
            }
        }
    }
    return;
}
/*==================================*/

void Time_watch(void){
    if(WL_length > 0 && tmr5_obj.count >= WL_NextMilestone->time){
        ACTIONLISTS al = WL_NextMilestone->al;
        unsigned char estado = WL_NextMilestone->estado;
        
        WL_OBJ *aux = WL_NextMilestone->next;
        WL_emptyPush(WL_NextMilestone);
        WL_length--;
        
        WL_NextMilestone = aux;
        
        if(WL_isON){actionList(al,estado);}
    }
}

#define DWL_max 10
DWL_OBJ DWL_emptyObjects[DWL_max];
DWL_OBJ* DWL_emptyFirst = NULL;
DWL_OBJ* DWL_emptyLast = NULL;
void DWL_init(void){
    
    DWL_emptyFirst = &DWL_emptyObjects[0];
    DWL_emptyLast = &DWL_emptyObjects[DWL_max-1];
    
    int i;
    for(i=1;i<DWL_max;i++){
        DWL_emptyObjects[i-1].next = &DWL_emptyObjects[i];
        DWL_emptyObjects[i].prev = NULL;
    }
}
void DWL_emptyPush(DWL_OBJ* newEmp){
    newEmp->next->prev = NULL;
    if(DWL_emptyFirst == NULL){
        newEmp->next = NULL;
        DWL_emptyFirst = DWL_emptyLast = newEmp;
    }else{
        DWL_emptyLast->next = newEmp;
        DWL_emptyLast = newEmp;
    }
}
DWL_OBJ* DWL_emptyPop(void){
    DWL_OBJ* ret = DWL_emptyFirst;
    if(ret != NULL){
        DWL_emptyFirst = DWL_emptyFirst->next;
        ret->next = NULL;
    }
    return ret;
}

DWL_OBJ* DWL_NextMilestone;
unsigned char DWL_length = 0;
bool DWL_isON = true;
bool DWL_unacomplished;
DWL_OBJ* DWL_findIndex(int i){
    if(i < DWL_length){
        DWL_OBJ* find = DWL_NextMilestone;
        while(i--){
            find = find->next;
        }
        return find;
    }
    return 0;
}
void DWL_insert(DWL_OBJ *new, DWL_OBJ *cur){
    new->next = cur->next;
    cur->next = new;
    new->next->prev = new;
    new->prev = cur;
}
void DWL_deleteAllMilestones(){
    DWL_isON = false;
    while(DWL_length){
        
        DWL_OBJ *aux = DWL_NextMilestone->next;
        DWL_emptyPush(DWL_NextMilestone);
        DWL_length--;
        
        DWL_NextMilestone = aux;
    }
    DWL_isON = true;
}
void DWL_registerMileStone(int32_t distance, ACTIONLISTS al, unsigned char estado)
{
    DWL_OBJ* new = DWL_emptyPop();
    if(new == NULL){
        return;
    }
    new->distance = distance;
    new->al = al;
    new->estado = estado;
    
    if (!DWL_length){
        DWL_NextMilestone = new;
        DWL_length = 1;
        DWL_unacomplished = false;
    }else{
        DWL_OBJ* current = DWL_NextMilestone;
        while(true){
            if(current->next != NULL){
                current = current->next;
            }else{
                /* insert especial (por la izquierda) */
                current->next = new;
                new->prev = current;
                DWL_length++;
                break;
            }
        }
        //        unsigned char dicotoIndex = DWL_length/2;
        //        DWL_OBJ* current = DWL_findIndex(dicotoIndex);
        //        while(true){
        //            if(new->distance >= current->distance){
        //                if(new->distance == current->distance){
        //                    DWL_insert(new, current);
        //                    DWL_length++;
        //                    break;
        //                }else{
        //                    if(current->next != NULL){
        //                        if(new->distance < current->next->distance){
        //                            DWL_insert(new, current);
        //                            DWL_length++;
        //                            break;
        //                        }else{
        //                            current = current->next;
        //                        }
        //                    }else{
        //                        /* insert especial (por la izquierda) */
        //                        current->next = new;
        //                        new->prev = current;
        //                        DWL_length++;
        //                        break;
        //                    }
        //                }
        //            }else{
        //                if(current->prev != NULL){
        //                    current = DWL_findIndex((dicotoIndex/=2));
        //                }else{
        //                    /* insert especial (por la izquierda) */
        //                    DWL_NextMilestone = new;
        //                    new->next = current;
        //                    current->prev = new;
        //                    DWL_length++;
        //                    break;
        //                }
        //            }
        //        }
    }
    return;
}

void Locomotion_watch(){
    if(DWL_length > 0){
        if(DWL_unacomplished){
            if((DWL_NextMilestone->distance>0)? (*distanceWatch >= DWL_NextMilestone->distance) : (*distanceWatch <= DWL_NextMilestone->distance)){
                DWL_OBJ *aux = DWL_NextMilestone->next;
                DWL_emptyPush(DWL_NextMilestone);
                DWL_length--;
                
                DWL_NextMilestone = aux;
                
                DWL_unacomplished = false;
            }
        }
        else{
            actionList(DWL_NextMilestone->al,DWL_NextMilestone->estado);
            DWL_unacomplished = true;
        }
    }
}
// Circunferencia = 2*PI*radio
#define RTTION_radius 16.4
#define RTTION_distance 2*PI*RTTION_radius

volatile int32_t *distanceWatch;

/* Params
 * - n  : nš de veces que se recorre el arco especificado (numerador)
 * - arc: arco de circunferencia a recorrer (denominador)
 * - direction: 1 izquierda,
 *              2 derecha, 
 *              + adelante,
 *              - atras
 */
void rotate_x_laps(int x, int arc, char direction){
    
    if (direction != 2 && direction != -2){
        IC1_Sum = 0;
        distanceWatch = &IC1_Sum;
        set_sampling_wheel(-1);
        set_Differential(-1000);
    }else{
        IC2_Sum = 0;
        distanceWatch = &IC2_Sum;
        set_sampling_wheel(1);
        set_Differential(1000);
    }
    change_S_toTurn();
    
    set_SpeedGoal(direction<0?-3:3);
    
    DWL_registerMileStone(cms_to_counts((double)x*(RTTION_distance/arc)),paseito,3);
    
}
void rotate_x_laps2(int x, int arc, char direction){
    
    unsigned char dir;
    switch(direction){
        case -1:
            dir = 5;
            break;
        case 1:
            dir = 4;
            break;
        case -2:
            dir = 7;
            break;
        case 2:
            dir = 6;
            break;
    }
    
    DWL_registerMileStone(cms_to_counts((double)x*(RTTION_distance/arc)),paseito,dir);
    
}

/* Params
 * - x  : distancia a recorrer
 * - speed: velocidad
 */
void go_x_cms(int32_t x, double speed){
    
    set_Differential(0);
    
    IC1_Sum = 0;
    IC2_Sum = 0;
    set_sampling_wheel(0);
    distanceWatch = &IC1_Sum;
    
    change_S_toStraight();
    
    set_SpeedGoal(speed);
    
    DWL_registerMileStone(cms_to_counts(x),paseito,4);
}
void go_x_cms2(int32_t x){
    
    unsigned char aux;
    if(x>0){
        aux = 3;
    }else if (x<0){
        aux = 8;
    }else{
        aux = 9;
    }
    DWL_registerMileStone(cms_to_counts(x),paseito,aux);
}