/*
  Jaap Scherphuis,  24/01/2004,  jaapsch_at_yahoo_do_com

  Thistlethwaite's algorithm.
*/

#include "main.h"

// get index of cube position from table t
int getposition(int t){
	int i=-1,n=0;
    int corn[8],j,k,l,corn2[4];
	switch(t){
	// case 0 does nothing so returns 0
	case 1://edgeflip
		// 12 bits, set bit if edge is flipped
		for(;++i<12;) n+= GET_ORI(i)<<i;
		break;
	case 2://cornertwist
		// get base 3 number of 8 digits - each digit is corner twist
		for(i=20;--i>11;) n=n*3+GET_ORI(i);
		break;
	case 3://middle edge choice
		// 12 bits, set bit if edge belongs in Um middle slice
		for(;++i<12;) n+= (GET_POS(i)&8)?(1<<i):0;
		break;
	case 4://ud slice choice
		// 8 bits, set bit if UD edge belongs in Fm middle slice
		for(;++i<8;) n+= (GET_POS(i)&4)?(1<<i):0;
		break;
	case 5://tetrad choice, twist and parity
		// 8 bits, set bit if corner belongs in second tetrad.
		// also separate pieces for twist/parity determination
		k=j=0;
		for(;++i<8;)
			if((l=GET_POS(i+12)-12)&4){
				corn[l]=k++;
				n+=1<<i;
			}else corn[j++]=l;
		//Find permutation of second tetrad after solving first
		for(i=0;i<4;i++) corn2[i]=corn[4+corn[i]];
		//Solve one piece of second tetrad
		for(;--i;) corn2[i]^=corn2[0];

		// encode parity/tetrad twist
		n=n*6+corn2[1]*2-2;
		if(corn2[3]<corn2[2])n++;
		break;
	case 6://two edge and one corner orbit, permutation
		numOfPermtonum+=3;
		n=PERMTONUM0()*576+PERMTONUM1()*24+PERMTONUM3();
		//n=PERMTONUM0();
		break;
	case 7://one edge and one corner orbit, permutation
		numOfPermtonum+=2;
		n=PERMTONUM2()*24+PERMTONUM4();
		//n=PERMTONUM1();
		break;
	}
	return n;
}

void filltable(){
	tables[0] = table0;
	tables[1] = table1;
	tables[2] = table2;
	tables[3] = table3;
	tables[4] = table4;
	tables[5] = table5;
	tables[6] = table6;
	tables[7] = table7;
}

// Pruned tree search. recursive.
int searchphase(int movesleft, int movesdone,int lastmove){
    int i,j;
	// prune - position must still be solvable in the remaining moves available
	if( tables[phase  ][getposition(phase  )]-1 > movesleft ||
	    tables[phase+1][getposition(phase+1)]-1 > movesleft ) return 0;

	// If no moves left to do, we have solved this phase
	if(!movesleft) return 1;

	// not solved. try each face move
	for(i=6;i--;){
		// do not repeat same face, nor do opposite after DLB.
		if( i-lastmove && (i-lastmove+1 || (i|1) ) ){
			move[movesdone]=i;
			// try 1,2,3 quarter turns of that face
			for(j=0;++j<4;){
				//do move and remember it
				DOMOVE(i);
				moveamount[movesdone]=j;
				//Check if phase only allows half moves of this face
				if( (j==2 || i>=phase ) &&
					//search on
					searchphase(movesleft-1,movesdone+1,i) ) return 1;
			}
			// put face back to original position.
			DOMOVE(i);
		}
	}
	// no solution found
	return 0;
}

