/*VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV
Primary Author :
          Tim Hunt
          University of Oklahoma
          Spring 2003 - CS 5973 - Dr.Hougen

Secondary Authors : 
          Adam Heck
          Manohar Pavuluri
          Romain Pradeau

Purpose : For the completion of Project 2 in An Introduction to Intelligent
          Robotics - A robot that continually seeks a goal while avoiding
          obstacles - Demonstration to be preformed on March 27 and 28, 2003
VVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVV*/


/***************************************************************************
            Definitions for the terms used with in the code
***************************************************************************/

/* These are the physical limits of the servos
They prevent the servos from unnecessarily draining power*/
#define low_limit 830 
#define high_limit 4000 
/* These are the three cardinal orthogonal directions */
#define left_servo_limit 1025 
#define center_servo_limit 2462 
#define right_servo_limit 3900
/* These are rates of change for the servos - used when sweeping the servos*/
#define small_servo_change 10
#define med_servo_change 30
#define large_servo_change 40
/* These are postions in the servo_pwm array that break the field of view
up into five areas in front of the robot for use when detecting light.
1 central field that is approx 25 degreeswith 2 fields of 45 degrees that
stradle the center field and 2 fields of 32 degrees to the sides*/
#define first_light_servo_stop 6
#define second_light_servo_stop 15
#define third_light_servo_stop 20
#define fourth_light_servo_stop 29
/* These are postions in the servo_pwm array that break the field of view
up into five areas in front of the robot for use when classifing 
obstacles. 1 central field that is approx 25 degreeswith 2 fields of 45
degrees that stradle the center field and 2 fields of 32 degrees to the
sides*/
#define first_range_servo_stop 4
#define second_range_servo_stop 13
#define third_range_servo_stop 22
#define fourth_range_servo_stop 31
/* These are differences used when comparing light sensors. two sensors that
differ by 2 constitutes a small light difference ... and so on ...*/
#define small_light_diff 2
#define med_light_diff 10
/* This is a value that dictates whether or not there is a lock on the light*/
#define light_lock 200
#define light_is_close 4
/* There is 80 pwms for every five degrees of servo travel*/
#define five_degree 80
/* This is used in setting up the array that shows what angles the servo will
record light data. it the interval of collection */
#define angle_step 5
/* This is the position of the servo that controls the light sensors*/
#define light_servo servo1
/* This is the position of the servo that controls the range sensor*/
#define range_servo servo3
/* These are the positions of the light sensors*/
#define left_light_sensor analog(2)
#define center_light_sensor analog(3)
#define right_light_sensor analog(4)
/* This is the position of the range sensor*/
#define range_finder analog(17)
/* These are the postions of the bumpers*/
#define left_front_bumper digital(9)
#define rear_bumper digital(10)
#define right_front_bumper digital(11)
/* This is a buffer region that allows the target direction to be +- 5 
degrees*/
#define directional_error 80
/* These are the analog values that correspond to distance in inches
for the range sensor*/
#define dist_12 25
#define dist_10 35
#define dist_8 45
#define dist_6 63
#define dist_4 83
/* This is the position of the motors*/
#define left_motor 3
#define right_motor 1
/* These are some initial settings for motor speeds*/
#define slow_speed 50
#define med_speed 75
#define full_speed 100
/* These are helpers for the communication with the motor arbitrator*/
#define TURN_LEFT 1
#define DRIFT_LEFT 2
#define FORWARD 3
#define DRIFT_RIGHT 4
#define TURN_RIGHT 5
#define BACKUP 6
#define STOP 0
/* These are zones for classifing ostacles in proximity of the robot*/
#define left_side 1
#define left_front_corner 2
#define front 3
#define right_front_corner 4
#define right_side 5
/* These describe how close an obstacle is when it is found*/
#define not_close 0
#define somewhat_close 1
#define is_close 2
#define too_close 3
/* These are helpers that signify the priority level for a given action*/
#define PRIORITY_GOD 100
#define PRIORITY_HIGH 10
#define PRIORITY_MED 6
#define PRIORITY_LOW 1

#define EPILEPSY_ATTACK 300
/***************************************************************************
                     Globals used with in the code
***************************************************************************/

/* These store the Process IDs for the respective processes*/
int track_pid;
int range_pid;
int sweep_pid;

/* Arrays */
int angle[37]; // 0 to 180 in 5 degree steps
int servo_pwm[37]; // servo pwms that correspond to the 5 degree steps
int range[37]; // continually changing values that represent obstacle distance
int intense[37]; // continually changing values that represent light intensity

/* Commuinications */
int tracking_priority; // states the priority level of the desired movement based on light tracking
int tracking_desired_direction; // the desired diredtion of travel as based on light/goal position
int range_priority; // states the priority level of the desired movement based on range data
int range_desired_direction; // the desired direction of travel as based on the range data

