/* misc.c */

/* sensor port defines */
#define PORT_DISTANCE_REAR_LEFT 4
#define PORT_DISTANCE_REAR_RIGHT 2
#define PORT_DISTANCE_RADAR 5
#define PORT_SHAFT_LEFT 8
#define PORT_SHAFT_RIGHT 7
#define PORT_CDS_ARRAY_START 16
#define PORT_CDS_FRONT_LEFT 16
#define PORT_CDS_FRONT_RIGHT 17
#define PORT_CDS_REAR_LEFT 18
#define PORT_CDS_REAR_RIGHT 19
#define PORT_CDS_CENTER 20
#define PORT_POT_ARM 22
#define PORT_POT_RADAR 31
#define PORT_TOUCH_REAR_RIGHT 9
#define PORT_TOUCH_REAR_LEFT 10
#define PORT_TOUCH_REAR_CENTER 11
#define PORT_IR_N 12
#define PORT_IR_E 13
#define PORT_IR_S 14
#define PORT_IR_W 15

/* motor port defines */
#define MOTOR_FRONT_LEFT 0
#define MOTOR_FRONT_RIGHT 1
#define MOTOR_REAR_LEFT 2
#define MOTOR_REAR_RIGHT 3
#define MOTOR_REAR_CENTER 4
#define MOTOR_RADAR 5

/* servo port defines */
#define SERVO_LEFT 0
#define SERVO_RIGHT 1
#define SERVO_ARM 2

/* distance between front and back axes
   units of back_axis_lenght/2 */
#define ROBOT_LENGTH (2.0*21.0/23.0)

/* back_axis_lenght/2 in inches */
#define HALF_RADIUS_IN_INCHES (11.5*5.0/16.0)

/* inches per breakbeam pulse */
#define INCHES_PER_PULSE (15.0*PI/8.0/108.0)

#define DISTANCE_SENSOR_MAX 30.0
#define DISTANCE_SENSOR_MAX_INT 300
#define DISTANCE_SENSOR_SHIFT 2.5
#define BALL_RADIUS 2.0

#define BOARD_MAX_X .0
#define BOARD_LINE_X 36.0
#define BOARD_MAX_Y 24.0
#define BOARD_BALL_1_Y -18.0
#define BOARD_BALL_2_Y 18.0

#define DIST_CDS_START_X 3.5

/* trig stuff */
#define _2PI 6.2831853072
#define PI 3.1415926536
#define PI_2 1.570796327
#define RAD_PER_DEG (PI/180.0)
#define DEG_PER_RAD (180.0/PI)

/* only one of these */
/* 1 */
/*#define QUICK_TRIG */
/* 2 */
#define qsin sin
#define qcos cos

#define DEBUG

/*int abs(int i)
{
  if (i>0) return i;
  else return -i;
}*/

float sqr(float x)
{
  return x*x;
}

float fabs(float f)
{
  if (f>0.0) return f;
  else return -f;
}

/* measured in units of 1/10 of an inch */
int distance(int port_val)
{
  float d = -3.16+950.0 * (1.0/(8.58+(float)port_val));
  if (d < DISTANCE_SENSOR_MAX)
    return (int)(d*10.0);
  else
    return DISTANCE_SENSOR_MAX_INT;
}

int pre_init()
{
  init_motors();
  init_servos();
  init_nav();
#ifdef QUICK_TRIG
  init_qtrig();
#endif
  init_cds();
  return 0;
}

int post_init()
{
  enable_servos();
  set_digital_out(6);
  start_process(nav_process());
  init_radar();
}