File content as of revision 67:5650f461722a:

#include <TA.h>
#include <types.h>
#include "DataStore.hh"

#define RED p3
#define GREEN p5
#define BLUE p6

#define ENABLE_1 p4
#define ENABLE_2 p7
#define ENABLE_3 p8

#define DEBUG_BAUD  57600 //57600

#define TRILAT_CONE      99

#define DEBOUNCE_MS 100
#define LIGHTS      TOUCHLIGHTS
#define FAKEOUT     0x08
#define FAIL_QUICK  0x10
#define SILENT      0x20
#define GRACE_PERIOD 3000

#define NEED_CONSOLE_OUTPUT 1 /* Set this if you need //////////////////////DEBUG messages on the console;
                               * it will have an impact on code-size and power consumption. */

#define LOOPBACK_MODE       0  // Loopback mode

#if NEED_CONSOLE_OUTPUT
#define DEBUG(...) { printf(__VA_ARGS__); }
#else
#define DEBUG(...) /* nothing */
#endif /* #if NEED_CONSOLE_OUTPUT */

extern int random(int numberone, int numbertwo);
extern bool is_master;

extern uint8_t* datastore_get_cones();
extern uint8_t* datastore_get_masks();
extern uint16_t* datastore_get_times();

unsigned long millis();
unsigned long micros();

TA ta;

static bool active_cones[NUM_CONES + 1]; // + 1 so we can be 1 based like the cone numbers are

const static int CONTROL_CONE = 1;

static Mode_t mode = PATTERN_M;
static patternState_t state_p = IDLE_P;
static inputState_t state_i = IDLE_I;

State_t state = IDLE;


static Message m1 = { 'm',0,2 };

static Message *m = &m1;
static Message m2 = { 'm',0,2 };
static Message *m_in = &m2;

static uint8_t active_cone = 0;
static unsigned long timeout = 10000L;
static uint8_t mask = 0x07;
static uint8_t fakeout = 0;
static uint8_t fail_quick = 0;
static uint8_t index = 0;
static bool new_state = false;
static bool tag_start = false; // flag to indicate we should wait for the user to activate the first station before going through the sequence

static bool in_menu = false;
static bool warning = false;
static bool penalty = false;
static bool logging = false;

// course setup (probably should make some of these persistant settings)
static uint8_t active_sequence = 0;
static uint8_t station = 1;
static uint8_t cone = 0;
static uint16_t ltime = 0;

volatile bool triggered;
volatile bool pin;
volatile bool ping = false;

static volatile bool captured = false;
static volatile unsigned long ping_timer = 0;
static volatile unsigned long dist_timeout = 0;

// These pointers are allocated in DataStore
static uint8_t *cone_table  = NULL;
static uint8_t *mask_table  = NULL;
static uint16_t *time_table = NULL;

// pointer to active table
static uint8_t *cones  = NULL;
static uint8_t *masks  = NULL; 
static uint16_t *times = NULL;

static bool lonely = false;

// Function prototypes
void spin();
patternState_t stateFromCone(uint8_t cone);
void printSplit(unsigned long timer);
uint8_t getRandomCone(void);
void clearCones(void);
void powerupCones(uint8_t sound);
void failCones(void);
void resetSensors(void);
void successCones(void);
void find_cones(void);
void printMsAsSeconds(unsigned long number);
void spinButtons(void);
uint8_t checkButtons(void);
Event getInputEvent(void); 
void getRadioInput(char *ibuffer, int size);
void interpret_master(char parameter, int value);
void interpret_slave(char parameter, int value);

extern "C" void writeToPhone(char *format, ...);

static const int active_colour  = 0x00FF00; /// 0x00FF;
static const int warn_colour    = 0xFFA500; /// 0xF050;
static const int fail_colour    = 0xFF0000; /// 0x0000FF;
static const int touch_colour   = 0x0000FF; /// 0x005050;
static const int success_colour = 0x00FF00; /// 0xFFFF;
static const int pen_colour     = 0xFFFF00; /// 0x10FF;
static const int idle_colour    = 0xFF0000; /// 0x00;
static const int batt_colour    = 0xFFFF00;

char local_input[50] = {0};

static void master_setup()
{
    ta.initialize(NODE_ID);
  
  uint16_t sent = 0;
  uint16_t lost = 0;
  unsigned long start = micros();

  m->command = 'p';
  m->value = 0;
  m->cone = 0;
  
  int i;
  
  for(i = 1; i < NUM_CONES + 1; ++i)
  {
    active_cones[i] = true; /// false;
  }
    
  unsigned long time = 0;
  
  cone_table = datastore_get_cones();
  mask_table = datastore_get_masks();
  time_table = datastore_get_times();   
  
  ta.post_color(0xFF0000);
  
  srand( rndHW() );
}

