/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "../headers/Dictionary.h"
#include "../headers/Util.h"
#include "../Main/RobotMain.h"
#include "../Vision/VisionInterface.h"
#include "Behaviors.h"
#include "Challenges.h"


/*==========================ObstacleAvoidance Challenge========================================*/

char const * const ObstacleAvoidance::beh_name = "ObstacleAvoidance";

char const * const ObstacleAvoidance::state_names[ObstacleAvoidance::NumStates] = {
  "WALK, LOCALIZE","RECORD_OBSTACLE", "FINISH", "HIT_OBST"};

static const double goal_attraction = 0;
static const double goal_falloff = 0.5;

static const double wall_repulsion = 1000;
static const double wall_falloff = -5.0;

static const double obstacle_repulsion = 3000.0;
static const double obstacle_falloff = -5.0;

static const ulong obst_timeout = 20000000L;//obstacles deleted after this time

static const bool debug = true;

ObstacleAvoidance::ObstacleAvoidance() {
  track_objects = new BeTrackObjects();
  goto_point = new BeGotoPointGlobal();

  fsm.init(state_names,NumStates,LOCALIZE,16,16);  
  resetVars();
}

ObstacleAvoidance::~ObstacleAvoidance() {
  if(track_objects!=NULL) {
    delete track_objects;
    track_objects = NULL;
  }
  if(goto_point!=NULL) {
    delete goto_point;
    goto_point = NULL;
  }
}

void
ObstacleAvoidance::resetVars() {
  mzero(obstacles);
  cell_center.set(0.0,0.0);
  num_known_obstacles = 0;
  last_min_point.set(0.0, 0.0);
}

void
ObstacleAvoidance::reset(ulong timestamp) {
  
  if(track_objects!=NULL)
    track_objects->reset(timestamp);

  if(goto_point!=NULL)
    goto_point->reset(timestamp);
  
  fsm.setState(LOCALIZE,0,"Reset",timestamp);
  resetVars();
}

