File content as of revision 35:c1405da88d3a:

#include <TA.h>
#include <types.h>

#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 MAX_LEN          24  // buffer input commands up to this length
#define NUM_CONES         3
#define STATIONS         20  // max length of a pattern
#define SEQUENCES         9  // number of patterns to store

#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 */

#if 1
extern int random(int numberone, int numbertwo);

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;

#ifdef MASTER
static Mode_t mode = PATTERN;
static patternState_t state_p = IDLE_P;
static inputState_t state_i = IDLE_I;
#else
State_t state = IDLE;
#endif

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 = 0;
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;

// all sequence data
uint8_t cone_table[STATIONS * SEQUENCES];
uint8_t mask_table[STATIONS * SEQUENCES];
uint16_t time_table[STATIONS * SEQUENCES];

// pointer to active table
uint8_t *cones = (uint8_t*)cone_table;
uint8_t *masks = (uint8_t*)mask_table; 
uint16_t *times = (uint16_t*)time_table;

static bool lonely = false;

// Function prototypes
#ifdef MASTER
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(char parameter, int value);
#endif

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

char local_input[50] = {0};

//DigitalOut red(RED);
//DigitalOut enable1(ENABLE_1);
//DigitalOut green(GREEN);
//DigitalOut blue(BLUE);
//DigitalOut enable2(ENABLE_2);
//DigitalOut enable3(ENABLE_3);

#ifdef MASTER
static void master_setup()
{
    ta.initialize(NODE_ID);
  
  uint16_t sent = 0;
  uint16_t lost = 0;
  unsigned long start = micros();
  //while (millis() - now <= ACK_TIME){
  m->command = 'p';
  m->value = 0;
  m->cone = 0;
  
  
  //find_cones(); // this causes the beep to be interrupted
  //ta.beep(100);  
  int i;
  
  for(i = 1; i < NUM_CONES + 1; ++i)
  {
    active_cones[i] = false;
  }
    
  unsigned long time = 0;
  
  // pull course sequences from non-volatile memory  
  for(uint8_t i = 0; i <STATIONS * SEQUENCES; ++i)
  {
    cone_table[i] = i+1;
    mask_table[i] = 1;
    time_table[i] = 1000;
  }    
  
  ta.post_color(0xFF0000);
}
#else
static void slave_setup()
{
    ta.initialize(NODE_ID);
    ta.beep(250);
}
#endif

void setup()
{
    #ifdef MASTER
    master_setup();
    #else
    slave_setup();
    #endif    
}

#ifdef SLAVE
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;
  
    if(last_state != state)
    {
        if(state == ACTIVE_TARGET)
            writeToPhone("State is ACTIVE_TARGET\n");
        if(state == IDLE)
            writeToPhone("State is IDLE");
        if(state == FAIL)
            writeToPhone("State is FAIL");
        if(state == SUCCESS)
            writeToPhone("State is SUCCESS");
    }
     
    last_state = state;

    ta.resetTouchIfStuck();
  
    char message = 'n';
    timer = millis() - start;
  
    if(ta.recieve(m_in))
    {
        //ta.beep(1000);
        
        writeToPhone("got message\r\n");
        
        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')
  {
    //a3 = 0;
    wait_ms(100);
    //a3 = 1;
  }
  
  if(message == 'f')
  { // Fail
     //serial.printf("Fail!\n");
     ta.pulse(25,200,3000,0xFF0000);
     state = FAIL;
  }
  
  if(message == 's')
  {  // Success
    //serial.printf("Success!\n");
    ta.post_color(0x00FF00);
    ta.beep(1500);
    state = SUCCESS;
  }

  if(message == 'g')
  { // This cone is the active target, GO!
    start = millis();
    warning = false;
    penalty = false;
    ta.pulse(50,750,~0L,0x00FF00);
    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
    writeToPhone("Clear!\r\n");
    ta.pulse_off();
    ta.mask_color(0xFF0000);
    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);
  }
  
  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.pulse_off();
        state = IDLE;
        //serial.printf("Done!\n");
      }
      else if(timer > timeout && !penalty)
      {
        //serial.printf("Penalty!\n");
        penalty = true;
        ta.pulse(50,325,~0L,0xFF00FF);
      }
      else if(timer > ((timeout*3)/4) && !warning)
      {
        //serial.printf("Warning!\n");
        warning = true;
        ta.pulse(50,750,~0L,0xFFFF00);
      }
      break;
    case IDLE:
      ta.post_color((ta.activated())?0xFF:0xFF0000);
      break;
    default:
    break;
  }
}
#endif