static void slave_setup()
{
    ta.initialize(NODE_ID);
}


void setup()
{
    if (is_master)
    {
        master_setup();
    }
    else
    {
        slave_setup();   
    }    
}

static void slave_loop()
{
    static unsigned long start = 0;
    static unsigned long timeout;
    static bool process_next_as_time = false;
    static bool process_next_as_mask = false;
    static unsigned long timer;
    static State_t last_state = IDLE;
    static int counter = 0;
  
    ta.spin();

    if(last_state != state)
    {
        if(state == ACTIVE_TARGET)
            SL_DEBUG("State is ACTIVE_TARGET\r\n");
        if(state == IDLE)
            SL_DEBUG("State is IDLE\r\n");
        if(state == FAIL)
            SL_DEBUG("State is FAIL\r\n");
        if(state == SUCCESS)
            SL_DEBUG("State is SUCCESS\r\n");
    }
    last_state = state;

    char message = 'n';
    timer = millis() - start;
  
    if(ta.recieve(m_in))
    {
        //ta.beep(1000);
        
        message = m_in->command;
        switch(m_in->command)
        {
        case 't':
            timeout = m_in->value;
            //serial.printf("timeout: ");
            //serial.printf("%d\n",timeout);
            break;
        case 'm':
            //serial.printf("config bits: ");
            mask = m_in->value;
            ta.setMask(mask);// & LIGHTS);
            //serial.printf("%s\n",byte_to_binary(mask));
            fakeout = mask & FAKEOUT;
            
            //if(fakeout)
            //    serial.printf("fakeout!");
            
            fail_quick = mask & FAIL_QUICK;
            break;
        default: break;
    }
  }
  
  if(message == 'x')
  {
      ta.resetTouch();
  }
  
  if(message == 'f')
  { // Fail
     //serial.printf("Fail!\n");
     SL_DEBUG("FAIL\r\n");
///  ta.post_color(fail_colour);
     ta.pulse(25,200,3000,fail_colour);
     state = FAIL;
  }
  
  if(message == 's')
  {  // Success
    //serial.printf("Success!\n");
    ta.post_color(success_colour);
    ta.beep(1500);
    state = SUCCESS;
  }

  if(message == 'g')
  { // This cone is the active target, GO!
    start = millis();
    warning = false;
    penalty = false;
/// ta.post_color(active_colour);
    ta.pulse(50,750,~0L,active_colour);
    state = ACTIVE_TARGET;
    //serial.printf("fakeout, fail_quick\n");
    //serial.printf("%d",fakeout);
    //serial.printf(", ");
    //serial.printf("%d\n",fail_quick);
  }

  if(message == 'q')
  {  // Quit
    SL_DEBUG("Clear!\r\n");
    ta.pulse_off();
//    ta.mask_color(idle_colour);
    state = IDLE;
  }
  
  if(message != 'n')
  {
    //serial.printf("%d",message);
    //serial.printf(", ");
    //serial.printf("%d\n",m_in->value);
  }
  
   if(message == 'u')
   {  // powerup noise
      ta.powerup(m_in->value);
   }
   
  if(message == 'z')
  {
    m->value = m_in->value;
    m->command = 'z';
    m->cone = 1;
    //// ta.send(m);
    ta.send_immediate(m);
  }
  
  timer = millis() - start;
  
  switch(state)
  {
    case ACTIVE_TARGET:
      if(ta.activated() || ((timer > timeout) && (fakeout)))
      {
        m->command = 'd';
        m->cone = CONTROL_CONE;
        ta.send(m);
///     ta.post_color(success_colour);
        ta.pulse_off();
        state = IDLE;
        //serial.printf("Done!\n");
      }
      else if(timer > timeout && !penalty)
      {
        SL_DEBUG("PEN\r\n");
        //serial.printf("Penalty!\n");
        penalty = true;
        ta.pulse(50,600,~0L,pen_colour);
///     ta.post_color(pen_colour);
      }
      else if(timer > ((timeout*3)/4) && !warning)
      {
        SL_DEBUG("WARN\r\n");
        //serial.printf("Warning!\n");
        warning = true;
        ta.pulse(50,750,~0L,warn_colour);
///     ta.post_color(warn_colour);
      }
      break;
    case IDLE:
      bool act = ta.activated();
      if( ta.batteryLow && !ta.isPulsing() )  // Temporary feature until batt-low-to-and-in-app.
      {                                       //  Note:  There's no in-a-run slave cone state--Can trigger during a run.
          ta.pulse( 75, 500, 1000L, batt_colour );
          ta.batteryLow = false;

      } else if( !ta.isPulsing() )
          ta.post_color(act?touch_colour:idle_colour);
      break;
    default:
    break;
  }
}