double
ObstacleAvoidance::operator()(FeatureSet *features,MotionCommand *command) {

  int i;
  bool obst=false;
  bool done=false;
  static vector2d recover_vel;
  static ulong last_loc_time = 0L;
  Obstacle new_obst;

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){   
      case WALK:

	//check for expired obstacles and remove them	
	for(i = 0; i < num_known_obstacles; i++){
	  if(obstacles[i].timer < features->current_time){
	    if(debug) pprintf(TextOutputStream,"removing obstacle (%f, %f)\n",obstacles[i].center.x, obstacles[i].center.y);
	    deleteObstacle(i);
	    i--;
	  }
	}

	//look for obstacles on the left
	obst = checkForObstaclesLeft(features);
	//i think there is an obstacle there, check that its not an obstacle that we have seen before
	if(obst){

	  if(debug) pprintf(TextOutputStream,"I believe my position is %f,%f\n",features->my_position.position.mean.x, features->my_position.position.mean.y);
	  calculateObstacleCenter(features, &new_obst);//calculate where we think the obstacle center is
	  
	  //compare center to all known obstacles to see if they are close
	  for(i = 0; i < num_known_obstacles; i++){
	    if(probablySameObstacle(features->getGlobalPosn(new_obst.center),obstacles[i].center, new_obst.color, obstacles[i].color, 200.0 )){
	      if(debug) pprintf(TextOutputStream,"already seen this obstacle\n");
	      updateObstacle(features,new_obst,i);//update timestamp and other stuff...
	      obst = false;
	    }
	  }
	  //this is a new obstacle
	  if(obst){
	    //if we havent localized recently, localize first
	    if(features->current_time - last_loc_time > 5000000){
	      TRANS_CONT(fsm,LOCALIZE,5,"Localize first");
	    }else{
	      //localized recently, just record the new obst.
	      TRANS_CONT(fsm,RECORD_OBSTACLE,6,"New Obstacle");
	    }
	  }
	}

	//look for obstacles on the right
	obst = checkForObstaclesRight(features);
	if(obst){
	  //i think there is an obstacle there, check that its not an obstacle that we have seen before
	  if(debug) pprintf(TextOutputStream,"I believe my position is %f,%f\n",features->my_position.position.mean.x, features->my_position.position.mean.y);
	  calculateObstacleCenter(features, &new_obst);
	  
	  //compare center to all known obstacles to see if they are close
	  for(i = 0; i < num_known_obstacles; i++){
	    if(probablySameObstacle(features->getGlobalPosn(new_obst.center),obstacles[i].center, new_obst.color, obstacles[i].color,200.0 )){
	      if(debug) pprintf(TextOutputStream,"already seen this obstacle\n");
	      updateObstacle(features,new_obst,i);
	      obst = false;
	    }
	  }
	  //this is a new obstacle
	  if(obst){
	    //if we havent localized recently, localize first
	    if(features->current_time - last_loc_time > 5000000){
	      TRANS_CONT(fsm,LOCALIZE,7,"Localize first");
	    }else{
	      //localized recently, just record the new obst.
	      TRANS_CONT(fsm,RECORD_OBSTACLE,8,"New Obstacle");
	    }
	  }
	}

	// Query the potential field to see which way we should move.
	ego_move_vec = incrementalGetMinPt(features) -
	  features->my_position.position.mean;
	

	// Translate into egocentric coordinates so we can use the
	// vector in a motion command.
	ego_move_vec = 
	  ego_move_vec.rotate(-features->my_position.heading.mean.angle());
	
	command->head_cmd = HEAD_SCAN_BALL;//modified scan ball 
	
	ego_move_vec = ego_move_vec.norm();
	ego_move_vec *= MaxDX/2.0; //slow down the walk

	//dont turn and walk because we cant see where we are going 
	if(fabs(ego_move_vec.angle()) > .78){
	  command->motion_cmd = MOTION_WALK_TROT;
	  command->vx = 0.0;
	  command->vy = 0.0; 
	  command->va = ego_move_vec.angle(); 
	}else{
	  command->motion_cmd = MOTION_WALK_TROT;
	  command->vx = ego_move_vec.x;
	  command->vy = ego_move_vec.y; 
	  command->va = ego_move_vec.angle(); 
	}
	
	//NOT TESTED
	//if stuck on something, maybe we should back up
	/*
	if(features->stuck_class == 0 || features->stuck_class == 2){
	  TRANS_CONT(fsm,HIT_OBST,9,"Stuck detector");
	  }*/
	
	//we are close to the final point, just walk to it
	if(features->pointInDefenseBox(features->my_position.position.mean + vector2d(100.0,0.0))){
	  TRANS_CONT(fsm,FINISH,10,"In Defense Box");
	}


	done = true;
	break;
    case LOCALIZE:
        
        if(fsm.timeInState() > 2000000){
	  if(debug) pprintf(TextOutputStream,"my position now is %f,%f\n",features->my_position.position.mean.x, features->my_position.position.mean.y);
	  TRANS_CONT(fsm,RECORD_OBSTACLE,5,"Localized");
	}

	last_loc_time = features->current_time;//time of localization
	command->motion_cmd = MOTION_STAND_NEUTRAL;
	(*track_objects)(features, command, true, false);
      
        done = true;
        break;

    case RECORD_OBSTACLE:
      
        command->motion_cmd = MOTION_STAND_NEUTRAL;
      	
	if(fsm.timeInState() < 2000000){
	  command->head_cmd = HEAD_SCAN_BALL;//look around to gather data
	}else{
	  
	  calculateObstacleCenter(features, &new_obst);//calculate the center again because we have more data
	  addObstacle(features,&new_obst);//add the obtacle to the obstacles[] array
	  TRANS_CONT(fsm,WALK,5,"Keep Moving");
	}
	
	done = true;
	break;
    
    case FINISH:
      
        goto_point->setTarget(-halfLength-200.0, 0.0);
	command->head_cmd = HEAD_SCAN_BALL;
	(*goto_point)(features, command);

	done = true;
	break;
    
    case HIT_OBST:
      //we hit an obstacle, need to recover
        if(fsm.isNewState()){
	  recover_vel.set(command->vx, command->vy);
	  recover_vel *= vector2d(-1.0,-1.0);
	}
	command->motion_cmd = MOTION_WALK_TROT;
	command->vx = recover_vel.x;
	command->vy = recover_vel.y;
	command->va = 0.0;
	
	if(fsm.timeInState() > 2000000){
	  TRANS_CONT(fsm,WALK,5,"Away from obst");
	}

	done = true;
	break;

    }
    if(fsm.error!=0){
      fsm.handleErr(command);
    }
  }
  fsm.endLoop();
  

  
  return 1.0;
}