int speed;  // the speed we want to go
int place;  // for light behavior... the direction we should point the sensor after a sweeep
int scan_count;  // for light behavior, numberof times we've looked for thelight.
int epilepsy;// for randomization while we don't have a front bumper
/***************************************************************************
                              Main Function
***************************************************************************/

void main()
{
    int loop;
    
    set_angles(angle);
    set_servo(servo_pwm);
    speed = slow_speed;
    init_expbd_servos(1);
    for(loop = 0; loop < 37; loop++)
      range[loop]=12;
    epilepsy = 0;
    printf("Press Start \n");
    while(!start_button());
    light_servo = left_servo_limit;
    range_servo = left_servo_limit;
    sleep(.5);
    
    //    printf("Initial Scan \n");
    //    light_servo_scan();
    
    printf("Processes are   Running \n");
    track_pid = start_process(tracking_behavior());
    range_pid = start_process(range_behavior());
    sweep_pid = start_process(range_servo_scan());
    
    while(!stop_button()){
        arbitrate();
    }
    
    printf("Processes are   Terminated");
    kill_process(track_pid);
    kill_process(range_pid);
    kill_process(sweep_pid);
    init_expbd_servos(0);
    ao();
}

/***************************************************************************
                               Subfunctions
***************************************************************************/
/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
       Arbitrate Function
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/
void arbitrate()
{
    int dir;
    
    // two bumper behaviors that override all
    if(left_front_bumper)
      {
        backup();
        sleep(.5);
        right_spin();
        sleep(.5);
        return;
    }
    if(right_front_bumper)
      {
        backup();
        sleep(.5);
        left_spin();
        sleep(.5);
        return;
    }
    
    if(range_priority >= tracking_priority){
        //        if(tracking_priority<0)
        //          tone(440.0, .1);// mid A beep, we don't know where the light is
        //        else
        //          tone(660.0, .1);// mid C beep, we're upset about obstacles
        dir = range_desired_direction;
    }
    else{
        //        tone(220.0, .1);// low A beep, we're going toward the light.
        dir = tracking_desired_direction;
    }
    
    //dir = range_desired_direction;
    //dir = tracking_desired_direction;
    if (dir== FORWARD){
        epilepsy++;
        if(epilepsy < EPILEPSY_ATTACK)
          forward();
        else{
            left_spin();
            sleep(0.1);
            epilepsy = 0;
        }
    }
    else epilepsy = 0;
    if (dir== TURN_LEFT)
      left_spin();
    if (dir== DRIFT_LEFT)
      left_drift();    
    if (dir== DRIFT_RIGHT)
      right_drift();
    if (dir== TURN_RIGHT)
      right_spin();
    if(dir == STOP)
      ao();
}
/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                         Light Tracking Behavior
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

void tracking_behavior()
{
    while(1)
      {
        if(!have_lock_on_light())
          {
            place = 0;
            intense[place] = 255;
            tracking_priority = PRIORITY_GOD;
            tracking_desired_direction = STOP;
            scan_count=0;
            // look in both directions if need be
            while((scan_count < 4)&& (intense[place]>light_lock)){
                light_servo_scan();
            }
            // if we se a light, point the servo there
            if(intense[place]<light_lock)
              {
                light_servo = get_reading(servo_pwm , place);
                sleep(0.5);
                tracking_priority = -1;
            }
            // if we don't, let the avoidance behavior rule for a bit
            // then we can try again.
            else{
                tracking_priority = -1;
                sleep(10.0);
            }
        }
        else
          {
            track_light();
            trailblaze_light();
        }
    }
}

void light_servo_scan()
{     
    int c, min, temp;
    
    scan_count++;
    for(c=0; c<37; c++)
      {
        light_servo = servo_pwm[c]; 
        if(c == 0)
          {
            sleep(1.0);
        }
        else
          {
            sleep(0.04);
        }
        temp = center_light_sensor;
        set_reading(intense,c,temp);
    }
    
    find_max_light();
}

void find_max_light()
{
    int c;
    int max=255;
    int trigger=0;
    int left_most=0;
    int right_most=0;
    int temp;
    
    place=0;
    
    for(c=0;c<37;c++)
      {
        temp=get_reading(intense,c);
        if(temp < max)
          {
            trigger=0;
            max=temp;
            left_most=c;
        }
        if(temp == max)
          {
            trigger=1;
            right_most=c;
        }
    }
    if(trigger)
      {
        place=(int)((left_most+right_most)/2);
    }
    else
      {
        place=left_most;
    }
    // if we can't find it, sing a little diddy, then spin around.
    if(intense[place]>light_lock){
        tone(440.0, .1);
        tone(660.0, .1);
        tone(440.0, .1);
        tracking_priority = PRIORITY_GOD;
        speed = 80;
        tracking_desired_direction = TURN_LEFT;
        sleep(1.2);
        speed = slow_speed;
        tracking_desired_direction = STOP;
    }
    // debug line    printf("%d %d %d \n",place,intense[place],servo_pwm[place]);
}

