/*
  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 colorNum[6] = {0,0,0,0,0,0};
    int centerColorTaken[6] = {0,0,0,0,0,0};
    int aa, 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');
                }
            }
        }
        for(aa=0;aa<9;aa++)
        {
        	SET_COLOR(ii-1,aa,colorConfig[ii-1][aa]);
        }
    }
    */
    int colorNum[6];
    int centerColorTaken[6];
	int face = 0, i, j;
	SET_BORDER(face);

CubeInput:

	for(i=0; i<6; i++) {
		colorNum[i] = 0;
		centerColorTaken[i] = 0;
	}

    for (;;) {
        status = read_make_code(&decode_mode, &key);
        if (status == PS2_SUCCESS) {
        	switch (decode_mode) {
        	case KB_ASCII_MAKE_CODE :
        		switch (key) {
        		case 97://KP 1
        			colorConfig[face][6] = colorConfig[face][6]==6?1:colorConfig[face][6]+1;
        			SET_COLOR(face,6,colorConfig[face][6]);
        			break;
        		case 98://KP 2
        			colorConfig[face][7] = colorConfig[face][7]==6?1:colorConfig[face][7]+1;
        			SET_COLOR(face,7,colorConfig[face][7]);
        			break;
        		case 99://KP 3
        			colorConfig[face][8] = colorConfig[face][8]==6?1:colorConfig[face][8]+1;
        			SET_COLOR(face,8,colorConfig[face][8]);
        			break;
        		case 100://KP 4
        			colorConfig[face][3] = colorConfig[face][3]==6?1:colorConfig[face][3]+1;
        			SET_COLOR(face,3,colorConfig[face][3]);
        			break;
        		case 101://KP 5
        			colorConfig[face][4] = colorConfig[face][4]==6?1:colorConfig[face][4]+1;
        			SET_COLOR(face,4,colorConfig[face][4]);
        			break;
        		case 102://KP 6
        			colorConfig[face][5] = colorConfig[face][5]==6?1:colorConfig[face][5]+1;
        			SET_COLOR(face,5,colorConfig[face][5]);
        			break;
        		case 103://KP 7
        			colorConfig[face][0] = colorConfig[face][0]==6?1:colorConfig[face][0]+1;
        			SET_COLOR(face,0,colorConfig[face][0]);
        			break;
        		case 104://KP 8
        			colorConfig[face][1] = colorConfig[face][1]==6?1:colorConfig[face][1]+1;
        			SET_COLOR(face,1,colorConfig[face][1]);
        			break;
        		case 105://KP 9
        			colorConfig[face][2] = colorConfig[face][2]==6?1:colorConfig[face][2]+1;
        			SET_COLOR(face,2,colorConfig[face][2]);
        			break;
        		}
        		break ;
        	case KB_LONG_BINARY_MAKE_CODE :
        	case KB_BINARY_MAKE_CODE :
        		switch (key) {
        		case 0x5a: //enter key
        			goto CubeCheck;
        			break;
        			/*
        		case 0x6B: //left arrow key
        			printf("Left arrow\n");
        			break;
        		case 0x74: //right arrow key
        			printf("Right arrow\n");
        			break;
        			*/
        		case 0x0D: //tab key
        			face = (face+1)%6;
        			SET_BORDER(face);
        			break;
        		default:
        			break;
        		}
        		break;
        	case KB_BREAK_CODE :
        		// do nothing
        		default:
        		break ;
        	}
        }
        else {
          printf(" Keyboard error ....\n");
        }
    }

CubeCheck:

    for(i=0; i<6; i++)
    {
        for(j=0; j<9; j++)
        {
            if(++colorNum[colorConfig[i][j]-1] > 9)
            {
                printf("Not a valid cube.\n");
                goto CubeInput;
            }
        }
    }
    
    for(i=0; i<6; i++)
    {
        int tmpColor = colorConfig[i][4] - 1;
        if(!centerColorTaken[tmpColor])
        {
            centerColorTaken[tmpColor] = 1;
        }
        else
        {
            printf("Not a valid cube.\n");
            goto CubeInput;
        }
    }
	
    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
	k = 0;
	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]);
			solution[k][0] = "FBRLUD"[move[i]];
			solution[k][1] = '0' + moveamount[i];
			solution[k++][2] = '\0';
		}
	}
	solution[k][0] = 'F';
	solution[k][1] = 'F';
	solution[k][2] = '\0';
    printf("\n");
}

void checkSolution()
{
	int i, j, flag;
	for(i=0;i<79;i++) {
		if(!strcmp(solution[i], "FF") || !strcmp(solution[i+1], "FF"))
			return;
		if(solution[i][0] == solution[i+1][0]) {
			flag = 1;
			if(solution[i][1] == '1' && solution[i][1] == '2')
				solution[i][1] = '3';
			else if(solution[i][1] == '3' && solution[i][1] == '2')
				solution[i][1] = '1';
			else if(solution[i][1] == '2' && solution[i][1] == '1')
				solution[i][1] = '3';
			else if(solution[i][1] == '2' && solution[i][1] == '3')
				solution[i][1] = '1';
			else
				flag = 0;
			if(flag) {
				j = i + 1;
				for(;j<79;j++)
					strcpy(solution[j], solution[j+1]);
			}
		}
	}
}