//check confidences to determine if the cell is clear or not
bool ObstacleAvoidance::cellIsClear() {
  if(cell.obs > 0.5 && 
     cell.evidence > 1.0 && 
     ((cell.red_robot_conf + cell.unknown_obs_conf) > .4  || (cell.blue_robot_conf + cell.unknown_obs_conf) > .4) &&
     (cell.red_robot_conf > 0.1 || cell.blue_robot_conf > 0.1)){
    return false;
  }
  return true;

}


bool ObstacleAvoidance::checkForObstaclesLeft(FeatureSet *features) {
  int j,i;
  vector2d center(0,0);
  vector2f dir;
     
  for(j=-2;j<=0;j++){
    for(i = 0; i < 6; i++){
      dir.set(1.0,0.0);

      center.set(i*200.0,j*200.0);
      cell_center = center;//cell_center is a global variable used in other functions
      
      features->queryLocalModel(&cell, dir, 
				vector2f(i*200.0,j*200.0), 
				vector2f(200.0,200.0));
      
      if(!cellIsClear()){

	pprintf(TextOutputStream,"");
	pprintf(TextOutputStream,"obs %2.2f, evidence %2.2f   (%d,%d)", cell.obs, cell.evidence,i,j);
	pprintf(TextOutputStream,"wall\t%2.2f\tunknown\t%2.2f\tred_robot\t%2.2f\tblue_robot\t%2.2f",cell.wall_conf,
		cell.unknown_obs_conf, cell.red_robot_conf, cell.blue_robot_conf);
	pprintf(TextOutputStream,"ball\t%2.2f\tcyan_goal\t%2.2f\tyellow_goal\t%2.2f\tstripe\t%2.2f",cell.ball_conf,
		cell.cyan_goal_conf, cell.yellow_goal_conf, cell.stripe_conf);
	
	if(debug) pprintf(TextOutputStream,"Obstacle detected left \n");
	return true;//terminate if an obstacle is found.  Calculate cell center will do the rest
      }
    }
  }
  return false;//no obstacles
}

bool ObstacleAvoidance::checkForObstaclesRight(FeatureSet *features) {
  int j,i;
  vector2d center(0,0);
  vector2f dir;
     
  for(j=1;j<=2;j++){
    for(i = 0; i < 6; i++){
      center.set(i*200.0,j*200.0);
      cell_center = center;//cell_center is a global variable used in other functions

      dir.set(1.0,0.0);
      
      features->queryLocalModel(&cell, dir, 
				vector2f(i*200.0,j*200.0), 
				vector2f(200.0,200.0));
      

      if(!cellIsClear()){
	pprintf(TextOutputStream,"");
	pprintf(TextOutputStream,"obs %2.2f, evidence %2.2f   (%d,%d)", cell.obs, cell.evidence,i,j);
	pprintf(TextOutputStream,"wall\t%2.2f\tunknown\t%2.2f\tred_robot\t%2.2f\tblue_robot\t%2.2f",cell.wall_conf,
		cell.unknown_obs_conf, cell.red_robot_conf, cell.blue_robot_conf);
	pprintf(TextOutputStream,"ball\t%2.2f\tcyan_goal\t%2.2f\tyellow_goal\t%2.2f\tstripe\t%2.2f",cell.ball_conf,
		cell.cyan_goal_conf, cell.yellow_goal_conf, cell.stripe_conf);
	

	if(debug) pprintf(TextOutputStream,"Obstacle detected right\n");
	return true;//terminate if an obstacle is found.  Calculate cell center will do the rest
      }
    }
  }
  return false;//no obstacles
}