void track_light()
{
    if((left_light_sensor < right_light_sensor) && (light_servo > left_servo_limit))
      {
        if((center_light_sensor - left_light_sensor) < small_light_diff)
          {
            light_servo -= small_servo_change;
        }
        else
          {
            if((center_light_sensor - left_light_sensor) < med_light_diff)
              {
                light_servo -= med_servo_change;
            }
            else
              {
                light_servo -= large_servo_change;
            }
        }
    }
    if(left_light_sensor > right_light_sensor && light_servo < right_servo_limit)
      {
        if((center_light_sensor - right_light_sensor) < small_light_diff)
          {
            light_servo += small_servo_change;
        }
        else
          {
            if((center_light_sensor - right_light_sensor) < med_light_diff)
              {
                light_servo += med_servo_change;
            }
            else
              {
                light_servo += large_servo_change;
            }
        }
    }
    // debug line    printf("%d %d %d \n",left_light_sensor,center_light_sensor,right_light_sensor);
}

int have_lock_on_light()
{
    if(center_light_sensor < light_lock && left_light_sensor < light_lock && right_light_sensor < light_lock)
      {
        return 1;
    }
    else
      {
        return 0;
    }
}

void trailblaze_light()
{
    int light_intensity;
    light_intensity = center_light_sensor;
    if(light_servo < get_reading(servo_pwm, first_light_servo_stop))
      {
        tracking_desired_direction = TURN_LEFT;
        tracking_priority = PRIORITY_HIGH;
        // debug line        printf("tl \n");
        return;
    }
    if(light_servo < get_reading(servo_pwm, second_light_servo_stop))
      {
        tracking_desired_direction = DRIFT_LEFT;
        if(light_intensity <= light_is_close)
          {
            tracking_priority = PRIORITY_MED;
        }
        else
          {
            tracking_priority = PRIORITY_LOW;
        }
        // debug line        printf("dl \n");
        return;
    }
    if(light_servo < get_reading(servo_pwm, third_light_servo_stop))
      {
        tracking_desired_direction = FORWARD;
        if(light_intensity <= light_is_close)
          {
            tracking_priority = PRIORITY_MED;
        }
        else
          {
            tracking_priority = PRIORITY_LOW;
        }
        // debug line        printf("fd \n");
        return;
    }
    if(light_servo < get_reading(servo_pwm, fourth_light_servo_stop))
      {
        tracking_desired_direction = DRIFT_RIGHT;
        if(light_intensity <= light_is_close)
          {
            tracking_priority = PRIORITY_MED;
        }
        else
          {
            tracking_priority = PRIORITY_LOW;
        }
        // debug line        printf("dr \n");
        return;
    }
    tracking_desired_direction = TURN_RIGHT;
    tracking_priority = PRIORITY_HIGH;
    // debug line    printf("tr \n");
}

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                         Range Finding Behavior
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

void range_behavior()
{
    while(1)
      {
        //range_servo_scan();
        trailblaze_range();
    }
}

void range_servo_scan()
{
    int c;
    int d;
    while(1){ 
        for(c=0; c<37; c++)
          {
            range_servo = servo_pwm[c];
            sleep(0.04);
            set_reading(range,c,decipher_range());
        }
        for(d=36; d>-1; d--)
          {
            range_servo = servo_pwm[d];
            sleep(0.04);
            set_reading(range,d,decipher_range());
        }
    }
}

int decipher_range()
{
    int temp;
    temp = range_finder;
    if(temp < dist_12) return 12;
    if(temp < dist_10) return 10;
    if(temp < dist_8) return 8;
    if(temp < dist_6) return 6;
    if(temp < dist_4) return 4;
    else return 1;
}