void inputFormatTransform(int colorConfig[6][9])
{
    int i;
    char centerColorPosition[6];
    
    for(i=0; i<6; i++)
        centerColorPosition[colorConfig[i][4] - 1] = CENTER_COLOR_POSITION[i];
    
    printf("The transformed format is:\n");
    
    for(i=0; i<12; i++)
    {
        printf("%c%c ", centerColorPosition[colorConfig[SIDE_INDEX_1[i]][SIDE_INDEX_2[i]]-1],
               centerColorPosition[colorConfig[SIDE_INDEX_3[i]][SIDE_INDEX_4[i]]-1]);
        transformedInputFormat[i][0] = centerColorPosition[colorConfig[SIDE_INDEX_1[i]][SIDE_INDEX_2[i]]-1];
        transformedInputFormat[i][1] = centerColorPosition[colorConfig[SIDE_INDEX_3[i]][SIDE_INDEX_4[i]]-1];
        transformedInputFormat[i][2] = '\0';
    }
    
    for(i=0; i<8; i++)
    {
        printf("%c%c%c ", centerColorPosition[colorConfig[CORNER_INDEX_1[i]][CORNER_INDEX_2[i]]-1],
               centerColorPosition[colorConfig[CORNER_INDEX_3[i]][CORNER_INDEX_4[i]]-1],
               centerColorPosition[colorConfig[CORNER_INDEX_5[i]][CORNER_INDEX_6[i]]-1]);
        transformedInputFormat[i+12][0] = centerColorPosition[colorConfig[CORNER_INDEX_1[i]][CORNER_INDEX_2[i]]-1];
        transformedInputFormat[i+12][1] = centerColorPosition[colorConfig[CORNER_INDEX_3[i]][CORNER_INDEX_4[i]]-1];
        transformedInputFormat[i+12][2] = centerColorPosition[colorConfig[CORNER_INDEX_5[i]][CORNER_INDEX_6[i]]-1];
    }
    printf("\n");
}

int inputColor()
{
    int colorConfig[6][9] = {{0,0,0,0,0,0,0,0,0},   //front
        {0,0,0,0,0,0,0,0,0},       //right
        {0,0,0,0,0,0,0,0,0},       //up
        {0,0,0,0,0,0,0,0,0},       //left
        {0,0,0,0,0,0,0,0,0},       //back
        {0,0,0,0,0,0,0,0,0}        //down
    };
    int colorNum[6] = {0,0,0,0,0,0};
    int centerColorTaken[6] = {0,0,0,0,0,0};
    int ii, jj;
    char *line = (char *)malloc(sizeof(char) * 11);
        
    printf("---------------------------------------------------------------\n");
    printf("Please input the color configuration.\n");
    printf("white = 1, red = 2, blue = 3, orange = 4, green = 5, yellow = 6\n\n");
    
    printf("Sequence: up -> down -> front -> right -> back -> left\n");
    printf("Please input from left to right, top to bottom, e.g. 544462653\n");
    for(ii=1; ii<=6; ii++)
    {
        
        printf("Please input for side %d:", ii);
        SET_BORDER(ii-1);
        fgets(line, 11, stdin);
        if((int)strlen(line) != 10)
        {
            printf("Invalid input.\n");
            ii--;
        }
        else
        {
            for(jj=0; jj<9; jj++)
            {
                if(line[jj] < '1' || line[jj] > '6')
                {
                    printf("Invalid input.\n");
                    ii--;
                    break;
                }
                else
                {
                    colorConfig[ii-1][jj] = (int)(line[jj] - '0');
                }
            }
        }
        int aa;
        for(aa=0;aa<9;aa++)
        {
        	SET_COLOR(ii-1,aa,colorConfig[ii-1][aa]);
        }
    }
    
    for(ii=0; ii<6; ii++)
    {
        for(jj=0; jj<9; jj++)
        {
            if(++colorNum[colorConfig[ii][jj]-1] > 9)
            {
                printf("Not a valid cube.\n");
                return -1;
            }
        }
    }
    
    for(ii=0; ii<6; ii++)
    {
        int tmpColor = colorConfig[ii][4] - 1;
        if(!centerColorTaken[tmpColor])
        {
            centerColorTaken[tmpColor] = 1;
        }
        else
        {
            printf("Not a valid cube.\n");
            return -1;
        }
    }
	
    inputFormatTransform(colorConfig);
    
    free(line);
    return 0;
}

void solveCube()
{
    int f,i=0,j=0,k=0,pc,mor;
    
    // initialise tables
	for(; k<20; k++) val[k]=k<12?2:3;

	//for(; j<8; j++) filltable(j);
	filltable();

	// read input, 20 pieces worth
	for(; i<20; i++){
		f=pc=k=mor=0;
		for(;f<val[i];f++){
			j=strchr(faces,transformedInputFormat[i][f])-faces;
			// keep track of principal facelet for orientation
			if(j>k) {k=j;mor=f;}
			//construct bit hash code
			pc+= 1<<j;
		}
		// find which cubelet it belongs, i.e. the label for this piece
		for(f=0; f<20; f++)
			if( pc==bithash[f]-64 ) break;
		// store piece
		//pos[order[i]-CHAROFFSET]=f;
		SET_POS(order[i]-CHAROFFSET, f);
		//ori[order[i]-CHAROFFSET]=mor%val[i];
		SET_ORI(order[i]-CHAROFFSET, mor%val[i]);
	}

	//solve the cube
	// four phases
	for(phase=0 ; phase<8; phase+=2){
		// try each depth till solved
		for( j=0; !searchphase(j,0,9); j++);
		//output result of this phase
		for( i=0; i<j; i++)
			printf("%c%d ", "FBRLUD"[move[i]], moveamount[i]);
	}
    printf("\n");
}