void ObstacleAvoidance::calculateObstacleCenter(FeatureSet *features, Obstacle *obst){
  
  vector2d init_cell = cell_center;
  double right, left, bottom;

  pprintf(TextOutputStream,"Initial cell where obstacle detected is %f %f\n", init_cell.x, init_cell.y);

  //keep shifting the center of the cell to the right to determine where the end of the obstacle is
  while(!cellIsClear()){
    cell_center.set(cell_center.x, cell_center.y+100.0);
    vector2d dir = cell_center.norm();
    
    features->queryLocalModel(&cell,vector2f((float)dir.x, (float)dir.y) , vector2f(cell_center.x,cell_center.y), vector2f(200.0,200.0));
  }
  right = cell_center.y - 100.0;

  cell_center = init_cell;

  //shift the cell to the left
  do{
    cell_center.set(cell_center.x, cell_center.y-100.0);
    vector2d dir = cell_center.norm();
    
    features->queryLocalModel(&cell,vector2f((float)dir.x, (float)dir.y) , vector2f(cell_center.x,cell_center.y), vector2f(200.0,200.0));
  }while(!cellIsClear());
  left = cell_center.y + 100.0;


  cell_center = init_cell;
  //shift the cell down
  do{
    cell_center.set(cell_center.x-100.0, cell_center.y);
    vector2d dir = cell_center.norm();
    features->queryLocalModel(&cell,vector2f((float)dir.x, (float)dir.y), vector2f(cell_center.x,cell_center.y), vector2f(200.0,200.0));
  }while(!cellIsClear());
  bottom = cell_center.x + 100.0;


  obst->rep_dist = bottom;
  pprintf(TextOutputStream,"Reported values: Left %f, right %f, bottom %f", left, right, bottom);

  //often reported x values are further than they should be, so I try to scale them, but this may not be
  //very accurate
  if(bottom <= 300)
    bottom *= 0.8;
  else if(bottom <= 500)
    bottom *= 0.7;
  else
    bottom *= 0.65;
  
  obst->center.set(bottom, (right + left)/2.0);
  pprintf(TextOutputStream,"center of obstacle at (%f, %f)\n",obst->center.x, obst->center.y);
    
  //now that we have the center, query it one more time to see the color of the obstacle.
  features->queryLocalModel(&cell, vector2f(1.0,0.0), 
			    vector2f(obst->center.x,obst->center.y), 
			    vector2f(200.0,200.0));
  if(cell.red_robot_conf > cell.blue_robot_conf){
    obst->color = TEAM_COLOR_RED;
  }else{
    obst->color = TEAM_COLOR_BLUE;
  }
 

}

/*
Initial idea:  I first wanted to update the center of the obstacle so that if we saw it once and then saw it again
with more confidence we would move the center to the new position based on more data and such. Unfortunately
more confidence in the model does not seem to mean that our distance estimate is better.  So here is the new method:

New idea:  Every time we see the obstacle, just add it to the list.  The cones in the potential field will overlap
and will cover a wide area but we will make sure that we dont walk there and this seems to work better than moving
one center around

Another idea: update center when distance measurements are lower.  Those seem to be more accurate
*/
void ObstacleAvoidance::updateObstacle(FeatureSet *features,Obstacle new_obst, int idx) {

  if(new_obst.rep_dist < obstacles[idx].rep_dist){
    pprintf(TextOutputStream,"obstacle (%f,%f) updated to (%f, %f)\n",obstacles[idx].center.x,obstacles[idx].center.y,features->getGlobalPosn(new_obst.center).x,features->getGlobalPosn(new_obst.center).y );
    obstacles[idx].center = features->getGlobalPosn(new_obst.center);
    obstacles[idx].rep_dist = new_obst.rep_dist;
    
    //addObstacle(features, &new_obst);
  }
  obstacles[idx].timer = features->current_time + obst_timeout;//update the time on it so that we dont delete it too soon

}