void trailblaze_range()
{
    int goal_position;
    int dist;
    int loop, best, bestValue;
    int temp;
    
    // an attempt to regulate speed by closest object
    /*best = 12;
    for(loop=0; loop < 37; loop++)
      {
        temp = range[loop];
        if(temp<best)
          best = temp;
    }
    if(best<12) speed = 50;   
    if(best==6) speed = 40;
    if(best==4) speed = 30;
    // end said attempt
    speed = 30;*/
    if(have_lock_on_light())
      {
        goal_position = where_is_light_at();
        if(clear_path_to_light(goal_position))
          {
            tracking_priority = PRIORITY_HIGH;
            range_priority = PRIORITY_LOW;
            return;
        }
    }
    
    if(dist=is_obstacle_there(front)){
        range_priority = 7+dist;// only overrides the high tracking priority if
        // we get close
    }
    else if((is_obstacle_there(left_front_corner)>=2)||(is_obstacle_there(right_front_corner)>=2)){
          range_priority = PRIORITY_HIGH;
      }
      else {
          range_priority = 0;
          range_desired_direction = FORWARD;
          return;
      }
    // just find the best area to go toward, and suggest that.
    // favor the side that the lighh sensor points to.
    best = front;
    bestValue = 4;
    if(light_servo < center_servo_limit)
      for(loop=left_side; loop<=right_side; loop++){
          dist = is_obstacle_there(loop);
          if(dist < bestValue){
              best = loop;
              bestValue = dist;
          }
      }
    else
      for(loop=right_side; loop>=left_side; loop--){
          dist = is_obstacle_there(loop);
          if(dist < bestValue){
              best = loop;
              bestValue = dist;
          }
      }
    
    range_desired_direction = best;
}

int where_is_light_at()
{
    int c;
    for(c=0; c<37; c++)
      {
        if(light_servo <= get_reading(servo_pwm, c))
          {
            return c;
        }
    }   
}

int clear_path_to_light(int place)
{
    int c;
    int peep;
    for(c=-2; c<3; c++)
      {
        peep = place + c;
        if(peep <= 0) peep = 0;
        if(peep >= 36) peep = 36;
        if(get_reading(range,peep) <= dist_10) return 0;
    }
    return 1;
}

int is_obstacle_there(int zone)
{
    int c;
    int temp;
    int twelves = 0;
    int tens = 0;
    int eights = 0;
    int sixs = 0;
    int fours = 0;
    int lower = 0;
    int start;
    int end;
    if(zone == left_side)
      {
        start = 0;
        end = first_range_servo_stop;
    }
    else if(zone == left_front_corner)
        {
          start = first_range_servo_stop + 1;
          end = second_range_servo_stop;
      }
      else if(zone == front)
          {
            start = second_range_servo_stop + 1;
            end = third_range_servo_stop;
        }
        else if(zone == right_front_corner)
            {
              start = third_range_servo_stop + 1;
              end = fourth_range_servo_stop;
          }
          else
            {
              start = fourth_range_servo_stop + 1;
              end = 36;
          }
    for(c=start; c<=end; c++)
      {
        temp = get_reading(range,c);
        if(temp == 12) twelves+=1;
        if(temp == 10) tens+=1;
        if(temp == 8) eights+=1;
        if(temp == 6) sixs +=1;
        if(temp == 4) fours+=1;
        else lower+=1;
    }
    // kludgeish changes, not necesarily what I think is best
    if((sixs+fours)>=2)
      return too_close;
    if((eights+sixs+fours)>=2)
      return is_close;
    if((tens+eights+sixs+fours)>=2)
      return somewhat_close;
    else
      return not_close;
}
/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                    Motor Commands for Specified Behavior
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

void left_spin()
{
    motor(left_motor, -speed);
    motor(right_motor, speed);
}

void left_drift()
{
    motor(left_motor, 0);
    motor(right_motor, speed);
}

void forward()
{
    motor(left_motor, speed);
    motor(right_motor, speed);
}

void right_drift()
{
    motor(left_motor, speed);
    motor(right_motor, 0);
}

void right_spin()
{
    motor(left_motor, speed);
    motor(right_motor, -speed);
}

void backup()
{
    motor(left_motor, -speed);
    motor(right_motor, -speed);
}

void stop()
{
    break_all_motors();
}

void break_all_motors()
{
    int c;
    for(c=0; c<40; c++)
      {
        motor(left_motor, -30);
        motor(right_motor, -30);
        motor(left_motor, 30);
        motor(right_motor, 30);
    }
    ao();
}

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                         Array Building Functions
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

void set_angles(int array[])
{
    int c;
    for(c=0; c<37; c++)
      {
        array[c] = c * angle_step;
    }
}

void set_servo(int array[])
{
    int c;
    for(c=0; c<37; c++)
      {
        array[c]=left_servo_limit + (c * five_degree);
    }
}

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                         Data Retrieval and Manipulation
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

int get_reading(int array[], int place)
{
    return array[place];
}

void set_reading(int array[], int place, int value)
{
    array[place]=value;
}

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                        Additional Testing Functions
               Not used in final run - only debugging trial runs
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX*/

void test_analog(int port)
{
    while(1)
      {
        printf("%d \n",analog(port));
        sleep(0.5);
    }
}

void test_triplet()
{
    while(1)
      {
        printf("%d %d %d \n",left_light_sensor,center_light_sensor,right_light_sensor);
        sleep(0.5);
    }
}