static void master_loop()
{
    static Mode_t last_mode = mode;
    static int counter = 0;
    
    if (last_mode != mode)
    {
        if (mode == FREEFORM_M)
        {
            srnd( rndHW() );
            writeToPhone("RR.\r\n");
        }
        if (mode == PATTERN_M)
            writeToPhone("SP.\r\n");    
    }
    
    last_mode = mode;
    
    ta.spin();
    //spinButtons();
    //if((logging || micros() < dist_timeout) && ta.recieve(m_in))
     //   writeToPhone("CMD: %c",m_in->command);
   // else 
        spin();  
}

void loop()
{
    if (is_master)
    {
        master_loop();
    }
    else
    {
        slave_loop();
    }
}

static void getNext()
{
    if(mode == FREEFORM_M)
    {
      active_cone = getRandomCone(); 

      if(Dbg)
      { 
         writeToPhone("GN_AC: %d\r\n", active_cone);
      }
      
      mask = 0x07;
      ta.setMask(mask & LIGHTS);
      timeout = ~0;
      
      return;
  }
  
  active_cone = cones[index];
  mask = masks[index];
  fakeout = mask & FAKEOUT;
  fail_quick = mask & FAIL_QUICK;
  timeout = times[index];
  index++;

  if( Dbg )  writeToPhone("%d %x %d\r\n", active_cone, mask, timeout);

  writeToPhone("Next cone is %u\r\n", active_cone);  // PROTOCOL
}

