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

  Thistlethwaite's algorithm.
*/

#include <io.h>
#include <system.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>

#define ENABLE_TIMER() \
  IOWR_16DIRECT(TIMER_BASE, 4, 1)
#define DISABLE_TIMER() \
  IOWR_16DIRECT(TIMER_BASE, 4, 0)

#define GET_TIMER_LOW() \
  IORD_16DIRECT(TIMER_BASE, 0)
#define GET_TIMER_HIGH() \
  IORD_16DIRECT(TIMER_BASE, 2)

void SET_COLOR(int face, int position, int color)
{/*
	unsigned int getColor = IORD_16DIRECT(VGA_RASTER_BASE, 2*(face/2*9+position));
  	getColor = (255 << (face+1)%2*8) & getColor;
  	unsigned int newColor = (unsigned int)color << face%2*8;
  	newColor = newColor + getColor;
	IOWR_16DIRECT(VGA_RASTER_BASE, 2*(face/2*9+position), newColor);
	*/
	//unsigned int getColor = IORD_8DIRECT(VGA_RASTER_BASE, 2*(face/2*9+position)+face/2);
	//getColor = (255 << (face+1)%2*8) & getColor;
	//unsigned int newColor = (unsigned int)color << face%2*8;
	//newColor = newColor + getColor;
	IOWR_16DIRECT(VGA_RASTER_BASE, 2*(face/2*9+position)+face/2, newColor);
}

//A solved cube format:
//UF UR UB UL DF DR DB DL FR FL BR BL UFR URB UBL ULF DRF DFL DLB DBR
const char CENTER_COLOR_POSITION[6] = {'U', 'D', 'F', 'R', 'B', 'L'};
const int SIDE_INDEX_1[12] = {0,0,0,0,1,1,1,1,2,2,4,4};
const int SIDE_INDEX_2[12] = {7,5,1,3,1,5,7,3,5,3,3,5};
const int SIDE_INDEX_3[12] = {2,3,4,5,2,3,4,5,3,5,3,5};
const int SIDE_INDEX_4[12] = {1,1,1,1,7,7,7,7,3,5,5,3};
const int CORNER_INDEX_1[8] = {0,0,0,0,1,1,1,1};
const int CORNER_INDEX_2[8] = {8,2,0,6,2,0,6,8};
const int CORNER_INDEX_3[8] = {2,3,4,5,3,2,5,4};
const int CORNER_INDEX_4[8] = {2,2,2,2,6,6,6,6};
const int CORNER_INDEX_5[8] = {3,4,5,2,2,5,4,3};
const int CORNER_INDEX_6[8] = {0,0,0,0,8,8,8,8};

char transformedInputFormat[20][3];

char
	// RLFBUD is the face order used for input, so that a correctly oriented
	// piece in the input has its 'highest value' facelet first. The rest of the
	// program uses moves in FBRLUD order.
	*faces="RLFBUD",
	// I use char arrays here cause they can be initialised with a string
	// which is shorter than initialising other arrays.
	// Internally cube uses slightly different ordering to the input so that
	//  orbits of stage 4 are contiguous. Note also that the two corner orbits
	//  are diametrically opposite each other.
	//input:  UF UR UB UL  DF DR DB DL  FR FL BR BL  UFR URB UBL ULF   DRF DFL DLB DBR
	//        A  B  C  D   E  F  G  H   I  J  K  L   M   N   O   P     Q   R   S   T   
	//        A  E  C  G   B  F  D  H   I  J  K  L   M   S   N   T     R   O   Q   P
	//intrnl: UF DF UB DB  UR DR UL DL  FR FL BR BL  UFR UBL DFL DBR   DLB DRF URB ULF 
	*order="AECGBFDHIJKLMSNTROQP",
	//To quickly recognise the pieces, I construct an integer by setting a bit for each
	// facelet. The unique result is then found on the list below to map it to the correct
	// cubelet of the cube.
	//intrnl: UF DF UB DB  UR DR UL DL  FR FL BR BL  UFR UBL DFL DBR   DLB DRF URB ULF 
	//bithash:20,36,24,40, 17,33,18,34, 5, 6, 9, 10, 21, 26, 38, 41,   42, 37, 25, 22
	*bithash="TdXhQaRbEFIJUZfijeYV",
	//Each move consists of two 4-cycles. This string contains these in FBRLUD order.
	//intrnl: UF DF UB DB  UR DR UL DL  FR FL BR BL  UFR UBL DFL DBR   DLB DRF URB ULF 
	//        A  B  C  D   E  F  G  H   I  J  K  L   M   N   O   P     Q   R   S   T   
	*perm="AIBJTMROCLDKSNQPEKFIMSPRGJHLNTOQAGCEMTNSBFDHORPQ",   //size: 48

	// current cube position
	pos[20],ori[20],val[20],
	// temporary variable used in swap macro
	TEMP,
	// pruning tables, 2 for each phase
	*tables[8];
	// current phase solution
int move[20],moveamount[20],
	// current phase being searched (0,2,4,6 for phases 1 to 4)
	phase=0,
	// Length of pruning tables. (one dummy in phase 1);
	tablesize[]={1,4096,  6561,4096,  256,1536,  13824,576};   //30946 bytes

// Use very ugly and unsafe macro to swap items instead of classic routine with
//   pointers for the sole reason of brevity
#define SWAP(a,b) TEMP=a;a=b;b=TEMP;
// number 65='A' is often subtracted to convert char ABC... to number 0,1,2,...
#define CHAROFFSET 65

unsigned long numOfCycle = 0;
unsigned long numOfTwist = 0;
unsigned long numOfPermtonum = 0;

// Cycles 4 pieces in array p, the piece indices given by a[0..3].
void cycle(char*p,char*a){
    numOfCycle++;
	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){
    numOfTwist++;
	i-=CHAROFFSET;
	ori[i]=(ori[i]+a+1)%val[i];
}


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

// convert permutation of 4 chars to a number in range 0..23
int permtonum(char* p){
    numOfPermtonum++;
	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;
}

// 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]++;
	}
}

// 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+= 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+ori[i];
		break;
	case 3://middle edge choice
		// 12 bits, set bit if edge belongs in Um middle slice
		for(;++i<12;) n+= (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+= (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=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
		n=permtonum(pos)*576+permtonum(pos+4)*24+permtonum(pos+12);
		break;
	case 7://one edge and one corner orbit, permutation
		n=permtonum(pos+8)*24+permtonum(pos+16);
		break;
	}
	return n;
}


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


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

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

// Pruned tree search. recursive.
int searchphase(int movesleft, int movesdone,int lastmove){
    int j;
    int i;
	// 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);
        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++)
        {
        	//printf("test: %d\n", ii-1);
        	//printf("test: %d\n", aa);
        	//printf("test: %d\n", colorConfig[ii-1][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);
    
	// 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;
		ori[order[i]-CHAROFFSET]=mor%val[i];
	}
    
	//solve the cube
	// four phases
	for( ; 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<3; k++)
    	for(i=0; i<6; i++)
    		for(j=0; j<9; j++)
    		{
    			SET_COLOR(i,j,rand()%6+1);
    			usleep(100000);
    		}

    if(inputColor())
        return -1;
    
    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 Cycle: %lu\n", numOfCycle);
    printf("Number of Twist: %lu\n", numOfTwist);
    printf("Number of Permtonum: %lu\n", numOfPermtonum);

    return 0;
}

