 #define HIGH_BYTE(x) x>>8
 #define LOW_BYTE(x) x & 0x00FF 
 
-// constants won't change. They're used here to 
-// set pin numbers:
+// constants won't change. They're used here to set pin numbers:
 const int joystick_x_pin = 0;
 const int joystick_y_pin = 1;
 const int button_pin = 2;
 
 volatile uint8_t rxflag = 0; 
+
+// keep the roomba command state
+#define DONTSEND 0
+#define MOVECOMMAND 1
+#define FIREGUN 2
 uint8_t txflag = 0;
 
 // packets are transmitted to this address roomba
 uint8_t roomba_num = 0;
-//uint8_t station_addr[5] = { 0xAB, 0xAB, 0xAB, 0xAB, 0xAB };
 
 // this is our radio's address
 uint8_t my_addr[5] = { 0x18,0x2d,0x44,0x54,0xfb }; 

  Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);
  
  Radio_Set_Tx_Addr(ROOMBA_ADDRESSES[roomba_num]);
  
  // Thanks Neil!  
  Scheduler_Init();
  Scheduler_StartTask(0,20,check_button);
  Scheduler_StartTask(8,20,handle_servo);
  Scheduler_StartTask(5,20,recv_packet_task);
  Scheduler_StartTask(1,200,send_packet_task);
}

void idle(uint32_t idle_period){
  delay(idle_period); 
}

void radio_rxhandler(uint8_t pipe_number)
{
 rxflag = 1;
}

void loop() {
  uint32_t idle_period = Scheduler_Dispatch();
  if( idle_period){
   idle(idle_period); 
 }
}

void setSenderAddress(uint8_t* dest, uint8_t *src,int len){
  for(int i = 0;i < len ;++i){
    dest[i] = src[i];
  }
}

void fireGun(uint8_t b){
  if( txflag == 1){
    if (send_packet.type = IR_COMMAND){
      return;
    }
  }
  send_packet.type = IR_COMMAND;
  
  pf_ir_command_t* cmd = &(send_packet.payload.ir_command);
  setSenderAddress(cmd->sender_address,my_addr,5);  
  cmd->ir_command = SEND_BYTE;
  cmd->ir_data = 0;
  cmd->servo_angle = 0;
  txflag = 1;
}

void moveCommand(int16_t spd, int16_t deg){
  if(txflag == 1){return;}    
  
  // clear_the_packet(&send_packet);
  
  send_packet.type = COMMAND;
  
  pf_command_t* cmd = &(send_packet.payload.command);
  setSenderAddress(cmd->sender_address,my_addr,5);
  
  cmd->command = 137; // DRIVE op code
  cmd->num_arg_bytes = 4;
  
  // clear the arguments array to zero;
//  memset(cmd->arguments,0,sizeof(uint8_t)*16);
  
  // set the  speed
  cmd->arguments[0] = HIGH_BYTE(spd);

radiopacket_t recv_packet;
radiopacket_t send_packet;

// setup a move roomba command packet
// speed = -500 --> 500 mm/sec
// deg = -2000 --> 2000 mm/sec
void moveCommand(radiopacket_t* packet,int spd,int deg);

// setup a roomba ir_command packet
// sending the specified uint8_t byte
void fireGun(radiopacket_t* packet,uint8_t b);

typedef struct _input_t {
  uint16_t button;
  int16_t x_value;
  int16_t y_value;
} input_t;
input_t input;

// read in input, setup a firePacket
void check_button(){
  // read the state of the switch into a local variable:
  input.button = analogRead(button_pin);  
  if( input.button < 50 ){
    if( FIREGUN >= txflag){
      txflag = FIREGUN;      
    }
  }
}

int16_t mapSpeedValue(int16_t val){
  return map(val,0,4,-500,500); 
}

int16_t mapRotValue(int16_t val){
  return map(val,0,4,-200,200);
}

// read in input. setup a move packet
void handle_servo(){ 
  // reads the value of the potentiometer (value between 0 and 1023) 
  input.x_value = map(analogRead(joystick_x_pin), 0, 1023, 0, 5);
  input.y_value = map(analogRead(joystick_y_pin), 0, 1023, 0, 5);

  if( MOVECOMMAND >= txflag){
    txflag = MOVECOMMAND;
  }  
}


// recv_packet code
void recv_packet_task(){
  if(rxflag == 0){return;}
  
  // remember always to read the packet out of the radio, even
  // if you don't use the data.
  if (Radio_Receive(&recv_packet) != RADIO_RX_MORE_PACKETS){
    // if there are no more packets on the radio, clear the receive flag;
    // otherwise, we want to handle the next packet on the next loop iteration.
    rxflag = 0;
  }

  txflag = DONTSEND;
  if( recv_packet.type == SENSOR_DATA){
    Serial.println("SENSOR_DATA");         
  }else if(recv_packet.type == IR_DATA){
    Serial.println("IR_DATA:");
    Serial.println(recv_packet.payload.ir_data.roomba_number);
    Serial.println(recv_packet.payload.ir_data.ir_data);
  }else{
    Serial.println("Unknown recv packet"); 
  }  
}

// send packet code
void send_packet_task(){
  if( txflag == DONTSEND){return;}
  Serial.println("sending packet");

  if(txflag == FIREGUN){
    // send a fire gun packet
    fireGun(&send_packet,'A');
  }else if(txflag == MOVECOMMAND){
    // send a move packet
    // map the x and y values to the correct ranges 
    uint16_t x_value = input.x_value;
    uint16_t y_value = input.y_value;
    if( x_value == 2 && y_value == 2){
      // stop  
      moveCommand(&send_packet,0,(int16_t)0x8000);
    } else if( x_value == 2){
      // move straight
      y_value = -mapSpeedValue(y_value);
      moveCommand(&send_packet,y_value,(int16_t)0x8000);
    } else if( y_value == 2){
      // turn in place
      uint16_t deg = -1;
      if( x_value < 2 ){
        deg = 1;
      }
      moveCommand(&send_packet,200,deg);
    }else{
      x_value = -mapRotValue(x_value); 
      y_value = -mapSpeedValue(y_value);      
      moveCommand(&send_packet,y_value,x_value);
    }
  }

  // transmit the send packet
  Radio_Transmit(&send_packet, RADIO_WAIT_FOR_TX);  
}

void setup() {