#ifdef MASTER
static void master_loop()
{
    static Mode_t last_mode = mode;
    static int counter = 0;
    
    if (last_mode != mode)
    {
        if (mode == FREEFORM)
            writeToPhone("Now running random routes.\n");
            
        if (mode == PATTERN)
            writeToPhone("Now running set patterns.\n");    
    }
    
    last_mode = mode;
    
    ta.spin();
    //spinButtons();
    if((logging || micros() < dist_timeout) && ta.recieve(m_in))
        writeToPhone("%c",m_in->command);
    else 
        spin();  
}
#endif  

void loop()
{
    #ifdef MASTER
    master_loop();
    #else
    slave_loop();
    #endif
}

static void getNext()
{
    #ifdef MASTER
    if(mode == FREEFORM)
    {
      DEBUG("get next freeform\r\n");
      ++active_cone;//getRandomCone(); 
      if (active_cone > 6)
      {
        active_cone = 1;    
      }
      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++;
  DEBUG("Next cone is \n");
  DEBUG("%d\n",active_cone);
  #endif
}
#if 1
static void spin()
{
    #ifdef MASTER
    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))
    {
        DEBUG("spin received: %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)
        DEBUG("State is START\n");
    if(state_p == WAITING_P)
        DEBUG("State is WAITING\n");
    if(state_p == ACTIVE_TARGET_P)
        DEBUG("State is ACTIVE_TARGET\n");
    if(state_p == IDLE_P)
        DEBUG("State is IDLE\n");
    if(state_p == FAIL_P)
        DEBUG("State is FAIL\n");
    if(state_p == SUCCESS_P)
        DEBUG("State is SUCCESS)\n");
  
    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)
        ta.post_color((ta.activated())?0xFF:0xFF0000);
    break;
      
  case START_P:
    //clearCones();
    tag_start = false;
    ta.post_color(0xFF0000);
    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 ");
     DEBUG("%d",active_cone);
     DEBUG(" to start.\n");
    }
    
    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");
      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);
      m->command = 't';
      m->value = (uint32_t)timeout;
      ta.send(m);
      m->command = 'g';
      ta.send(m);
      
      DEBUG("m_in cone=%d\n", active_cone);
      m_in->cone = active_cone;
      command = 'd';
      
    }
    
    ta.post_color(( ta.activated())?0xFF:0xFF0000);
    
    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)
      {
        DEBUG("Timesplit penalty! ");
      }
      printSplit(timer);
      getNext(); 
      start = millis();
      
      state_p = stateFromCone(active_cone); 
      
      if(state_p == WAITING_P) 
      {
      DEBUG("New state from waiting\n");
        new_state = true;
        }
    }
    else if(~timeout != 0 && mode == PATTERN && timer >= (timeout + ((fail_quick)?0:GRACE_PERIOD)))
    {
      DEBUG("Failing from wait\n");
      state_p = FAIL_P;
    }
    break;
    
  case ACTIVE_TARGET_P:
    if(first)
    {
      ta.setMask(mask);
      
      warning = false;
      penalty = false;
      ta.pulse(50,750,~0L,0x00FF00);
      //if(!(mask & SILENT))ta.pulse(50,750,~0L,0c00FF00);
    }
    if(timer >= timeout)
    {
      if(!penalty)
      {
        ta.pulse(50,325,~0L,0xFF00FF);
        
        penalty = true;
      }
      ta.mask_color(0xFF00FF);
    }
    else if(timer > ((timeout*3)/4) && !warning)
    {
      warning = true;
      ta.pulse(50,750,~0L,0xFFFF00);
    }

    if(ta.activated() || ((timer >= timeout) && fakeout))
    {
      if((timer >= timeout) && !fakeout)
        DEBUG("Timesplit penalty! ");
        
      printSplit(timer);
      ta.post_color(0xFF0000);
      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 && 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,0xFF0000);
      
      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(0x00FF00);
      ta.beep(1500);
      successCones(); 
      //ta.success();
    }
    else if(timer > 1500)
    {
      DEBUG("Clear!\n");
      state_p = IDLE_P;
      clearCones();
    }
    break;
    
  default:
    break;
  }  
    #endif
}
#endif

