Colony

#include "comm_server.h"

#include <avr/pgmspace.h>

#include <string.h>

#include "global.h"

#include "tests.h"

#include "comm_interactive.h"

// ##############

// ## Settings ##

// ##############

//#define comm_server_debug

// ###############

// ## Constants ##

// ###############

#define buffer_size 120

// Put all symbolic string constants into the program memory because else all of them would be copied into the RAM (see

// the avr-libc documentation, chapter "Data in Program Space").

// Commands 

const char command_ping          [] PROGMEM = "ping"       ;

const char command_start_test    [] PROGMEM = "start_test" ;

const char command_all           [] PROGMEM = "all"        ;

const char command_comm          [] PROGMEM = "comm"       ;

const char command_bom           [] PROGMEM = "bom"        ;

const char command_rangefinder   [] PROGMEM = "rangefinder";

const char command_motor         [] PROGMEM = "motor"      ;

const char command_encoder       [] PROGMEM = "encoder"    ;

const char command_emitter       [] PROGMEM = "emitter"    ;

const char command_detector      [] PROGMEM = "detector"   ;

const char command_help          [] PROGMEM = "help"       ;

const char command_interactive   [] PROGMEM = "interactive";

const char command_i             [] PROGMEM = "i"          ;

// Help

const char help_text          [] PROGMEM =

        "# Available commands:" NL

        "#   - help" NL

        "#   - ping" NL

        "#   - start_test all" NL

        "#   - start_test comm" NL

        "#   - start_test bom all|<num> (0..15)" NL

        "#   - start_test rangefinder all|<num> (0..4)" NL

        "#   - start_test motor all|<num> (0..1)" NL

        "#   - start_test encoder all|<num> (0..1)" NL

        ;

// ####################

// ## Initialization ##

// ####################

void comm_server_init ()

{

        usb_init ();

}

// ########################

// ## Message processing ##

// ########################

static char *find_next_parameter (char *p)

{

        // Advance p until a blank or the terminating zero is found.

        while ((*p)!=' ' && (*p)!=0) p++;

        // Advance p until a non-blank (including the terminating zero) is found.

        while (*p==' ') p++;

        // We have found the next parameter unless p points to the terminating zero.

        if (*p==0)

                return NULL;

        else

                return p;

}

/** Check if the buffer starts with the given text, followed by a space */

static bool serial_match (PGM_P text, const char *buffer)

{

        uint8_t text_len=strlen_P (text);

        uint8_t buffer_len=strlen (buffer);

        // If the buffer is shorter than the text, there is no match

        if (buffer_len<text_len) return false;

        // If the buffer is longer than the text, it must have a space at position after the text

        if (buffer_len>text_len && buffer[text_len]!=' ') return false;

        // Test for match

        if (strncmp_P ((char *)buffer, text, text_len)==0) return true;

        return false;

}

static bool serial_number_u8 (uint8_t *num, const char *buffer)

{

        uint8_t result=0;

        // Iterate over the buffer

        while (1)

        {

                // Look at the character

                char c=*buffer;

                if (c>='0' && c<='9')

                {

                        // It's a digit. Process it.

                        result*=10;

                        result+=(c-'0');

                }

                else if (c==' ' || c==0)

                {

                        // It's a space or a terminating zero, which means that the paramter is a valid number.

                        *num=result;

                        return true;

                }

                else

                {

                        // It's a different character, which means that the parameter is not a number.

                        return false;

                }

                // Move to the next byte

                buffer++;

        }

}

// ########################

// ## Messages (sending) ##

// ########################

void server_send_finished ()

{

        usb_puts ("finished");

}

// ##########################

// ## Messages (Receiving) ##

// ##########################

static void unhandled_parameters (char *buffer)

{

        usb_puts ("# Warning: unhandled parameters: [");

        usb_puts (buffer);

        usb_puts ("]" NL);

}

static void parameter_missing (void)

{

        usb_puts ("# Parameter missing" NL);

}

// ######################

// ## Message handlers ##

// ######################

static void handle_message_ping (char *buffer)

{

        usb_puts ("pong" NL);

        orbs_set (255, 127, 0, 0, 255, 0);

        delay_ms (400);

        orbs_set (0, 255, 0, 255, 127, 0);

}

static void handle_message_start_test_all (char *buffer)

{

        test_all ();

}

static void handle_message_start_test_comm (char *buffer)

{

        test_comm ();

}

/** If the next parameter is "all", call the test all function. If it's a number, call the test one function. **/

static void handle_message_start_test_component (char *buffer, void (*test_all_fn)(void), void (*test_one_fn)(uint8_t))

{

        // Find the next parameter

        buffer=find_next_parameter (buffer);

        // Initialize a buffer for the component number

        uint8_t num=0;

        if (!buffer)

                // No paramter found (we could also interpret that as implicit "all")

                parameter_missing ();

        else if (serial_match (command_all, buffer))

                // The paramter is "all". Call the all-tests function

                test_all_fn ();

        else if (serial_number_u8 (&num, buffer))

                // The paramter is a number. Call the one-test function 

                test_one_fn (num);

        else

                // Unknwon parameter

                unhandled_parameters (buffer);

}

static void test_bom_both     (uint8_t num) { test_bom (num, true , true ); }

static void test_bom_emitter  (uint8_t num) { test_bom (num, true , false); }

static void test_bom_detector (uint8_t num) { test_bom (num, false, true ); }