void demo()
{
	int i;
	for(i=0;i<79;i++) {
		for (;;) {
	        status = read_make_code(&decode_mode, &key);
	        if (status == PS2_SUCCESS) {
	        	switch (decode_mode) {
	        	case KB_ASCII_MAKE_CODE :
	        		break ;
	        	case KB_LONG_BINARY_MAKE_CODE :
	        	case KB_BINARY_MAKE_CODE :
	        		switch (key) {
	        		case 0x5a: //enter key
	        			return;
	        			break;
	        		case 0x6B: //left arrow key
	        			if(i) {
	        				updateColorConfigOld();
	        				if(!strcmp(solution[i-1], "U1")) {
	        					U3();
	        				} else if(!strcmp(solution[i-1], "U2")) {
	        					U2();
	        				} else if(!strcmp(solution[i-1], "U3")) {
	        					U1();
	        				} else if(!strcmp(solution[i-1], "D1")) {
	        					D3();
	        				} else if(!strcmp(solution[i-1], "D2")) {
	        					D2();
	        				} else if(!strcmp(solution[i-1], "D3")) {
	        					D1();
	        				} else if(!strcmp(solution[i-1], "F1")) {
	        					F3();
	        				} else if(!strcmp(solution[i-1], "F2")) {
	        					F2();
	        				} else if(!strcmp(solution[i-1], "F3")) {
	        					F1();
	        				} else if(!strcmp(solution[i-1], "B1")) {
	        					B3();
	        				} else if(!strcmp(solution[i-1], "B2")) {
	        					B2();
	        				} else if(!strcmp(solution[i-1], "B3")) {
	        					B1();
	        				} else if(!strcmp(solution[i-1], "L1")) {
	        					L3();
	        				} else if(!strcmp(solution[i-1], "L2")) {
	        					L2();
	        				} else if(!strcmp(solution[i-1], "L3")) {
	        					L1();
	        				} else if(!strcmp(solution[i-1], "R1")) {
	        					R3();
	        				} else if(!strcmp(solution[i-1], "R2")) {
	        					R2();
	        				} else if(!strcmp(solution[i-1], "R3")) {
	        					R1();
	        				}
		        			updateCube();
	        				i -= 2;
	        			} else {
	        				i -= 1;
	        			}
	        			goto NextIteration;
	        			break;
	        		case 0x74: //right arrow key
	        			updateColorConfigOld();
	        			if(!strcmp(solution[i], "FF")) {
	        				i--;
	        				goto NextIteration;
	        			} else if(!strcmp(solution[i], "U1")) {
	        				U1();
	        			} else if(!strcmp(solution[i], "U2")) {
	        				U2();
	        			} else if(!strcmp(solution[i], "U3")) {
	        				U3();
	        			} else if(!strcmp(solution[i], "D1")) {
	        				D1();
	        			} else if(!strcmp(solution[i], "D2")) {
	        				D2();
	        			} else if(!strcmp(solution[i], "D3")) {
	        				D3();
	        			} else if(!strcmp(solution[i], "F1")) {
	        				F1();
	        			} else if(!strcmp(solution[i], "F2")) {
	        				F2();
	        			} else if(!strcmp(solution[i], "F3")) {
	        				F3();
	        			} else if(!strcmp(solution[i], "B1")) {
	        				B1();
	        			} else if(!strcmp(solution[i], "B2")) {
	        				B2();
	        			} else if(!strcmp(solution[i], "B3")) {
	        				B3();
	        			} else if(!strcmp(solution[i], "L1")) {
	        				L1();
	        			} else if(!strcmp(solution[i], "L2")) {
	        				L2();
	        			} else if(!strcmp(solution[i], "L3")) {
	        				L3();
	        			} else if(!strcmp(solution[i], "R1")) {
	        				R1();
	        			} else if(!strcmp(solution[i], "R2")) {
	        				R2();
	        			} else if(!strcmp(solution[i], "R3")) {
	        				R3();
	        			}
	        			updateCube();
	        			goto NextIteration;
	        			break;
	        		default:
	        			break;
	        		}
	        		break;
	        	case KB_BREAK_CODE :
	        		// do nothing
	        		default:
	        		break ;
	        	}
	        }
	        else {
	          printf(" Keyboard error ....\n");
	        }
	    }

NextIteration:
		continue;
	}
}

int main(){

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

    //int i,j,k;
	int i;
    SET_TEXT(1, "WELCOME TO RUBIK'S CUBE SOLVER");
    SET_TEXT(2, "FOLLOW THE STEPS TO\nSOLVE THE CUBE!\n\n\n");
    SET_TEXT(3, "USE NUMPAD AND TAB\nTO INPUT\n\nPRESS ENTER TO SOLVE");

    // Initialize the keyboard
    clear_FIFO();
    switch (get_mode()) {
    case PS2_KEYBOARD:
    	break;
    case PS2_MOUSE:
    	printf("Error: Mouse detected on PS/2 port\n");
    	goto ErrorExit;
    default:
    	printf("Error: Unrecognized or no device on PS/2 port\n");
    	goto ErrorExit;
    }

Restart:

	for(i=0;i<9;i++) {
		colorConfig[0][i] = 3;
		colorConfig[1][i] = 6;
		colorConfig[2][i] = 4;
		colorConfig[3][i] = 5;
		colorConfig[4][i] = 1;
		colorConfig[5][i] = 2;
	}
    updateCube();
/*
    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();
    CLEAR_BORDER();
    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);
/*
    k=0;
    while(1) {
    	printf("%s ", solution[k]);
    	k++;
    	if(!strcmp(solution[k], "FF"))
    		break;
    }
    printf("\n");*/
    checkSolution();
    demo();
    goto Restart;
ErrorExit:
    printf("Program terminated with an error condition\n");
    return 1;
}
