#define rightlight_port 2
#define leftlight_port 3
#define backlight_port 4
#define backleftlight_port 6
#define lightpwr_port 7
#define lightpwr_port2 6
#define rightshaft_port 7
#define leftshaft_port 8
int rightlight,leftlight,backlight,backleftlight,lightrdy=0;
int rightlight_thres,leftlight_thres,backlight_thres,backleftlight_thres;

void light_handler(float delay) {
  int rightlightbuff,leftlightbuff,backlightbuff,backleftlightbuff;
  while(1) {
    clear_digital_out(lightpwr_port);
    clear_digital_out(lightpwr_port2);
    sleep(.012);
    rightlightbuff=analog(rightlight_port);
    leftlightbuff=analog(leftlight_port);
    backlightbuff=analog(backlight_port);
    backleftlightbuff=analog(backleftlight_port);
    set_digital_out(lightpwr_port);
    set_digital_out(lightpwr_port2);
    sleep(.008);
    rightlight=(rightlightbuff-analog(rightlight_port))>rightlight_thres;
    leftlight=(leftlightbuff-analog(leftlight_port))>leftlight_thres;
    backlight=(backlightbuff-analog(backlight_port))>backlight_thres;
    backleftlight=(backleftlightbuff-analog(backleftlight_port))>backleftlight_thres;
    lightrdy=1;
  }
}

void shaft_handler() {
  enable_encoder(rightshaft_port);
  enable_encoder(leftshaft_port);
  reset_encoder(rightshaft_port);
  reset_encoder(leftshaft_port);
}

int read_light(int port,float delay) {
  int light;
  clear_digital_out(lightpwr_port);
  clear_digital_out(lightpwr_port2);
  sleep(delay);
  light=analog(port);
  set_digital_out(lightpwr_port);
  set_digital_out(lightpwr_port2);
  sleep(delay);
  return(light-analog(port));
}

int sample(float sec,int port,float delay) {
  int accum=0,n=0;
  float final_time=seconds()+sec;
  while(seconds()<final_time) {
    printf("\n\n%d",read_light(port,delay));
    accum+=read_light(port,delay);
    n++;
  }
  return(accum/n);
}

void calibrate_light(float delay) {
  while(start_button()) {defer();}
  while(!start_button()) {
    printf("\nLight %d %d %d %d",read_light(rightlight_port,delay),read_light(leftlight_port,delay),read_light(backlight_port,delay),read_light(backleftlight_port,delay));
    sleep(.1);
  }
  rightlight_thres=sample(1.,rightlight_port,delay);
  leftlight_thres=sample(1.,leftlight_port,delay);
  backlight_thres=sample(1.,backlight_port,delay);
  backleftlight_thres=sample(1.,backleftlight_port,delay);
  while(start_button()) {defer();}
  while(!start_button()) {
    printf("\nDark %d %d %d %d",read_light(rightlight_port,delay),read_light(leftlight_port,delay),read_light(backlight_port,delay),read_light(backleftlight_port,delay));
    sleep(.1);
  }
  rightlight_thres+=sample(1.,rightlight_port,delay);
  leftlight_thres+=sample(1.,leftlight_port,delay);
  backlight_thres+=sample(1.,backlight_port,delay);
  backleftlight_thres+=sample(1.,backleftlight_port,delay);
  rightlight_thres/=2;
  leftlight_thres/=2;
  backlight_thres/=2;
  backleftlight_thres/=2;
  printf("\n%d %d %d %d",rightlight_thres,leftlight_thres,backlight_thres,backleftlight_thres);
}


void start_servos() {
  arm(2);
  enable_servos();
  servo(servo_port,servo_turn_left);
}