#include <angles.h>
#include <solver.h>
#include <nav.h>



unsigned char IR_Threshold[6];
int Nav_MovingForwards;
int Nav_Busy;
int Nav_WallOnLeft,Nav_WallOnRight;
int IR_FrontCentre,IR_RearCentre;
Path *Nav_CurPath;
Exits Nav_Exits;
Exits Nav_CurExits;
Dir Nav_CurDir;

void navmain()
{
    Nav_MovingForwards = 1;
    Nav_CurDir = DIR_N;
    Nav_Busy=0;
    Nav_CurPath=NULL;
    Nav_Exits=0;
    SetMotorSpeed(0,0);
    IR_FrontCentre=0xCF;
    IR_RearCentre=0xCF;

    while (1) {
        while (!Nav_CurPath)
            idle();
        IR_on();
        Nav_Busy=1;
        Nav_ExecPath();
        IR_off();
        Nav_Busy=0;
    }



}


Exits MC_Move(Path *P) {
    Nav_CurPath=Path_COPY(P);
    printf("MC_Move Called.\n",0);
    Print(P);
    while (Nav_Busy)
        idle();
    while (Nav_CurPath)
        idle();
    return Nav_Exits;
}

void Nav_ExecPath() {
    int MoveCount;
    int CurMove,cur_x,cur_y;
    int MoveLen,MoveDir;
    int i;
    Move M;

    MoveCount=Path_NumberOfMoves(Nav_CurPath);

    cur_x=X*NAV_CELL_SIZE;
    cur_y=Y*NAV_CELL_SIZE;

    for (CurMove=0;CurMove<MoveCount;CurMove++) {
        int dx,dy;
        int dx1,dy1;
        M = Path_MoveNo(Nav_CurPath,CurMove);
        MoveDir=M&MOVE_DIRMASK;
        MoveLen=M&MOVE_LENMASK;

        if ((Nav_CurDir^DIR_REVERSE)==(Dir)MoveDir)
            Nav_MovingForwards=!Nav_MovingForwards;
        Nav_CurDir=(Dir)MoveDir;


        dx=DX[DIRNUM(M)]*NAV_CELL_SIZE;
        dy=DY[DIRNUM(M)]*NAV_CELL_SIZE;


        for (i=1;i<MoveLen;i++) {
            Nav_MoveTo(cur_x+(dx/2),cur_y+(dy/2));
            cur_x+=dx;
            cur_y+=dy;
            Nav_MoveTo(cur_x,cur_y);
            Nav_ErrorCorrect();
        }

        Nav_MoveTo(cur_x+(dx/2),cur_y+(dy/2));
        Nav_CurExits=Nav_GetExits();
        Nav_ErrorCorrect();
        if (CurMove==(MoveCount-1))  {
            Nav_Exits=Nav_CurExits;
            Path_KILL(&Nav_CurPath);
        }

        dx1=DX[DIRNUM(M)]*NAV_CELL_STOPPOS;
        dy1=DY[DIRNUM(M)]*NAV_CELL_STOPPOS;
        Nav_MoveTo(cur_x+dx1,cur_y+dy1);
        cur_x+=dx;
        cur_y+=dy;

    }
    SetMotorSpeed(0,0);
}

Exits Nav_GetExits() {
    int Front,Left,Right;
    if (Nav_MovingForwards) {
        Front=Analog(0)>IR_Threshold[0];
        Left=Analog(1)>IR_Threshold[1];
        Right=Analog(2)>IR_Threshold[2];
    } else {
        Front=Analog(3)>IR_Threshold[3];
        Left=Analog(5)>IR_Threshold[5];
        Right=Analog(4)>IR_Threshold[4];
    }
    Nav_WallOnLeft=!Left;
    Nav_WallOnRight=!Right;


    switch (Nav_CurDir) {
        case DIR_N:
            return EXIT_S+(Front?EXIT_N:0)+(Left?EXIT_W:0)+(Right?EXIT_E:0);
        case DIR_S:
            return EXIT_N+(Front?EXIT_S:0)+(Left?EXIT_E:0)+(Right?EXIT_W:0);
        case DIR_E:
            return EXIT_W+(Front?EXIT_E:0)+(Left?EXIT_N:0)+(Right?EXIT_S:0);
        case DIR_W:
            return EXIT_E+(Front?EXIT_W:0)+(Left?EXIT_S:0)+(Right?EXIT_N:0);
    }
    return 0;
}


