File content as of revision 11:16d807d6b9c5:

#include <stdio.h>
#include <stdint.h>
#include <cstring> //cstring is stupid.
#include <string.h>

#include "cube.h"

char soln_moves[] = "UDLRFB";
sequence_t solve_sequence;
move_t moves[30];

// 0, ... ,5, UDLRFB
// 6, 2
// 7, '
// 8, <space>
// -1 other

int get_char_type(char a)
{
    for(int i = 0; i < 6; i++)
        if(soln_moves[i] == a)
            return i;
    if(a == '2')
        return 6;
    if(a == 39) // '
        return 7;
    if(a == ' ')
        return 8;

    printf("[ERROR] unrecognized character in solution %c (decimal %d)\r\n",a,a);
    return -1;
}

void string_to_sequence(char* string)
{
    int move = 0;

    for(int i = 0; i < strlen(string); i++)
    {
        int type = get_char_type(string[i]);

        // direction
        if(i%3 == 0)
        {
            if(type>5 || type <0)
            {
                printf("[ERROR] Could not find direction for move %d! Got %c instead.\r\n",move,string[i]);
                return;
            }
            moves[move].face = (face_t)type;
        }

        if(i%3 == 1)
        {
            if(type == 6)
                moves[move].n_turns = 2;
            else if(type == 7)
                moves[move].n_turns = -1;
            else if(type == 8)
                moves[move].n_turns = 1;
            else
            {
                printf("[ERROR] Not a valid move modifier for move %d: %c\r\n",move,string[i]);
                return;
            }
        }

        if(i%3 == 2)
        {
            if(type != 8)
            {
                printf("[ERROR] Got invalid space character for move %d: %c\r\n",move,string[i]);
                return;
            }
            move++;
        }
    }

    solve_sequence.n_moves = move;
    solve_sequence.moves = moves;
}

sequence_t* get_sequence()
{
    return &solve_sequence;
}

void print_sequence(sequence_t* seq)
{
    printf("print sequence 0%x\r\n",seq);
    int n_moves = seq->n_moves;
    printf("Sequence with %d moves\r\n",n_moves);
    for(int i = 0; i < n_moves; i++)
        printf("\tMove face %c %d turns.\r\n",soln_moves[seq->moves[i].face],seq->moves[i].n_turns);
}

int sequence_to_serial(sequence_t* seq, uint8_t* buffer, int n_bytes)
{
    if(n_bytes < seq->n_moves)
    {
        printf("SERIAL BUFFER TOO SMALL!\r\n");
        return 1;
    }

    // i = 0
    buffer[0] = K_START;
    // i = 1
    buffer[1] = K_MOVES + (seq->n_moves & 0x3f);
    // i = 2 .. (n_moves+2)
    for(int i = 0; i < seq->n_moves; i++)
        buffer[i+2] = (0x7 & (seq->moves[i].n_turns)) + (0x38 & ((uint8_t)seq->moves[i].face<<3));
    return seq->n_moves + 2;
}

int serial_to_sequence(sequence_t* seq, uint8_t* buffer, int n_bytes)
{
    printf("[serial to sequence] seq: 0x%x, buf: 0x%x, n_bytes: %x\r\n",seq,buffer,n_bytes);
    int start_index = -1;
    int n_moves = -1;
    for(int i = 0; i < n_bytes; i++)
    {
        if(buffer[i] == K_START)
        {
            start_index = i+2;
            break;
        }
    }

    if(start_index == -1)
    { 
        printf("FAILED TO FIND START OF MOVE!\r\n");
        return 1;
    }

    if((buffer[start_index-1] & 0xC0) != K_MOVES)
    {
        printf("COULD NOT FIND NUMBER OF MOVES!\r\n");
        return 1;
    }

    n_moves = buffer[start_index-1]&0x3f;
    seq->n_moves = n_moves;
    if(n_moves < 15)
    {
        printf("NUMBER OF MOVES LESS THAN 15.\r\n");
        //return 1;
    }

    for(int i = 0; i < n_moves; i++)
    {
        uint8_t ser_move = buffer[start_index+i];
        int8_t face = (face_t)((ser_move>>3) & 0x07);
        int8_t n_turns = (int8_t)((ser_move) & 0x07);
        // 2's complement negation
        if(n_turns > 2) n_turns |= 0xf8;
        if(face < 0 || face > 5)
        {
            printf("GOT INVALID FACE.\r\n");
            return 1;
        }
        if(!(n_turns == -2 || n_turns == -1 || n_turns == 1 || n_turns == 2))
        {
            printf("GOT INVALID NUMBER OF TURNS: %d\r\n",n_turns);
            printf("Serial data: " BYTE_TO_BINARY_PATTERN "\r\n",BYTE_TO_BINARY(ser_move));

            return 1;
        }
        printf("move %d nturns: %d\r\n",i,n_turns);
        seq->moves[i].n_turns = n_turns;
        seq->moves[i].face = (face_t)face;
    }

    return 0;
}