static void spin()
{
    static patternState_t last_state = START_P;
    static uint8_t last_cone = 255;
    static unsigned long start;
    static unsigned long timer;
    static bool first;  // first should be true when we first enter a state (even if we just exited the same state)
    char command = 'n';
    unsigned long value = 0;  
    
    m_in->command = 0;
    m_in->value = 0;
    m_in->cone = 0;
  
    if(ta.recieve(m_in))
    {
        if(Dbg)
        {  
            writeToPhone("SR: %d '%c'\r\n", m_in->cone, m_in->command);
        }
        
        command = m_in->command;
        value = m_in->value;
    }
  
  first = false;
  
  if(last_state != state_p || new_state)
  {
    if(state_p == START_P)
        writeToPhone("State is START\r\n");          // PROTOCOL
    if(state_p == WAITING_P)
        writeToPhone("State is WAITING\r\n");        // PROTOCOL
    if(state_p == ACTIVE_TARGET_P)
        writeToPhone("State is ACTIVE_TARGET\r\n");  // PROTOCOL
    if(state_p == IDLE_P)
        writeToPhone("State is IDLE\r\n");           // PROTOCOL
    if(state_p == FAIL_P)
        writeToPhone("State is FAIL\r\n");           // PROTOCOL
    if(state_p == SUCCESS_P)
        writeToPhone("State is SUCCESS\r\n");        // PROTOCOL
  
    first = true;
    new_state = false;
  }

  last_state = state_p;
  last_cone = active_cone;
  
  timer = millis() - start;

  switch(state_p)
  {
  case IDLE_P:
    ta.setMask( DEFTOUCHMASK );
    if(!in_menu)
    {
      if( ta.batteryLow && !ta.isPulsing() )  // Temporary feature until batt-low-in-app.
      {
          ta.pulse( 75, 500, 1000L, batt_colour );
          ta.batteryLow = false;

      } else if( !ta.isPulsing() )
          ta.post_color((ta.activated())?touch_colour:idle_colour);
    }
    break;
      
  case START_P:
    clearCones();
    tag_start = false;
    ta.post_color(idle_colour);
    index = 0;
    getNext();
    
    if(timeout == 0)
    {  // timeout of 0 means wait indefinitely for the first cone to be activated, then start the rest of the sequence
     timeout = ~0;
     tag_start = true; 
//     DEBUG("Activate cone ");   // May use this again in future.
//     DEBUG("%d",active_cone);
//     DEBUG(" to start.\n");
     if( Dbg )  writeToPhone("ACS: %d\r\n", active_cone);
    }
    
    start = millis();
    state_p = stateFromCone(active_cone);
    if(active_cone == 0)
    {  // in this case stateFromCone returns SUCCESS_P, but really there was no course sequence
      DEBUG("\n");
      DEBUG("Empty course sequence!\n");
      writeToPhone("ECS\r\n");
      ta.post_color(0x0000FF);
      state_p = IDLE_P;
      break;
    }
    break;
    
  case WAITING_P:
    if(first)
    {
      m->command = 'm';
      m->value = mask;
      m->cone = active_cone;
      ta.send(m);

////      wait_ms(50);
///      wait_ms(300);
      m->command = 't';
      m->value = (uint32_t)timeout;
      ta.send(m);

////      wait_ms(50);
///      wait_ms(300);
      m->command = 'g';
      ta.send(m);
      
      if(Dbg)
      {  
        writeToPhone("ACW: %d\r\n", active_cone);
      }
    }
    
    ta.post_color(( ta.activated())?touch_colour:idle_colour);
    
    DEBUG("Waiting: %d %d %c\n", m_in->cone, active_cone, command);
    
    if((m_in->cone == active_cone && command == 'd' ) || ((timer >= timeout) && fakeout))
    {
      if((timer >= timeout) && !fakeout)
      {
        writeToPhone("Timesplit penalty! ");  // PROTOCOL
      }
      printSplit(timer);                      // PROTOCOL
      getNext(); 
      start = millis();
      
      state_p = stateFromCone(active_cone); 
      
      if(state_p == WAITING_P) 
      {
        if(Dbg)
        {  
            writeToPhone("NSW\r\n");
        }
        
        new_state = true;
      }
    }
    else if(~timeout != 0 && mode == PATTERN_M && timer >= (timeout + ((fail_quick)?0:GRACE_PERIOD)))
    {
      if(Dbg)
      {  
        writeToPhone("FW\r\n");
      }
      state_p = FAIL_P;
    }
    break;
    
  case ACTIVE_TARGET_P:
    if(first)
    {
      ta.setMask(mask);
      
      warning = false;
      penalty = false;
///   ta.post_color(active_colour);
      ta.pulse(50,750,~0L,active_colour);
      //if(!(mask & SILENT))ta.pulse(50,750,~0L,active_colour);
    }
    if(timer >= timeout)
    {
      if(!penalty)
      {
///     ta.post_color(pen_colour);
        ta.pulse(50,325,~0L,pen_colour);
        
        penalty = true;
      }
      ta.post_color(pen_colour);
    }
    else if(timer > ((timeout*3)/4) && !warning)
    {
      warning = true;
///   ta.post_color(warn_colour);
      ta.pulse(50,750,~0L,warn_colour);
    }

    if(ta.activated() || ((timer >= timeout) && fakeout))
    {
      if((timer >= timeout) && !fakeout)
        writeToPhone("Timesplit penalty! ");  // PROTOCOL
      printSplit(timer);                      // PROTOCOL

      ta.post_color(success_colour);
      getNext();
        start = millis();
      state_p = stateFromCone(active_cone); 
      
      if(state_p == ACTIVE_TARGET_P)
        new_state = true;
      ta.pulse_off();
      
    }
    else if(~timeout != 0 && mode == PATTERN_M && timer >= (timeout + ((fail_quick)?0:GRACE_PERIOD)))
    {
      state_p = FAIL_P;
      ta.pulse_off();
    }
    //DEBUG.println(timeout + ((fail_quick)?0:GRACE_PERIOD));
    //DEBUG.println(timeout);
    
    break;
    
  case FAIL_P:
    if(first)
    {
      start = millis();
      DEBUG("\n");
      DEBUG("Timeout\n");
      //ta.post_color();
      ta.pulse(25,200,3000,fail_colour);
///   ta.post_color(fail_colour);
      
      failCones(); 
      //ta.fail();
    }
    else if(timer > 3000)
    {
      DEBUG("Clear!\n");
      state_p = IDLE_P;
      clearCones();
    }
    break;
  case SUCCESS_P:
    if(first)
    {
      start = millis();
      DEBUG("\n");
      DEBUG("Done!\n");
      ta.post_color(success_colour);
      ta.beep(1500);
      successCones(); 
      //ta.success();
    }
    else if(timer > 1500)
    {
      DEBUG("Clear!\n");
      state_p = IDLE_P;
      clearCones();
    }
    break;
    
  default:
    ta.post_color(idle_colour);
    break;
  }  
}

void getRadioInput(char *ibuffer, int size)
{
  static int i = 0;
  int buffer_counter = 0;
  static char parameter = '_';
  static char buffer[MAX_LEN + 1];
  int value = 0;
  char *endp = NULL;

  // listen for commands coming over bluetooth
  while (buffer_counter < size)
  {
    char ch = ibuffer[buffer_counter++];

    if((ch == '\r' || ch == ';' || ch == '\n') && parameter != '_')
    {
      if(i > 1)
      {
        buffer[i-1] = 0;        
        if(parameter == 'l')
            value = strtoul(buffer, &endp, 2);
        else if(parameter == 'B')
            value = strtoul(buffer, &endp, 16);
        else
            value = atoi(buffer);
      }

      if (is_master)
      {
        interpret_master(parameter, value);
      }
      else
      {
          interpret_slave(parameter, value);
      }
      
      DEBUG("After interp: '%c'\r\n", parameter);
      parameter = '_';
      buffer[0] = 0;
      i=0;
    }
    else
    {
      if(i==0)
        parameter = ch;
      else 
        buffer[i-1] = ch;
      i++;
    }
    
    if(ch == '_' || ch == '\r' || ch == ';' || ch == '\n')
    {
      parameter = '_';
      buffer[0] = 0;
      i=0;
    }
  }
}