#ifdef MASTER
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;
        value = atoi(buffer);
        
        if(parameter == 'l')
            value = strtoul(buffer, &endp, 2);
      }

      interpret(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(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;
  
  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;
    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;
      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;
      }
      else
      {
        writeToPhone("This pattern is not available.  Please select a value between 1 and %d", SEQUENCES);
      }
    }
    break;
  case 's':
    station = value;
    writeToPhone("Selected station %d\r\n", 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':
    c = value;
    writeToPhone("Station %d will be cone %d\r\n", station, 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);
///    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 '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);
    for(int i=0; i<STATIONS; i++){
      writeToPhone("Station %d: cone %d, ", i+1, cones[i]);
      split = times[i];
      printMsAsSeconds(split);
      //Serial.print(F("s timeout, lights: "));
      writeToPhone("s timeout, config bits: ");
      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 );
      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;
      writeToPhone("Saved sequence %d\r\n", value);
    }
    else{
      length = STATIONS * SEQUENCES;
      offset = 0;
      writeToPhone("Saved all sequences.\r\n");
    }
    for(i=offset;i<length+offset;i++){
      //eeprom_update_byte(addressConeTable + i, cone_table[i]);
      //eeprom_update_byte(addressMaskTable + i, mask_table[i]);
      //eeprom_update_word(addressTimeTable + i, time_table[i]);
    }
    break;
   case 'x':
     resetSensors();
     //digitalWrite(A3, LOW);
     //delay(100);
     //digitalWrite(A3, HIGH);
     break;
     
   case 'z':
    find_cones();
    /*m->value = value;
    m->command = 'z';
    m->cone = 2;
    Serial.println("sending...");
    ta.send(m);
    Serial.println("sent");*/
    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)
  {
    //DEBUG("Sequence started at cone ");
    //DEBUG("%d",active_cone);
    //DEBUG(" (");
    printMsAsSeconds(timer);
    //DEBUG(" seconds).\n");
    tag_start = false;
  }
  else
  {
   //DEBUG("Target cone is: ");
   //DEBUG("%d",active_cone);
   //DEBUG(", ");
   printMsAsSeconds(timer);
   //DEBUG(" split");
  }
}

static uint8_t getRandomCone(void)
{
  static uint8_t lastCone = 0;
  uint8_t cone;
  
  do
  {
    cone = random(1, NUM_CONES + 1);
  }
  while(cone == lastCone || active_cones[cone] == false);
  
  //DEBUG("Target cone is ");
  //DEBUG("%d\n",cone);
  lastCone = cone;
  return cone;
}

static void clearCones(void)
{
  #if 0
  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"); 
  #endif
}

static void powerupCones(uint8_t sound)
{
  #if 1
  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);
    }
  }
  #endif
  //DEBUG("sent powerup");
}

static void failCones(void)
{
  #if 0
  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");
  #endif
}

static void resetSensors(void)
{
  #if 0
  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");
  #endif
}

static void successCones(void)
{
  #if 0
  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();
  }
  
  //DEBUG("sent success\n");
  #endif
}

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;
  
  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 current = 0L;
    unsigned long delta = 0L;
    
    writeToPhone("S: %lu F: %d\r\n", st, i);  /// DEBUG-only message

    while(1)
    {
      current = millis();
      delta = current - st;
      
      if(delta > 300L) 
      {
        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;
        
        writeToPhone("FS: %d\r\n", m_in->cone);
        break;
      }
    }
    
    wait_us(50);
  }
  
  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 );
}

 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)
        interpret('f', 0);
      else 
        interpret('p', 0);
    }
    
    break;
  case RUNNING_I:
    if(event.type == Event::finish) 
        state_i = IDLE_I;
    break;
  case MENU_I:
    if(first_i)
        interpret('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;
        ta.post_color(0xFF);
        ////////DEBUG.println(F("Freeform mode.");
        state_i = IDLE_I;
        break;
      }
      
      if(section < 4)
      {
        mode = PATTERN;
        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;
      //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('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() | TA::buttonsRising;
    TA::buttonsRising = 0;
  }
  
  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;
}
#endif

#endif