#include <conio.h> 
#include <string.h> 
#include <unistd.h> 
#include <dsensor.h> 
#include <stdlib.h> 
#include <unistd.h> 
#include <dmotor.h> 
#include <dsound.h> 
#include <lnp.h> 

#define TH_WHITE 0x2E 
#define TH_BLACK 0x2A 
#define LIGHT_SENSOR SENSOR_3 
#define CHECK_WHITE (LIGHT(LIGHT_SENSOR)>=TH_WHITE) 
#define CHECK_BLACK (LIGHT(LIGHT_SENSOR)<=TH_BLACK) 
#define ARM ROTATION_1 
#define ARM_SENSOR SENSOR_1 
// 80 == 5*16 is the full circle
#define ARM_ORTOG 20 
#define ARM_MAX 35 
#define ARM_PREV 32 
#define ROAD_WIDTH 7 

struct s_road {
  short left;
  short right;
  short error;
};

#define ABS(x) ( x>=0 ? (x) : (-x) ) 

void arm_to(int x) {
  while (ARM!=x) {
    while (ARM!=x) {
      if (ARM>x)
	motor_b_dir(fwd);
      else
	motor_b_dir(rev);
    }
    motor_b_dir(brake);
    delay(50);
  }
}

void arm_calibr() {
  int armright, armleft;
  
  motor_b_speed(60);
  motor_b_dir(fwd);
  armright=ARM;
  delay(50);
  while (ARM!=armright) {
    armright=ARM;
    delay(50);
  }
  motor_b_dir(brake);
  motor_b_dir(rev);
  armleft=ARM;
  delay(50);
  while (ARM!=armleft) {
    armleft=ARM;
    delay(50);
  }
  motor_b_dir(brake);
  motor_b_speed(40);
  arm_to((armright+armleft+1)/2);
  ds_rotation_set(&ARM_SENSOR, 0);
  delay(100);
}

void init() {
  motor_a_dir(off);
  motor_b_dir(off);
  motor_c_dir(off);
  motor_a_speed(255);
  motor_b_speed(30);
  motor_c_speed(255);
  ds_active(&LIGHT_SENSOR);
  ds_active(&ARM_SENSOR);
  ds_rotation_on(&ARM_SENSOR);
  ds_rotation_set(&ARM_SENSOR, 0);
  arm_calibr();
}

void deinit() {
  motor_b_dir(off);
  motor_a_dir(off);
  motor_c_dir(off);
  ds_passive(&LIGHT_SENSOR);
  ds_rotation_off(&ARM_SENSOR);
  ds_passive(&ARM_SENSOR);
  exit(0);
}


// feheret keres a karral; megszakad, ha ARM_ORTOG/2-nel tobbet mozog
// a kar, ekkor 0-val ter vissza, rendes esetben 1-el, ekkor meg
// ret is ertelmes...
int fence_search(short* ret) {
  int arm=ARM;
  while (CHECK_BLACK && ABS(ARM-arm)<=ARM_ORTOG/2);
  motor_b_dir(brake);
  *ret=ARM;
  if (ABS(ARM-arm)>=ARM_ORTOG/2)
    return 0;
  else
    return 1;
}

void _calibr(int x, int adir, int cdir) {
  motor_a_dir(adir);
  motor_c_dir(cdir);
  delay(x);
  motor_a_dir(brake);
  motor_c_dir(brake);
}

#define AHEAD_CALIBR(x) _calibr(x, fwd, fwd) 
#define RIGHT_CALIBR(x) _calibr(x, fwd, rev) 
#define LEFT_CALIBR(x) _calibr(x, rev, fwd) 
#define BACK_CALIBR(x) _calibr(x, rev, rev) 

// go until black found, or x reached
// if black found, 1 returned, otherwise 0
int arm_max(int x) {
  int cb=0;
  while (ARM!=x && !(cb=CHECK_BLACK)) {
    if (ARM>x)
      motor_b_dir(fwd);
    else
      motor_b_dir(rev);
  }
  motor_b_dir(brake);
  return cb;
}

struct s_road road_search() {
  struct s_road ret;

  arm_to(0);
  if (!CHECK_BLACK) {
    int road_left=0,road_right=0;
    // the road is not ahead, trouble, trouble
    // it can be on the left
    arm_to(ARM_ORTOG/8);
    road_left=arm_max(ARM_ORTOG/2);
    delay(50);
    // or on the right
    arm_to(-ARM_ORTOG/8);
    road_right=arm_max(-ARM_ORTOG/2);
    arm_to(0);
    // but can't be both
    if (road_left && road_right) {
      cputs("ER1");
      deinit();
    } else if (road_left) {
      arm_to(-2);
      while (!CHECK_BLACK) { LEFT_CALIBR(25); delay(50); }
    }
    else if (road_right) {
      arm_to(2);
      while (!CHECK_BLACK) { RIGHT_CALIBR(25); delay(50); }      
    }
    else { // neither, no way around us, probably a dead end...
      ret.error=1;
      ret.right=0;
      ret.left=0;
      return ret;
    }
  }

  arm_to(0);
  motor_b_dir(fwd);
  if (!fence_search(&ret.right)) {
    ret.error=1;
    return ret;
  }
  motor_b_dir(rev);
  while(ARM<0);
  if (!fence_search(&ret.left)) {
    ret.error=1;
    return ret;
  }
  
  ret.error=0;
  return ret;
}