char* getInput()
{

// printf("Getting Keystroke");
int valid = 0;
char* input_string;
// while (valid == 0){
while(!IORD_8DIRECT(PS2_BASE, 0));
//printf("polled status is %x \n" , IORD_8DIRECT(DE2_PS2_0_BASE, 0));
// printf("Got Keystroke!");
code = IORD_8DIRECT(PS2_BASE,1);
// printf("scan code is %x\n", code);

// if (scan_code == 1) {
switch(code)
{

case 0x6B: // LEFT Key
	if (keyflag == 1){
		 valid = 1;
		 input_string ="Left";
		} // ELSE DO NOTHING
		keyflag = 0;
		printf("left ");
break;
case 0x74: // RIGHT Key
	if (keyflag == 1){
		 valid = 1;
		 input_string ="Right";
		} // ELSE DO NOTHING
		keyflag = 0;
		printf("right ");
break;
case 0x75: //UP Key
	if(keyflag == 1){
	    valid = 1;
	    input_string = "Up";
	}
	keyflag = 0;
    printf("up ");
break;
case 0x72: //Down Key
	if(keyflag == 1){
	    valid = 1;
	    input_string = "Down";
	}
	keyflag = 0;
	printf("down ");
break;
case 0x16:
		if(keyflag == 1){
			valid = 1;
			input_string = "1";
		}
		keyflag = 0;
		printf("1 ");
break;
case 0x1E:
		if(keyflag == 1){
			valid = 1;
			input_string = "2";
		}
		keyflag = 0;
		printf("2 ");
break;
case 0x26:
		if(keyflag == 1){
			valid = 1;
			input_string = "3";
		}
		keyflag = 0;
		printf("3 ");
break;
case 0x25:
		if(keyflag == 1){
			valid = 1;
			input_string = "4";
		}
		keyflag = 0;
		printf("4 ");
break;
case 0x2E:
		if(keyflag == 1){
			valid = 1;
			input_string = "5";
		}
		keyflag = 0;
		printf("5 ");
break;
case 0x36:
		if(keyflag == 1){
			valid = 1;
			input_string = "6";
		}
		keyflag = 0;
		printf("6 ");
break;
case 0x3D:
		if(keyflag == 1){
			valid = 1;
			input_string = "7";
		}
		keyflag = 0;
		printf("7 ");
break;
case 0x3E:
		if(keyflag == 1){
			valid = 1;
			input_string = "8";
		}
		keyflag = 0;
		printf("8 ");
break;
case 0x46:
		if(keyflag == 1){
			valid = 1;
			input_string = "9";
		}
		keyflag = 0;
		printf("9 ");
break;
default:
keyflag = 0;
break;
}
//printf("input_string is %s\n",input_string);
return input_string;
}

int main(){

	unsigned int timerLow, timerHigh;
    unsigned long timerValue, timerValueAdd;
    double duration;

    int i,j,k;

    while(1)
    	getInput();

    srand((int)time(NULL));
    for(k=0; k<1; k++)
    	for(i=0; i<6; i++)
    		for(j=0; j<9; j++)
    		{
    			SET_COLOR(i,j,rand()%6+1);
    			usleep(100000);
    		}

    for(i=0; i<1; i++)
    {
    	for(k=1; k<6; k++)
    	{
    		SET_BORDER(k);
    		usleep(300000);
    	}

    	for(k=4; k>=0; k--)
    	{
    		SET_BORDER(k);
    		usleep(300000);
    	}
    }

    if(inputColor())
        return -1;

    RESET_TIMER();
    ENABLE_TIMER();

    solveCube();

    DISABLE_TIMER();
    timerLow = GET_TIMER_LOW();
    timerHigh = GET_TIMER_HIGH();
    timerValue = ((unsigned long)timerHigh) << 16;
    timerValueAdd = timerValue + (unsigned long)timerLow;
    duration = ((double)timerValueAdd)*40/1000000000;

    printf("Running time: %lf\n", duration);
    printf("Number of Domove: %lu\n", numOfDomove);
    printf("Number of Permtonum: %lu\n", numOfPermtonum);

    return 0;
}