static void interpret_slave(char parameter, int value)
{
    switch(parameter)
    {
        case 'M':
            datastore_set_master();
            SL_DEBUG("Power Cycle\r\n");
            break;
        case 's':
            SL_DEBUG("MS: %d\r\n", datastore_is_master());
            SL_DEBUG("RP: %d\r\n", read_params());
            break;
    }   
}

static void interpret_master(char parameter, int value)
{
  int remainder;
  uint16_t split;
  uint16_t t;
  uint8_t c;
  uint8_t l;
  int last;
  int middle;
  uint8_t length;
  uint8_t offset;
  int i;
  uint8_t v;
  uint16_t val;
  
  DEBUG("data\r\n");
  
  switch(parameter)
  {
  case 'f':
    if(lonely)
    {
      writeToPhone("Sorry, no other cones detected, please try detecting cones first.\r\n");
      break;
    }
    
    writeToPhone("Entered freeform\r\n");
    mode = FREEFORM_M;
    state_p = START_P;
    clearCones();
    //find_cones();
    break;
  case 'p':
    if(value == 0)
    {
      writeToPhone("\r\n");
      writeToPhone("Running pattern %d\r\n", active_sequence + 1);
      
      mode = PATTERN_M;
      state_p = START_P;
      active_cone = 0;
      
      clearCones();
    }
    else
    {
      writeToPhone("Selected pattern %d\r\n", value);
      
      if(value <= SEQUENCES && value > 0)
      {
        active_sequence = value - 1;
        cones = (uint8_t*)cone_table + (value-1)*STATIONS;
        times = (uint16_t*)time_table + (value-1)*STATIONS;
        masks = (uint8_t*)mask_table + (value-1)*STATIONS;
        
        if (Dbg)
        {
            for (int i = 0; i < STATIONS; ++i)
            {
                writeToPhone("PS: %d %x %d\r\n", cones[i], masks[i], times[i]);    
            }
        }
      }
      else
      {
        writeToPhone("This pattern is not available.  Please select a value between 1 and %d\r\n", SEQUENCES);
      }
    }
    break;
  case 'P':
    if(value != 0)
    {
      if(value <= SEQUENCES && value > 0)
      {
        active_sequence = value - 1;
        cones = (uint8_t*)cone_table + (value-1)*STATIONS;
        times = (uint16_t*)time_table + (value-1)*STATIONS;
        masks = (uint8_t*)mask_table + (value-1)*STATIONS;

        // Side effect:  Also sets current station to 1:
        station = 1;
      }
      else
      {
        writeToPhone("Only from 1 to %d\r\n", SEQUENCES);
      }
    }
    else  // P; or P0;
    {
      writeToPhone("Nothing done\r\n");
    }
    break;
  case 's':
    writeToPhone("Selected station %d\r\n", value);
  case 'S':
    station = value;
    break;
  case 'd':
    if(value == 0){
      logging = false;
      m->value = 0;
      dist_timeout = micros() + 2000000;
    }
    if(value == 1){
      logging = true;
      m->value = 1;
    }
    m->command = 'd';
    m->cone = TRILAT_CONE;
    ta.send(m);
    //Serial.print("Sent d");
    //Serial.println(value);
    break;
  case 'c':
    writeToPhone("Station %d will be cone %d\r\n", station, value);
  case 'C':
    c = value;
    if(station <= STATIONS && station > 0)cones[station-1] = c;
    break;
  case 't':
    t = (uint16_t)value;
    remainder = t%1000;
    writeToPhone("Station %d split time is: %d.%03d seconds.\r\n", station, t/1000, remainder);
  case 'T':
    t = (uint16_t)value;
//  remainder = t%1000;
///    if(remainder < 100)writeToPhone("0");
///    if(remainder < 10)writeToPhone("0");
///    writeToPhone("%d seconds.\r\n",remainder);
    if(station <= STATIONS && station > 0)times[station-1] = t;
    break;
  case 'l':
//  l = 0;
    l = (uint8_t)value;
    masks[station-1] = l;
    writeToPhone( "Station %d config bits: ", station );
    writeBitsToPhone( l );
    writeToPhone( "\r\n" );
    break;
  case 'B':  // After setting bits, echo back station data.
    l = (uint8_t)value;
    masks[station-1] = l;
    writeToPhone( "%c%02u;C%02u;T%05u;B%02x\r\n", (1==station) ? 'P'                 : 'S',
                                                  (1==station) ?  active_sequence +1 : station,
                                                  cones[station-1], times[station-1], l );
    break;
  case 'q':
    state_p = IDLE_P;
    new_state = true; // force state reporting, even if we're already in IDLE
    ta.pulse_off();  /// //
    clearCones();    /// //
    break;
  case 'r':
    //Serial.println(F(""));
    // writeToPhone("Current pattern is %d:\r\n", active_sequence+1);
    writeToPhone("Pattern is %d:\r\n", active_sequence+1);  // PROTOCOL optimization
    for(int i=0; i<STATIONS; i++){
      // writeToPhone("Station %d: cone %d, ", i+1, cones[i]);
      writeToPhone("S%02u,C%02u", i+1, cones[i]);           // PROTOCOL optimization
      split = times[i];
      // printMsAsSeconds(split);               // Removed for PROTOCOL optimization
      //Serial.print(F("s timeout, lights: "));
      // writeToPhone("s timeout, config bits: ");
      writeToPhone(",T%05u", split);                        // PROTOCOL optimization
      l = masks[i];
      /*
      if(l<0b10000000)
        writeToPhone("0");
      if(l<0b1000000)
        writeToPhone("0");
      if(l<0b100000)
        writeToPhone("0");
      if(l<0b10000)
        writeToPhone("0");
      if(l<0b1000)
        writeToPhone("0");
      if(l<0b100)
        writeToPhone("0");
      if(l<0b10)
        writeToPhone("0");
        */
      // writeBitsToPhone( l, 3 );              // Removed for PROTOCOL optimization
      writeToPhone(",B%02x", l);                            // PROTOCOL optimization
      writeToPhone( "\r\n" );
      //Serial.println(l, BIN);
    }
    break;
  case 'u':
    // let any pending messages clear
    while(ta.get_buffer_size()){
      ta.spin();
    }
    if(value == 1){
      writeToPhone("Course leader!\r\n");
      powerupCones(value);
      ta.powerup(value);
    }
    if(value == 2){
      writeToPhone("Split leader!\r\n");
      powerupCones(value);
      //ta.powerup(value);
    }
    if(value > 10 && value < 5000){
      ta.beep(value);
    }
    break;
  case 'w':
    if(value > 0 && value <= SEQUENCES)
    {
      length = STATIONS;
      offset = (value - 1) * STATIONS;
      datastore_write_pattern(value-1);
      writeToPhone("Saved sequence %d\r\n", value);
    }
    else
    {
      datastore_write_patterns();
      writeToPhone("Saved all sequences.\r\n");
    }
    break;
   case 'x':
     resetSensors();
     ta.resetTouch();
     break;
#ifdef D
    case '5': // DEBUG CODE!
       write_test_pattern();
       datastore_write_patterns();
       break;
#endif
   case 'z':
    find_cones();
    break;
  }
}