void calibr_after_turn() {
  int i=0;
  arm_to(0);
  while (!CHECK_BLACK && i++<=2) {LEFT_CALIBR(25); delay(50);}
  while (!CHECK_WHITE) {LEFT_CALIBR(25); delay(50);}
  while (!CHECK_BLACK) {RIGHT_CALIBR(25); delay(50);}
}

void right_turn() {
  cputs("RIGH");
  arm_to(-ARM_ORTOG);
  AHEAD_CALIBR(10);
  delay(10);
  while(!CHECK_BLACK) {
    AHEAD_CALIBR(20);
    delay(25);
  }
  arm_to(-ARM_ORTOG);
  while (!CHECK_WHITE) {RIGHT_CALIBR(40); delay(50);}
  while (!CHECK_BLACK) {RIGHT_CALIBR(40); delay(50);}
  calibr_after_turn();
  RIGHT_CALIBR(40);
}

void left_turn() {
  cputs("LEFT");
  arm_to(ARM_ORTOG-3);
  AHEAD_CALIBR(10);
  delay(10);
  while(!CHECK_BLACK) {
    AHEAD_CALIBR(20);
    delay(25);
  }
  arm_to(ARM_ORTOG);
  while (!CHECK_WHITE) {LEFT_CALIBR(40); delay(50);}
  while (!CHECK_BLACK) {LEFT_CALIBR(40); delay(50);}
  calibr_after_turn();
  BACK_CALIBR(40);
}

void no_turn() {
  cputs("AHEA");
  calibr_after_turn();
}

void around_turn() {
  cputs("TURN");
  arm_to(ARM_MAX);
  while (ARM>0) {
    // spin until white occurs after black
    while (!CHECK_BLACK) { LEFT_CALIBR(40); delay(20); }
    while (CHECK_BLACK) { LEFT_CALIBR(40); delay(20); }
    while (!CHECK_WHITE) { LEFT_CALIBR(40); delay(20); }
    // turn the arm back
    arm_to(ARM-10);
    BACK_CALIBR(12);
    delay(20);
  }
  BACK_CALIBR(20);
  calibr_after_turn();
}

static char* junction_search(int havetodetermine) {
  struct s_road road_res;
  int white_left=0;
  int white_right=0;
  static char ret[5];

  cputs("SEAR");
  //arm_calibr
  ret[4]=0;
  // search for a junction, follow the road
  road_res=road_search();
  while (road_res.error==0 && road_res.left-road_res.right<=ROAD_WIDTH) {
    if (ABS((short)(road_res.left+road_res.right))<=1)
      AHEAD_CALIBR(20);
    else if (((short)road_res.left+road_res.right)<0)
      RIGHT_CALIBR(15);
    else // (ROAD_DIFF(road_res)>0)
      LEFT_CALIBR(15);
    road_res=road_search();
    //    cputw(ROAD_DIFF(road_res));
  }
  motor_a_dir(brake);
  motor_c_dir(brake);
  // junction found
  // see if we are near to the previous junction
  cputs("FIND");
  if (havetodetermine) {
    arm_to(-ARM_ORTOG);
    if (arm_max(-ARM_PREV))
      ret[3]='1';
    else {
      arm_to(ARM_ORTOG);
      if (arm_max(ARM_PREV))
	ret[3]='1';
      else
	ret[3]='2';
    }
  }
  // go ahead until we are in the 'middle'
  AHEAD_CALIBR(30);
  while (!white_left || !white_right) {
    AHEAD_CALIBR(20);
    arm_to(-ARM_ORTOG/2);
    white_right=CHECK_WHITE;
    arm_to(ARM_ORTOG/2);
    white_left=CHECK_WHITE;    
  }
  // we are in the 'middle', determine the type of junction
  if (havetodetermine) {
    cputs("DETR");
    arm_to(ARM_ORTOG);
    ret[0]=arm_max(ARM_ORTOG/2)+48;
    arm_to(ARM_ORTOG/4);
    ret[1]=arm_max(-ARM_ORTOG/4)+48;
    arm_to(-ARM_ORTOG/2);
    ret[2]=arm_max(-ARM_ORTOG)+48;
    return ret;
  } else return "NRES";
}

#ifndef ROBOTOOLS_INCLUDED 
int main(int argc, char **argv) {
  init();
  /*  while(1) {
    cputs(junction_search());
    right_turn();
    cputs(junction_search());
    right_turn();
    cputs(junction_search());
    right_turn();
    cputs(junction_search());
    right_turn();
    cputs(junction_search());
    left_turn();
    cputs(junction_search());
    left_turn();
    cputs(junction_search());
    left_turn();
    cputs(junction_search());
    left_turn();
    }
  cputs(junction_search(0));
  delay(400);
  right_turn();
  cputs(junction_search(0));
  delay(400);
  right_turn();
  cputs(junction_search(0));
  delay(400);
  left_turn();
  cputs(junction_search(0));
  delay(400);
  left_turn();
  cputs(junction_search(0));
  delay(400);
  around_turn();
  cputs(junction_search(0));
  delay(400);
  no_turn();
  cputs(junction_search(0));
  delay(400);
  around_turn();
  cputs(junction_search(0));
  delay(400);
  left_turn();*/
  cputs(junction_search(1));
  delay(400);
  right_turn();
  cputs(junction_search(1));
  deinit();

  return 0;
}
#endif