int check_double_move(int f1, int f2)
{
    return (f1 == 0 && f2 == 3) ||
           (f2 == 0 && f1 == 3) ||
           (f2 == 1 && f1 == 4) ||
           (f2 == 1 && f1 == 4) ||
           (f2 == 2 && f1 == 5) ||
           (f2 == 2 && f1 == 5);
}

int states[6] = {0,0,0,0,0,0};
int move_counts[6] = {0,0,0,0,0,0};
int wait_counter[6] = {0,0,0,0,0,0};

//states: 
// 0 - start command
// 1 - delay for command
// 2 - wait for and
// 3 - delay for and
// 4 - done.
void reset_mbed()
{
    solve_sequence.moves = moves;
    for(int i = 0; i < 6; i++)
    {
        states[i] = 0;
        move_counts[i] = 0;
        wait_counter[i] = 0;
    }
}
void* run_sequence_2(void* command)
{
    
    mbed_info_t* cmd = (mbed_info_t*)command;
    if(cmd->seq == NULL)
        return;
    int n_moves = cmd->seq->n_moves;
    int n_turns = 0;
    if(states[cmd->face] == 0)
    {
        cmd->set_and(0,cmd->face); // and off

        // check if done
        if(move_counts[cmd->face] >= n_moves)
        {
            printf("SEQUENCE DONE!\r\n");
            cmd->set_and(1,cmd->face);
            states[cmd->face] = 4;
            reset_mbed();
            cmd->seq = NULL;
            return NULL;
        }
        
        // don't check if we're on the second to last move
        int current_move = move_counts[cmd->face];
        int second_to_last_move = n_moves - 1;


        if( !(current_move >= second_to_last_move))
        {
            
            // check the faces...
            int face_1 =  cmd->seq->moves[move_counts[cmd->face]].face;
            int face_2 =  cmd->seq->moves[move_counts[cmd->face] + 1].face;
            
            // if they don't match, go to old thing.
            if(!check_double_move(face_1,face_2)) goto old_thing;
            
            // first check the current move...
            if(cmd->face == face_1)
                n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
            else if(cmd->face == face_2)
                n_turns = cmd->seq->moves[move_counts[cmd->face]+1].n_turns;
            
            move_counts[cmd->face]+=2;
            
            // rotate if needed
            if(n_turns) 
                cmd->rotate(n_turns,cmd->face);
    
            // wait...
            states[cmd->face] = 1;
            wait_counter[cmd->face] = 0;
            return NULL;
            
        }

old_thing:
//***************** do the old thing ***************************
        
        // check how many turns needed
        if(cmd->face == cmd->seq->moves[move_counts[cmd->face]].face)
        {
            n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
        }
        else n_turns = 0;

        // increment move counter
        move_counts[cmd->face]++;

        // rotate if needed
        if(n_turns) 
            cmd->rotate(n_turns,cmd->face);

        // wait...
        states[cmd->face] = 1;
        wait_counter[cmd->face] = 0;
        return NULL;

    }
    else if(states[cmd->face] == 1)
    {
        //printf("b%d 1\n",cmd->face);
        wait_counter[cmd->face]++;
        if(wait_counter[cmd->face] < 500) return NULL;
        states[cmd->face] = 2;
        cmd->set_and(1,cmd->face);
        return NULL;
    }
    else if(states[cmd->face] == 2)
    {
        //printf("b%d 2\n",cmd->face);
        //printf("b 1 %d\n",cmd->face);
        if(cmd->get_and())
        {
            wait_counter[cmd->face] = 0;
            states[cmd->face] = 3;
            return NULL;
        }
    }
    else if(states[cmd->face] == 3)
    {
        //printf("b%d 3\n",cmd->face);
        if(wait_counter[cmd->face]++ < 100) return NULL;
        states[cmd->face] = 0;
        cmd->set_and(0,cmd->face);
        return NULL;
    }
    else if(states[cmd->face] == 4)
        ;

    return NULL;
    //printf("b%d 4\n",cmd->face);
}