static patternState_t stateFromCone(uint8_t cone)
{
  if(cone == 0 || index == STATIONS) 
    return SUCCESS_P;
  if(cone == 1) 
    return ACTIVE_TARGET_P;
    
  return WAITING_P;
}

static void printSplit(unsigned long timer)
{
  if(tag_start)
  {
    writeToPhone( "Sequence started at cone %u (", active_cone );  // PROTOCOL
    printMsAsSeconds(timer);                                       // PROTOCOL
    writeToPhone( " seconds).\r\n" );                              // PROTOCOL
    tag_start = false;
  }
  else
  {
    writeToPhone( "Target cone is: %u, ", active_cone );           // PROTOCOL
    printMsAsSeconds(timer);                                       // PROTOCOL
    writeToPhone( " split\r\n" );                                  // PROTOCOL
  }
}

static uint8_t getRandomCone(void)
{
  static uint8_t last_cone = 0;
  static uint8_t ac[NUM_CONES] = {1};

  uint8_t cone;

  uint8_t avail = 0;
  for( uint8_t i=1; i<=NUM_CONES; i++ )
  {
      if( ((1==i) ? true : active_cones[i]) && (i != last_cone) )  ac[avail++] = i;
  }

  cone = ((avail != 0) ? ac[rnd() % avail] : 0);

  writeToPhone( "Target cone is %u\r\n", cone );  // PROTOCOL
  
  last_cone = cone;

  return  cone;
}

static void clearCones(void)
{
  uint8_t i;
  m->command = 'q';
  
  for(i=2;i<NUM_CONES+1;i++)
  {
    m->cone = i;
    if(active_cones[i])
        ta.send(m);
///    active_cones[i] = false;
    //ta.send("q", i);
  }
  
  DEBUG("sent clear\r\n"); 
}

static void powerupCones(uint8_t sound)
{
  uint8_t i;
  m->command = 'u';
  m->value = sound;
  
  if(sound == 2)
  {
    if(active_cone == NODE_ID)
    {
        ta.powerup(sound);
    }
    else
    {
      m->cone = active_cone;
      ta.send(m);
    }
  }
  else
  {
    for(i=2;i<NUM_CONES+1;i++)
    {
      m->cone = i;
      if(active_cones[i])
        ta.send(m);
      //ta.send("f", i);
    }
  }
  //DEBUG("sent powerup");
}