void Nav_ErrorCorrect() {
    if (Nav_WallOnLeft&&Nav_WallOnRight) {
        int Error,XError,YError,AngError;
        int n1,n2;

        if (Nav_MovingForwards) {
            n1=Analog(1);
            n2=Analog(2);
        } else {
            n1=Analog(5);
            n2=Analog(4);
        }

        IR_FrontCentre=(IR_FrontCentre*14+n1+n2)/16;
        Error=(n1-n2)/12;

        XError=Error*(DY[DIRNUM(Nav_CurDir)]);
        YError=Error*(-DX[DIRNUM(Nav_CurDir)]);
        AngError=-Error;
        OPT_Correct(XError,YError,AngError);
    } else {
        if (Nav_WallOnLeft) {
            int Error,XError,YError,AngError;
            int n1;

            if (Nav_MovingForwards) {
                n1=Analog(1);
                Error=(n1-IR_FrontCentre)/10;
            } else {
                n1=Analog(5);
                Error=(n1-IR_RearCentre)/10;
            }

            XError=Error*(DY[DIRNUM(Nav_CurDir)]);
            YError=Error*(-DX[DIRNUM(Nav_CurDir)]);
            AngError=-Error;
            OPT_Correct(XError,YError,AngError);
        }
        if (Nav_WallOnRight) {
            int Error,XError,YError,AngError;
            int n1;

            if (Nav_MovingForwards) {
                n1=Analog(2);
                Error=(IR_FrontCentre-n1)/10;
            } else {
                n1=Analog(4);
                Error=(IR_RearCentre-n1)/10;
            }

            XError=Error*(DY[DIRNUM(Nav_CurDir)]);
            YError=Error*(-DX[DIRNUM(Nav_CurDir)]);
            AngError=-Error;
            OPT_Correct(XError,YError,AngError);
        }
    }




}






void Nav_MoveTo(int x,int y) {
    IR_on();
    if (Nav_MovingForwards)
        switch(Nav_CurDir) {
        case DIR_N:
            move_FWD_N(y,x);
            break;
        case DIR_E:
            move_FWD_E(x,y);
            break;
        case DIR_W:
            move_FWD_W(x,y);
            break;
        case DIR_S:
            move_FWD_S(y,x);
            break;
        }
    else
        switch(Nav_CurDir)
        {
        case DIR_N:
            move_REV_N(y,x);
            break;
        case DIR_E:
            move_REV_E(x,y);
            break;
        case DIR_W:
            move_REV_W(x,y);
            break;
        case DIR_S:
            move_REV_S(y,x);
            break;
        }

}


void move_FWD_N(int dest_Y,int centre_X) {
    int dif;
    while (OPT_Y_Pos()<dest_Y) {
        dif=sgn(OPT_X_Pos()-centre_X,SGN_COEFF);
        dif+=DIST_COEFF*(OPT_X_Pos()-centre_X);
        dif-=ANG_COEFF*Sin127(OPT_Angle());
        SetMotorSpeed(400-dif,400+dif);
    }
}

void move_FWD_E(int dest_X,int centre_Y) {
    int dif;
    while (OPT_X_Pos()<dest_X) {
        dif=sgn(centre_Y-OPT_Y_Pos(),SGN_COEFF);
        dif+=DIST_COEFF*(centre_Y-OPT_Y_Pos());
        dif-=ANG_COEFF*Cos127(OPT_Angle());
        SetMotorSpeed(400-dif,400+dif);
    }
}

void move_FWD_S(int dest_Y,int centre_X) {
    int dif;
    while (OPT_Y_Pos()>dest_Y) {
        dif=sgn(centre_X-OPT_X_Pos(),SGN_COEFF);
        dif+=DIST_COEFF*(centre_X-OPT_X_Pos());
        dif+=ANG_COEFF*Sin127(OPT_Angle());
        SetMotorSpeed(400-dif,400+dif);
    }
}


void move_FWD_W(int dest_X,int centre_Y) {
    int dif;
    while (OPT_X_Pos()>dest_X) {
        dif=sgn(OPT_Y_Pos()-centre_Y,SGN_COEFF);
        dif+=DIST_COEFF*(OPT_Y_Pos()-centre_Y);
        dif+=ANG_COEFF*Cos127(OPT_Angle());
        SetMotorSpeed(400-dif,400+dif);
    }
}

void move_REV_N(int dest_Y,int centre_X) {
    int dif;
    while (OPT_Y_Pos()<dest_Y) {
        dif=+sgn(OPT_X_Pos()-centre_X,SGN_COEFF);
        dif+=DIST_COEFF*(OPT_X_Pos()-centre_X);
        dif+=ANG_COEFF*Sin127(OPT_Angle());
        SetMotorSpeed(-400-dif,-400+dif);
    }
}

void move_REV_E(int dest_X,int centre_Y) {
    int dif;
    while (OPT_X_Pos()<dest_X) {
        dif=sgn(centre_Y-OPT_Y_Pos(),SGN_COEFF);
        dif+=DIST_COEFF*(centre_Y-OPT_Y_Pos());
        dif+=ANG_COEFF*Cos127(OPT_Angle());
        SetMotorSpeed(-400-dif,-400+dif);
    }
}

void move_REV_S(int dest_Y,int centre_X) {
    int dif;
    while (OPT_Y_Pos()>dest_Y) {
        dif=+sgn(centre_X-OPT_X_Pos(),SGN_COEFF);
        dif+=DIST_COEFF*(centre_X-OPT_X_Pos());
        dif-=ANG_COEFF*Sin127(OPT_Angle());
        SetMotorSpeed(-400-dif,-400+dif);
    }
}


void move_REV_W(int dest_X,int centre_Y) {
    int dif;
    while (OPT_X_Pos()>dest_X) {
        dif=+sgn(OPT_Y_Pos()-centre_Y,SGN_COEFF);
        dif+=DIST_COEFF*(OPT_Y_Pos()-centre_Y);
        dif-=ANG_COEFF*Cos127(OPT_Angle());
        SetMotorSpeed(-400-dif,-400+dif);
    }
}