void ObstacleAvoidance::addObstacle(FeatureSet *features, Obstacle *new_obst) {
  int i,k;

  new_obst->center = features->getGlobalPosn(new_obst->center);
 
  
  //check if its REALLY similar to another obstacle
  for(i = 0; i < num_known_obstacles; i++){
    if(probablySameObstacle(obstacles[i].center, new_obst->center, obstacles[i].color, new_obst->color, 5.0)){
      return;//dont bother to add it if its within 5 cm
    }
  }

  /*
//this will check all the obstacles and if there are two close to each other it will say "they are the same!" and
//delete one.  We may not want this, just keep them both?

  for(i = 0; i < num_known_obstacles; i++){
    for(j = 0; j < num_known_obstacles; j++){
      if(i != j){
	if(probablySameObstacle(obstacles[i].center, obstacles[j].center, obstacles[i].color, obstacles[j].color)){
	  deleteObstacle(j);
	  j--;
	  i--;
	}
      }
    }
  }
  */

  if(debug){
    pprintf(TextOutputStream,"List of current obstacles:");
    for(k = 0; k < num_known_obstacles; k++){
      pprintf(TextOutputStream,"(%2.2f, %2.2f)", obstacles[k].center.x, obstacles[k].center.y);
    }
  }

  
  if(num_known_obstacles == tot_num_obst){
    if(debug) pprintf(TextOutputStream,"TOO MANY OBSTACLES DETECTED\n");
    return;
  }

  if(debug){
    pprintf(TextOutputStream,"New obstacle at (%2.2f, %2.2f) \n",
	    new_obst->center.x, new_obst->center.y);
  }
  obstacles[num_known_obstacles].center = new_obst->center;
  obstacles[num_known_obstacles].rep_dist = new_obst->rep_dist;
  obstacles[num_known_obstacles].color = new_obst->color;
  obstacles[num_known_obstacles].timer = features->current_time + obst_timeout;
  num_known_obstacles++;
  
  if(debug) pprintf(TextOutputStream,"%d obstacles\n", num_known_obstacles);
    

}

void ObstacleAvoidance::deleteObstacle(int idx){
  int k;
  
  for(k = idx; k < num_known_obstacles; k++){
    obstacles[k].center.x = obstacles[k+1].center.x;
    obstacles[k].center.y = obstacles[k+1].center.y;
    obstacles[k].rep_dist = obstacles[k+1].rep_dist;
    obstacles[k].color = obstacles[k+1].color;
    obstacles[k].timer = obstacles[k+1].timer;
  }
  num_known_obstacles--;
}


bool ObstacleAvoidance::probablySameObstacle(vector2d center1, vector2d center2, int color1, int color2, double thresh){

  if(color1 != color2){
    pprintf(TextOutputStream,"different based on color\n");
    return false;
  }

  if( fabs(center1.x - center2.x) > thresh ){
    pprintf(TextOutputStream,"(%f,%f) (%f,%f) are different\n", center1.x, center1.y, center2.x, center2.y);
    return false;
  }
  if( fabs(center1.y - center2.y) > thresh ){
    pprintf(TextOutputStream,"(%f,%f) (%f,%f) are different\n", center1.x, center1.y, center2.x, center2.y);
    return false;
  }

  pprintf(TextOutputStream,"(%f,%f) (%f,%f) are the same\n", center1.x, center1.y, center2.x, center2.y);
  
  return true;

}