static void failCones(void)
{
  uint8_t i;
  m->command = 'f';
  
  for(i=2;i<NUM_CONES+1;i++)
  {
    m->cone = i;
    
    if(active_cones[i])
        ta.send(m);
    //ta.send("f", i);
  }
  
  while(ta.get_buffer_size())
  {
    ta.spin();
  }
  
  //DEBUG("sent fail\n");
}

static void resetSensors(void)
{
  uint8_t i;
  m->command = 'x';
  
  for(i=2;i<NUM_CONES+1;i++)
  {
    m->cone = i;
    if(active_cones[i])
        ta.send(m);
    //ta.send("f", i);
  }
  
  DEBUG("sent sensor reset\n");
}

static void successCones(void)
{
  uint8_t i;
  m->command = 's';
  
  for(i=2;i<NUM_CONES+1;i++)
  {
    m->cone = i;
    if(active_cones[i])
        ta.send(m);
    //ta.send("s", i);
  }
  
  while(ta.get_buffer_size())
  {
    ta.spin();
  }
}

static void find_cones(void)
{
  while(ta.get_buffer_size())
  {
    ta.spin(); // wait for all messages to leave queue
  }
  
  ta.beep_off();
  uint8_t i;
  m->command = 'z';
  
  lonely = true;
  
  if(Dbg)
  {
      writeToPhone("FC\r\n");
  }
  
  int retry_count = 0;

  for(i = 2; i < NUM_CONES + 1; ++i)
  {
      retry_count = 0;
go_again:
    active_cones[i] = false;
    m->cone = i;
    ta.send_immediate(m);

    unsigned long st = millis();
    unsigned long delta = 0L;
   /* ///
    unsigned long current = 0L;
    
    if(Dbg)
    {  
        writeToPhone("S: %lu F: %d\r\n", st, i);
    }

    while(1)
    {
      current = millis();
      delta = current - st;
      
      if(delta > 300L) 
      {
        if(Dbg)
        {  
            writeToPhone("FT: %d %lu\r\n", i, current);
        }
        
        if (++retry_count < 3)
        {
            goto go_again;
        }
        else
        {
            break;
        }
      }
      if(ta.recieve(m_in))
      {
        lonely = false;
        active_cones[m_in->cone] = true;
        
        if(Dbg)
        {  
            writeToPhone("FS: %d\r\n", m_in->cone);
        }
        break;
      }
    }
    
    wait_us(50);
   */ ///
    while( 1 ) ////
    {
      delta = millis() - st;
      if( delta > 50 )  break;
      ta.spin();
      if( ta.recieve( m_in ) )
      {
        lonely = false;
        active_cones[m_in->cone] = true;
        DEBUG( "Reply from cone: %d, %dms\r\n", m_in->cone, delta );
        break;
      }
    }
  }

  writeToPhone("Available cones are: (1");
  
  for(i = 2; i < NUM_CONES + 1; ++i)
  {
    if(active_cones[i])
    {
      writeToPhone(", %d", i);
    }
  }
  
  writeToPhone(")\r\n");
}

