cube.cpp

00001 #include <stdio.h>
00002 #include <stdint.h>
00003 #include <cstring> //cstring is stupid.
00004 #include <string.h>
00005 
00006 #include "cube.h"
00007 
00008 char soln_moves[] = "UDLRFB";
00009 sequence_t solve_sequence;
00010 move_t moves[30];
00011 
00012 // 0, ... ,5, UDLRFB
00013 // 6, 2
00014 // 7, '
00015 // 8, <space>
00016 // -1 other
00017 
00018 int get_char_type(char a)
00019 {
00020     for(int i = 0; i < 6; i++)
00021         if(soln_moves[i] == a)
00022             return i;
00023     if(a == '2')
00024         return 6;
00025     if(a == 39) // '
00026         return 7;
00027     if(a == ' ')
00028         return 8;
00029 
00030     printf("[ERROR] unrecognized character in solution %c (decimal %d)\r\n",a,a);
00031     return -1;
00032 }
00033 
00034 void string_to_sequence(char* string)
00035 {
00036     int move = 0;
00037 
00038     for(int i = 0; i < strlen(string); i++)
00039     {
00040         int type = get_char_type(string[i]);
00041 
00042         // direction
00043         if(i%3 == 0)
00044         {
00045             if(type>5 || type <0)
00046             {
00047                 printf("[ERROR] Could not find direction for move %d! Got %c instead.\r\n",move,string[i]);
00048                 return;
00049             }
00050             moves[move].face = (face_t)type;
00051         }
00052 
00053         if(i%3 == 1)
00054         {
00055             if(type == 6)
00056                 moves[move].n_turns = 2;
00057             else if(type == 7)
00058                 moves[move].n_turns = -1;
00059             else if(type == 8)
00060                 moves[move].n_turns = 1;
00061             else
00062             {
00063                 printf("[ERROR] Not a valid move modifier for move %d: %c\r\n",move,string[i]);
00064                 return;
00065             }
00066         }
00067 
00068         if(i%3 == 2)
00069         {
00070             if(type != 8)
00071             {
00072                 printf("[ERROR] Got invalid space character for move %d: %c\r\n",move,string[i]);
00073                 return;
00074             }
00075             move++;
00076         }
00077     }
00078 
00079     solve_sequence.n_moves = move;
00080     solve_sequence.moves = moves;
00081 }
00082 
00083 sequence_t* get_sequence()
00084 {
00085     return &solve_sequence;
00086 }
00087 
00088 void print_sequence(sequence_t* seq)
00089 {
00090     printf("print sequence 0%x\r\n",seq);
00091     int n_moves = seq->n_moves;
00092     printf("Sequence with %d moves\r\n",n_moves);
00093     for(int i = 0; i < n_moves; i++)
00094         printf("\tMove face %c %d turns.\r\n",soln_moves[seq->moves[i].face],seq->moves[i].n_turns);
00095 }
00096 
00097 int sequence_to_serial(sequence_t* seq, uint8_t* buffer, int n_bytes)
00098 {
00099     if(n_bytes < seq->n_moves)
00100     {
00101         printf("SERIAL BUFFER TOO SMALL!\r\n");
00102         return 1;
00103     }
00104 
00105     // i = 0
00106     buffer[0] = K_START;
00107     // i = 1
00108     buffer[1] = K_MOVES + (seq->n_moves & 0x3f);
00109     // i = 2 .. (n_moves+2)
00110     for(int i = 0; i < seq->n_moves; i++)
00111         buffer[i+2] = (0x7 & (seq->moves[i].n_turns)) + (0x38 & ((uint8_t)seq->moves[i].face<<3));
00112     return seq->n_moves + 2;
00113 }
00114 
00115 int serial_to_sequence(sequence_t* seq, uint8_t* buffer, int n_bytes)
00116 {
00117     printf("[serial to sequence] seq: 0x%x, buf: 0x%x, n_bytes: %x\r\n",seq,buffer,n_bytes);
00118     int start_index = -1;
00119     int n_moves = -1;
00120     for(int i = 0; i < n_bytes; i++)
00121     {
00122         if(buffer[i] == K_START)
00123         {
00124             start_index = i+2;
00125             break;
00126         }
00127     }
00128 
00129     if(start_index == -1)
00130     { 
00131         printf("FAILED TO FIND START OF MOVE!\r\n");
00132         return 1;
00133     }
00134 
00135     if((buffer[start_index-1] & 0xC0) != K_MOVES)
00136     {
00137         printf("COULD NOT FIND NUMBER OF MOVES!\r\n");
00138         return 1;
00139     }
00140 
00141     n_moves = buffer[start_index-1]&0x3f;
00142     seq->n_moves = n_moves;
00143     if(n_moves < 15)
00144     {
00145         printf("NUMBER OF MOVES LESS THAN 15.\r\n");
00146         //return 1;
00147     }
00148 
00149     for(int i = 0; i < n_moves; i++)
00150     {
00151         uint8_t ser_move = buffer[start_index+i];
00152         int8_t face = (face_t)((ser_move>>3) & 0x07);
00153         int8_t n_turns = (int8_t)((ser_move) & 0x07);
00154         // 2's complement negation
00155         if(n_turns > 2) n_turns |= 0xf8;
00156         if(face < 0 || face > 5)
00157         {
00158             printf("GOT INVALID FACE.\r\n");
00159             return 1;
00160         }
00161         if(!(n_turns == -2 || n_turns == -1 || n_turns == 1 || n_turns == 2))
00162         {
00163             printf("GOT INVALID NUMBER OF TURNS: %d\r\n",n_turns);
00164             printf("Serial data: " BYTE_TO_BINARY_PATTERN "\r\n",BYTE_TO_BINARY(ser_move));
00165 
00166             return 1;
00167         }
00168         printf("move %d nturns: %d\r\n",i,n_turns);
00169         seq->moves[i].n_turns = n_turns;
00170         seq->moves[i].face = (face_t)face;
00171     }
00172 
00173     return 0;
00174 }
00175 
00176 int check_double_move(int f1, int f2)
00177 {
00178     return (f1 == 0 && f2 == 3) ||
00179            (f2 == 0 && f1 == 3) ||
00180            (f2 == 1 && f1 == 4) ||
00181            (f2 == 1 && f1 == 4) ||
00182            (f2 == 2 && f1 == 5) ||
00183            (f2 == 2 && f1 == 5);
00184 }
00185 
00186 int states[6] = {0,0,0,0,0,0};
00187 int move_counts[6] = {0,0,0,0,0,0};
00188 int wait_counter[6] = {0,0,0,0,0,0};
00189 
00190 //states: 
00191 // 0 - start command
00192 // 1 - delay for command
00193 // 2 - wait for and
00194 // 3 - delay for and
00195 // 4 - done.
00196 void reset_mbed()
00197 {
00198     solve_sequence.moves = moves;
00199     for(int i = 0; i < 6; i++)
00200     {
00201         states[i] = 0;
00202         move_counts[i] = 0;
00203         wait_counter[i] = 0;
00204     }
00205 }
00206 void* run_sequence_2(void* command)
00207 {
00208     
00209     mbed_info_t* cmd = (mbed_info_t*)command;
00210     if(cmd->seq == NULL)
00211         return;
00212     int n_moves = cmd->seq->n_moves;
00213     int n_turns = 0;
00214     if(states[cmd->face] == 0)
00215     {
00216         cmd->set_and(0,cmd->face); // and off
00217 
00218         // check if done
00219         if(move_counts[cmd->face] >= n_moves)
00220         {
00221             printf("SEQUENCE DONE!\r\n");
00222             cmd->set_and(1,cmd->face);
00223             states[cmd->face] = 4;
00224             reset_mbed();
00225             cmd->seq = NULL;
00226             return NULL;
00227         }
00228         
00229         // don't check if we're on the second to last move
00230         int current_move = move_counts[cmd->face];
00231         int second_to_last_move = n_moves - 1;
00232 
00233 
00234         if( !(current_move >= second_to_last_move))
00235         {
00236             
00237             // check the faces...
00238             int face_1 =  cmd->seq->moves[move_counts[cmd->face]].face;
00239             int face_2 =  cmd->seq->moves[move_counts[cmd->face] + 1].face;
00240             
00241             // if they don't match, go to old thing.
00242             if(!check_double_move(face_1,face_2)) goto old_thing;
00243             
00244             // first check the current move...
00245             if(cmd->face == face_1)
00246                 n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
00247             else if(cmd->face == face_2)
00248                 n_turns = cmd->seq->moves[move_counts[cmd->face]+1].n_turns;
00249             
00250             move_counts[cmd->face]+=2;
00251             
00252             // rotate if needed
00253             if(n_turns) 
00254                 cmd->rotate(n_turns,cmd->face);
00255     
00256             // wait...
00257             states[cmd->face] = 1;
00258             wait_counter[cmd->face] = 0;
00259             return NULL;
00260             
00261         }
00262 
00263 old_thing:
00264 //***************** do the old thing ***************************
00265         
00266         // check how many turns needed
00267         if(cmd->face == cmd->seq->moves[move_counts[cmd->face]].face)
00268         {
00269             n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
00270         }
00271         else n_turns = 0;
00272 
00273         // increment move counter
00274         move_counts[cmd->face]++;
00275 
00276         // rotate if needed
00277         if(n_turns) 
00278             cmd->rotate(n_turns,cmd->face);
00279 
00280         // wait...
00281         states[cmd->face] = 1;
00282         wait_counter[cmd->face] = 0;
00283         return NULL;
00284 
00285     }
00286     else if(states[cmd->face] == 1)
00287     {
00288         //printf("b%d 1\n",cmd->face);
00289         wait_counter[cmd->face]++;
00290         if(wait_counter[cmd->face] < 500) return NULL;
00291         states[cmd->face] = 2;
00292         cmd->set_and(1,cmd->face);
00293         return NULL;
00294     }
00295     else if(states[cmd->face] == 2)
00296     {
00297         //printf("b%d 2\n",cmd->face);
00298         //printf("b 1 %d\n",cmd->face);
00299         if(cmd->get_and())
00300         {
00301             wait_counter[cmd->face] = 0;
00302             states[cmd->face] = 3;
00303             return NULL;
00304         }
00305     }
00306     else if(states[cmd->face] == 3)
00307     {
00308         //printf("b%d 3\n",cmd->face);
00309         if(wait_counter[cmd->face]++ < 100) return NULL;
00310         states[cmd->face] = 0;
00311         cmd->set_and(0,cmd->face);
00312         return NULL;
00313     }
00314     else if(states[cmd->face] == 4)
00315         ;
00316 
00317     return NULL;
00318     //printf("b%d 4\n",cmd->face);
00319 }