#include "..\kernel\angles.h"
#include <stdbran.h>
#include "printf.c"
/*#include "..\lcd\lcd.h"*/
#include <solver.h>

#include <nav.h>

int abs(int a)
{
        return (a>0?a:-a);
}

int Debug;

extern char LCD_Buffers[10][16];

extern const char DIRNAME[4];

Print(Path *P) {
	int i;
    printf("Path ",0);
    if (P) {
        if (P->MoveList) {
            for(i=0;i<P->MoveCount;i++) {
                printf("%c%d ",DIRNAME[P->MoveList[i] >> 6],P->MoveList[i]&MOVE_LENMASK);
            }
        } else
            printf("<empty>",0);
        printf("\n",0);
    } else
        printf("<NULL>\n",0);
}

DumpMaze(Maze *M) {
    Node *N;
    Node *N1;

    printf("Maze %x  Free memory=%x\n",(int)M,FreeMemory());
    N=M->FirstNode;
    while (N) {
        int i;

        printf(" Node %x (%d,%d)->\n",(int)N,N->XPos,N->YPos);
        i=-1;
        while (++i<N->LinkCount) {
            N1=N->LinkList[i];
            if (N1)
                printf("  Node %x (%d,%d) via ",(int)N1,N1->XPos,N1->YPos);
            else
                printf("  Node <NULL> via ",0);
            Print(N->PathList[i]);
        }
    N=N->Next;
    }
}



int main() {
    int i;

    for (i=0;i<20000;i++); /* delay loop to ensure keypad has been read */
    Debug=(keypad()==4);

    if (Debug) {
        LCD_Print(1,"Debug ON:       ",0);
        LCD_BufferSwap(1);
		while (keypad()==4);
	}

    LCD_Print(1,"Press F4        ",0);
	LCD_BufferSwap(1);

	while (keypad()!=4);
    SetupSensors();
    OPT_Reset();

    if (!fork(500)) {
        Solver_Go();
        return 1;
    }

    if (!fork(400)) {
        navmain();
        return 1;
    }

    PM_SetTimeSlice(500); /* 1ms timeslice */
    diagmain();
    return 1;

}