vector2d ObstacleAvoidance::incrementalGetMinPt(FeatureSet *features) {

  last_min_point = features->my_position.position.mean;

  const double step_size = 50;

  double min_p = calcPoint(last_min_point, features);

  /* First we do a fine grained search around the current point. */
  double start_x, start_y, end_x, end_y;

  // Scan a 500 mm block around the current point.
  // Clip it to the size of the field.

  // NOTE Changed to 1 m block at American Open
  start_x = max(-halfLength, last_min_point.x - 500);
  end_x = min(halfLength, last_min_point.x + 500);
  start_y = max(-halfWidth, last_min_point.y - 500);
  end_y = min(halfWidth, last_min_point.y + 500);
  
  for(double x=start_x; x<end_x; x += step_size) {
    for(double y=start_y; y<end_y; y += step_size) {
      vector2d pt = vector2d(x, y);
      double temp = calcPoint(pt, features);
      
      if(temp < min_p) {
	last_min_point = pt;
	min_p = temp;
      }
    }
  }

#if 0
  // Now that we've examined the neighborhood of our current min,
  // let's examine a single row of points on the field and update
  // scan_x so we look at a different row next frame.
  for(double y = -halfWidth; y <= halfWidth; y += step_size) {
    vector2d pt = vector2d(scan_x, y);
    double temp = calcPoint(pt, features);
      
    if(temp < min_p) {
      last_min_point = pt;
      min_p = temp;
    }
  }
  
  scan_x += step_size;
  if(scan_x > halfLength)
    scan_x = -halfLength;
#endif

  //pprintf(TextOutputStream,"min_point = %f\n", last_min_point);
  return last_min_point;
}


double ObstacleAvoidance::calcPoint(vector2d point,
		 FeatureSet *features) {

  double retval = 0.0;
  
  retval += getWallPF(point);
  
  // Avoid obstacles
  for(int i=0; i<num_known_obstacles; i++) {
    vector2d obs_pt = obstacles[i].center;
    retval += getObstaclePF(point, obs_pt);
  }
  
  retval += getFieldPF(point, vector2d(-halfLength, 0.0));
      

  //pprintf(TextOutputStream,"retval = %f\n", retval);
  return retval;
}


double ObstacleAvoidance::getFieldPF(vector2d point, vector2d goal){
  
  double dist = GVector::distance(point, goal);
  return (goal_attraction + dist*goal_falloff);

}

double ObstacleAvoidance::getObstaclePF(vector2d point, vector2d obstacle) {
  
  double dist = GVector::distance(point, obstacle);
  //pprintf(TextOutputStream,"dist (%f,%f) (%f,%f)\n", point.x, point.y, obstacle.x, obstacle.y);

  return max(0.0, obstacle_repulsion + dist*obstacle_falloff);
}


double ObstacleAvoidance::getWallPF(vector2d point){
			      
  double retval = 0;
  double tmp;
    
  /* -X wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfLength + point.x));
  //pprintf(TextOutputStream, "WallPF contrib: %f\n", tmp);
  retval += tmp;
 
  /* +X wall */
  /* This depends only on x-coordinate.  Is this ok? */
  tmp = max(0.0, wall_repulsion +
	    wall_falloff*(penaltyRegionLengthOffset - point.x));
  //pprintf(TextOutputStream, "WallPF contrib: %f\n", tmp);
  retval += tmp;
  
  /* -Y wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfWidth + point.y));
  //pprintf(TextOutputStream, "WallPF contrib: %f\n", tmp);
  retval += tmp;

  /* +Y wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfWidth - point.y));
  //pprintf(TextOutputStream, "WallPF contrib: %f\n\n", tmp);
  retval += tmp;
  
  return retval;
}


static ObstacleAvoidance *oa=NULL;

void BehaviorSystem::obstacleAvoidance(FeatureSet *features){
  if(oa == NULL) {
    oa = new ObstacleAvoidance;
  }

  (*oa)(features,command);
}