static void printMsAsSeconds(unsigned long number)
{
  uint16_t remainder;
  
  remainder = number%1000;

  writeToPhone( "%d.%03d", number/1000, remainder );  /// Needs to stay without CRLF.
}

 static void spinButtons(void)
 {
  static inputState_t last_state = IDLE_I;
  static bool first_i;  // first should be true when we first enter a state (even if we just exited the same state)
  static Event event;
  static uint8_t sequence = 0;
  static unsigned long start = 0;
  uint8_t section = 0;

  //uint8_t buttons;
  
  //timer = millis() - start;
  if(millis() > 500)
    event = getInputEvent();
    
  if(event.type == Event::press)
  {
    in_menu = true;
    state_i = MENU_I;
  }

  first_i = false;
  
  if(last_state != state_i)
  {
    //if(state_i == IDLE_I)////////////DEBUG.println(F("");
    //if(state_i == RUNNING_I)//////////////DEBUG.println(F("State is WAITING");
    // need to print menu timeout?
    if(state_i == MENU_I)
        ////DEBUG("Menu\n");
    if(state_i == PATTERN_SELECT_I)
        ////DEBUG("Choosing pattern");
    first_i = true;
  }

  last_state = state_i;
  
  if(state_i == RUNNING_I)
    in_menu = false;
  
  switch(state_i)
  {
  case IDLE_I:
    // display something distinctive
    if(event.type == Event::tap)
    {
      state_i = RUNNING_I;
      //send 'p' or 'f' command as appropriate
      if(mode == FREEFORM_M)
        interpret_master('f', 0);
      else 
        interpret_master('p', 0);
    }
    
    break;
  case RUNNING_I:
    if(event.type == Event::finish) 
        state_i = IDLE_I;
    break;
  case MENU_I:
    if(first_i)
        interpret_master('q', 0);
    // every 3 seconds we cycle through the menu
    // one option per second
    // light changes color and stays on for 500ms
    section = ((millis() - start)%3000)/500;
    
    if(section == 0) 
        ta.post_color(0xFF);
    if(section == 1) 
        ta.post_color(0);
    if(section == 2) 
        ta.post_color(0xFF00);
    if(section == 3) 
        ta.post_color(0);
    if(section == 4) 
        ta.post_color(0xFF0000);
    if(section == 5) 
        ta.post_color(0);
    if(event.type == Event::tap)
    {
      // set state here based on current light color
      if(section < 2)
      {
        mode = FREEFORM_M;
        ta.post_color(0xFF);
        ////////DEBUG.println(F("Freeform mode.");
        state_i = IDLE_I;
        break;
      }
      
      if(section < 4)
      {
        mode = PATTERN_M;
        ta.post_color(0xFF00);
        ////////DEBUG.println(F("Pattern mode.");
        state_i = IDLE_I;
        break;
      }
      
      if(section < 6)
      {
        state_i = PATTERN_SELECT_I;
        ta.post_color(0xFF0000);
        break;
      }
    }
    break;
  case PATTERN_SELECT_I:
    if(first_i)
    {
      sequence = active_sequence;
      mode = PATTERN_M;
      //start = millis();
    }
    if(event.type == Event::tap)
    {
      ta.beep(50);
      if(event.value == 1 && sequence > 0)
      {
        sequence--;
      }
      
      if(event.value == 2 && sequence < SEQUENCES - 1)
      {
        sequence++;
      }
      
      if(event.value == 3)
      {
        interpret_master('p', sequence + 1);
        state_i = IDLE_I;
      }
      
      if(event.value != 3)
      {
        ////DEBUG("Menu says: sequence ");
        ////DEBUG("%d\n",sequence + 1);
      }
      break;
    case TEACH_I:
      // not implemented yet
      break;
    default: break;    
    }
  }
}

static uint8_t checkButtons(void)
{
  static unsigned long last_time = 0;
  //static uint8_t last = 0;
  static uint8_t buttons = 0;
  
  // listen for commands from the buttons
  unsigned long time = millis();
  // only check every DEBOUNCE_MS to avoid jitter
  if(time - last_time > DEBOUNCE_MS)
  {
    last_time = time;
    buttons = ta.buttons();
  }
  
  return buttons;
}

static Event getInputEvent(void)
{
  static uint8_t last_buttons = 0;
  static unsigned long time1 = 0;
  static unsigned long time2 = 0;
  static unsigned long time3 = 0;
  unsigned long duration = 0;
  uint8_t buttons = 0;
  Event event;
  event.type = Event::none;
  event.value = 0;

  buttons = checkButtons(); 
  uint8_t rising = buttons & ~last_buttons;
  uint8_t falling = ~buttons & last_buttons;

  /*if(rising){
    //DEBUG.print("Rising ");
    //DEBUG.println(rising);
  }
  if(falling){
    //DEBUG.print("Falling ");
    //DEBUG.println(falling);
    }*/
  if(rising & 0x01)
    time1 = millis();
  if(rising & 0x02)
    time2 = millis();
  if(rising & 0x04)
    time3 = millis();
  
  // simultaneous falling edges will cause lower values to be ignored
  if(falling & 0x01)
  {
    duration = millis() - time1;
    event.value = 1;
  }
  
  if(falling & 0x02)
  {
    duration = millis() - time2;
    event.value = 2;
  }
  
  if(falling & 0x04)
  {
    duration = millis() - time3;
    event.value = 3;
  }
  
  if(duration > 0)
    event.type = Event::tap;
  if(duration > 2000)
    event.type = Event::press;
  
  // give feedback that we've waited lng enough for a press
  unsigned long t = millis() - time1;
  
  if(2020 > t && t > 2000 && (buttons & 0x01)) 
    ta.beep(20);
    
  t = millis() - time2;
  
  if(2020 > t && t > 2000 && (buttons & 0x02)) 
    ta.beep(20);
    
  t = millis() - time3;
  
  if(2020 > t && t > 2000 && (buttons & 0x04))
     ta.beep(20);

  if(event.type != Event::none)
  {
    //DEBUG("Event: ");
    if(event.type == Event::tap) 
        ////DEBUG("tap, ");
    if(event.type == Event::press) 
        ////DEBUG("press, ");
        
    uint8_t val = event.value;
    //DEBUG("%d\n",val);
    //////DEBUG.println(duration);
  }
  
  last_buttons = buttons;
  return event;
}