/*
  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},   //up
        {0,0,0,0,0,0,0,0,0},       //down
        {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},       //back
        {0,0,0,0,0,0,0,0,0}        //left
    };
    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;//val[k] = {2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,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");
}

int main(){

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

    int i,j,k;

    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;
}

/*
// set cube to solved position
void reset(){
    int i;
	for(i=0; i<20; pos[i]=i, ori[i++]=0);
}
*/

/*
// convert number in range 0..23 to permutation of 4 chars.
void numtoperm(char* p,int n,int o){
    int a, b;
    p+=o;
	p[3]=o;
	for (a=3; a--;){
		p[a] = n%(4-a) +o;
		n/=4-a;
		for (b=a; ++b<4; )
			if ( p[b] >= p[a]) p[b]++;
	}
}
*/

/*
// sets cube to any position which has index n in table t
void setposition(int t, int n){
	int i=0,j=12,k=0;
	char *corn="QRSTQRTSQSRTQTRSQSTRQTSR";
	reset();
	switch(t){
	// case 0 does nothing so leaves cube solved
	case 1://edgeflip
		for(;i<12;i++,n>>=1) ori[i]=n&1;
		break;
	case 2://cornertwist
		for(i=12;i<20;i++,n/=3) ori[i]=n%3;
		break;
	case 3://middle edge choice
		for(;i<12;i++,n>>=1) pos[i]= 8*n&8;
		break;
	case 4://ud slice choice
		for(;i<8;i++,n>>=1) pos[i]= 4*n&4;
		break;
	case 5://tetrad choice,parity,twist
		corn+=n%6*4;
		n/=6;
		for(;i<8;i++,n>>=1)
			pos[i+12]= n&1 ? corn[k++]-CHAROFFSET : j++;
		break;
	case 6://slice permutations
		numtoperm(pos,n%24,12);n/=24;
		numtoperm(pos,n%24,4); n/=24;
		numtoperm(pos,n   ,0);
		break;
	case 7://corner permutations
		numtoperm(pos,n/24,8);
		numtoperm(pos,n%24,16);
		break;
	}
}
*/
/*
// calculate a pruning table
void filltable(int ti){
	int n=1,l=1, tl=tablesize[ti];
    int i, f, q;
	// alocate table memory
	char* tb = tables[ti]=(char*) malloc(sizeof(char)*tl);
	//clear table
	memset(tb, 0, tl);
	//mark solved position as depth 1
	reset();
	tb[getposition(ti)]=1;

	// while there are positions of depth l
	while(n){
		n=0;
		// find each position of depth l
		for(i=0;i<tl;i++){
			if( tb[i]==l ){
				//construct that cube position
				setposition(ti,i);
				// try each face any amount
				for(f=0; f<6; f++){
					for(q=1;q<4;q++){
						domove(f);
						// get resulting position
						int r=getposition(ti);
						// if move as allowed in that phase, and position is a new one
						if( ( q==2 || f>=(ti&6) ) && !tb[r]){
							// mark that position as depth l+1
							tb[r]=l+1;
							n++;
						}
					}
					domove(f);
				}
			}
		}
		l++;
	}
}
*/
/*
void getInput()
{
    int valid = 0;
    unsigned char code;
    while (!valid){
    	printf("test1\n");
        while(!IORD_8DIRECT(PS2_BASE, 0))
        	printf("%d\n", IORD_8DIRECT(PS2_BASE, 0));
        printf("test2\n");
        code = IORD_8DIRECT(PS2_BASE,4);

        switch(code)
        {
            case 0x69: // 1
              printf("1\n");
              valid = 1;
              break;
            case 0x72: // 2
              valid = 1;
              break;
            case 0x7A: // 3
              valid = 1;
              break;
            case 0x6B: // 4
              valid = 1;
              break;
            case 0x73: // 5
              valid = 1;
              break;
            case 0x74: // 6
              valid = 1;
              break;
            case 0x6C: // 7
              valid = 1;
              break;
            case 0x75: // 8
              valid = 1;
              break;
            case 0x7D: // 9
              valid = 1;
              break;
            case 0x1D: // up
              printf("w\n");
              valid = 1;
              break;
            case 0x1B: // down
              valid = 1;
              break;
            case 0x1C: // left
              valid = 1;
              break;
            case 0x23: // right
              valid = 1;
              break;
            case 0x76: // return
              valid = 1;
              break;
        }
    }
}
*/
/*
// Cycles 4 pieces in array p, the piece indices given by a[0..3].
void cycle(char*p,char*a){
	SWAP(p[*a-CHAROFFSET],p[a[1]-CHAROFFSET]);
	SWAP(p[*a-CHAROFFSET],p[a[2]-CHAROFFSET]);
	SWAP(p[*a-CHAROFFSET],p[a[3]-CHAROFFSET]);
}

// twists i-th piece a+1 times.
void twist(int i,int a){
	i-=CHAROFFSET;
	ori[i]=(ori[i]+a+1)%val[i];
}

// convert permutation of 4 chars to a number in range 0..23
int permtonum(char* p){
	int n=0;
    int a, b;
	for (a=0; a<4; a++) {
		n*=4-a;
		for(b=a; ++b<4; )
			if (p[b]<p[a]) n++;
	}
	return n;
}
*/
/*
//do a clockwise quarter turn cube move
void domove(int m){
	char *p=perm+8*m;
    int i=8;
	//cycle the edges
	cycle(pos,p);
	cycle(ori,p);
	//cycle the corners
	cycle(pos,p+4);
	cycle(ori,p+4);
	//twist corners if RLFB
	if(m<4)
		for(;--i>3;) twist(p[i],i&1);
	//flip edges if FB
	if(m<2)
		for(i=4;i--;) twist(p[i],0);
}
*/