static void test_bom_all_both     (void) { test_bom_all (true , true ); }

static void test_bom_all_emitter  (void) { test_bom_all (true , false); }

static void test_bom_all_detector (void) { test_bom_all (false, true ); }

static void handle_message_start_test_bom (char *buffer)

{

        // "start_test bom..."

        char *param=find_next_parameter (buffer);

        // In addition to "all" and a number, both handled by handle_message_start_test_component, we can also have

        // "emitter" and "detector" here. Not that we pass param rather than buffer in these cases.

        if (!param)

                // "start_test bom"

                // No paramter found (we could also pass that on to start_test_component)

                parameter_missing ();

        else if (serial_match (command_emitter, param))

                // "start_test bom emitter..."

                handle_message_start_test_component (param, test_bom_all_emitter, test_bom_emitter);

        else if (serial_match (command_detector, param))

                // "start_test bom detector..."

                handle_message_start_test_component (param, test_bom_all_detector, test_bom_detector);

        else

                // No emitter/detector given

                handle_message_start_test_component (buffer, test_bom_all_both, test_bom_both);

}

static void handle_message_start_test_rangefinder (char *buffer)

{

        handle_message_start_test_component (buffer, test_rangefinder_all, test_rangefinder);

}

static void handle_message_start_test_motor (char *buffer)

{

        handle_message_start_test_component (buffer, test_motor_all, test_motor);

}

static void handle_message_start_test_encoder (char *buffer)

{

        handle_message_start_test_component (buffer, test_encoder_all, test_encoder);

}

static void handle_message_start_test (char *buffer)

{

        // TODO add required comm test

        buffer=find_next_parameter (buffer);

        if (!buffer)                                         parameter_missing ();

        else if (serial_match (command_all        , buffer)) handle_message_start_test_all         (buffer);

        else if (serial_match (command_comm       , buffer)) handle_message_start_test_comm        (buffer);

        else if (serial_match (command_bom        , buffer)) handle_message_start_test_bom         (buffer);

        else if (serial_match (command_rangefinder, buffer)) handle_message_start_test_rangefinder (buffer);

        else if (serial_match (command_motor      , buffer)) handle_message_start_test_motor       (buffer);

        else if (serial_match (command_encoder    , buffer)) handle_message_start_test_encoder     (buffer);

        else                                                 unhandled_parameters (buffer);

}

static void handle_message_help (char *buffer)

{

        usb_puts_P (help_text);

}

// ##########################

// ## Messages (receiving) ##

// ##########################

/** message is 0 terminated */

static void handle_message (char *message)

{

#ifdef comm_server_debug

        // Output: "# Received a message, size <size>: [<message>]"

        usb_puts ("# Received a message, size ");

        usb_puti(size);

        usb_puts (": [");

        usb_puts ((char *)message);

        usb_puts ("]" NL);

#endif

        bool handled=false;

        if (serial_match (command_ping       , message)) { handle_message_ping       (message); handled=true; }

        if (serial_match (command_start_test , message)) { handle_message_start_test (message); handled=true; }

        if (serial_match (command_help       , message)) { handle_message_help       (message); handled=true; }

        if (serial_match (command_interactive, message)) { interactive_main ()                ; handled=true; }

        if (serial_match (command_i          , message)) { interactive_main ()                ; handled=true; }

        // More messages go here

        if (!handled)

        {

                usb_puts ("# Warning: unhandled message: [");

                usb_puts ((char *)message);

                usb_puts ("]" NL);

        }

}

// ###############

// ## Debugging ##

// ###############

void byte_transmission_test (void)

{

        uint8_t c;

        while (1)

        {

                c=usb_getc ();

                usb_puts ("[");

                usb_puti (c);

                usb_puts ("] ");

        }

}

// ###############

// ## Main loop ##

// ###############

static void ready (void)

{

        usb_puts ("ready" NL);

}

void server_main (void)

{

        char buffer[buffer_size];

        uint8_t c;

        uint8_t buffer_fill=0;

        // Set the orbs to green/yellow

        orbs_set (0,255,0, 255,127,0);

        //byte_transmission_test (); // Does not return

        usb_puts (NL);

        usb_puts ("# Diagnostic station server mode" NL);

        ready ();

        while (1)

        {

                // Do nothing until we receive a byte (this function will return 0 when a character has been read)

                // (Yes, we could use the blocking function for now, but we may need the non-blocking later)

                while (usb_getc_nb ((char *)&c));

                if (c=='\r' || c=='\n')

                {

                        // A newline character was received. This terminates the message.

                        // Empty lines are ignored.

                        if (buffer_fill>0)

                        {

                                // Add a terminating 0 to the buffer so the string functions won't try to read outside the buffer.

                                buffer[buffer_fill]=0;

                                // Handle the message.

                                handle_message (buffer);

                                // Reset the buffer

                                buffer_fill=0;

                        }

                        // TODO do better, make a function get_message?

                        ready ();

                }

                else if (c==8)

                {

                        // Backspace

                        // Of course, the server will never send one, but it's convenient to have for debugging.

                        if (buffer_fill>0) --buffer_fill;

                }

                else

                {

                        // If there is enough space left in the buffer, add the character we just received. Leave one byte for a

                        // terminating 0. If there is not enough space in the buffer, the rest of the message is ignored.

                        if (buffer_fill<buffer_size-1)

                        {

                                buffer[buffer_fill]=c;

                                ++buffer_fill;

                        }

                }

        }

}