#define TEAM_0_FREQ 100
#define TEAM_1_FREQ 125

int ir_port = 15;

/* Plug a single IR sensor into one of the digital ports 12 - 15,
   Call setup robot with the team number and this port's number */
/* setup_robot must be called before any of the other
   IR functions can be used */
void setup_robot(int team, int setup_port) {
  if (team == 0)
    set_ir_receive_frequency(TEAM_0_FREQ);
  else if (team == 1)
    set_ir_receive_frequency(TEAM_1_FREQ);
  else
    return;

  ir_receive_on();
  ir_port = setup_port;
  ir_counts(ir_port); /* Clear old data */
}

/* Switches the frequency to be recieved to "team"'s frequency */
void switch_robot(int team) {
  if (team == 0)
    set_ir_receive_frequency(TEAM_0_FREQ);
  else if (team == 1)
    set_ir_receive_frequency(TEAM_1_FREQ);
  else
    return;

  ir_counts(ir_port); /* Clear old data */
}

/* Returns the number of consecutive cycles of successful IR reception for
   a given port.  0 indicates no reception.  255 indicates about 2 seconds
   of continuous reception.  Any number higher than 5 should be considered
   solid detection. */
int get_ir() {
  return ir_counts(ir_port);
}

/*****************************************************************************/
/* Low-level functions adapted from 6.270 */

/* routines to control infrared "beacon" transmission and detection */

/***************************************************************/
/* this file was originally r22_ir.c used with the 6.270 board */
/*                                                             */
/* I slightly modified it (and r22_eq.asm and r22_ir.asm were  */
/*   modified and recompiled to a new version of r22_ir.icb,   */
/*   renamed hb_irbeacon.icb) to work with the Handyboard.     */  
/*                                                             */
/* Max Davis, 4/99                                             */
/***************************************************************/

void ir_transmit_on()
{
    bit_set(0x1022, 0b01000000);	/* enable interrupt */
}

void ir_transmit_off()
{
    bit_clear(0x1022, 0b01000000);	/* disable interrupt */
    bit_clear(0x1000, 0b01000000);	/* bit clear == IR off */
    bit_set(0x1026, 0x80);		/* make TOC1 be output */
}

void set_ir_transmit_period(int period)
{
    ir_xmit_period= period;
}
/* Added by Matt Domsch Tue Jan 11 11:24:22 EST 1994 */
void set_ir_transmit_frequency(int f)
{
    ir_xmit_period= (1000/f)*1000;
}
/* Added by Matt Domsch Tue Jan 11 11:24:22 EST 1994 */
void set_ir_receive_period(int period)
{
  ir_freq_rcv_select=(int)((float)1000/(float)period*(float)1000);
}
void set_ir_receive_frequency(int f)
{
    ir_freq_rcv_select= f;
}

int ir_counts(int p)
{
    if (p == 12) return ir_counts_zero;
    if (p == 13) return ir_counts_one;
    if (p == 14) return ir_counts_two;
    if (p == 15) return ir_counts_three;

    return -1;
}

void ir_receive_off()
{
    num_ir_sensors= 0;
}

void ir_receive_on()
{
    num_ir_sensors= 4;
}