/* MED - is for Maximize the Min Escape Distance */


void stMEDTargetTracker :: getMETConfig(const stCLCamera & roboCam,
					const point   & ptTarget,
					const stMotionManager &motionManger,
					const list<segment> & lstMapSeg,
			                stCLCamera  & bestCam,
					segment & closestSeg,
					double & MaxMinDist,
					int numTrials)
{
 
 
  stCLCamera trialCam = roboCam;//IMP:to copy the range and view angle
  
  /* with the current robo position & latest target position
     find the Max of Min escape Time & bestCam position*/

  bestCam  = roboCam;
  
  // no need to check for collision as the robot is in a valid position
  minEscapeDistance(trialCam,ptTarget,lstMapSeg,
		    closestSeg,MaxMinDist);


  stConfigR2S roboCamConfig;
  roboCam.getConfig(roboCamConfig);
  
  /* Try out for different positions */
  
  
  const float dRad = (360.0/float(numTrials))*0.01746;
  
  for(int disp = 3; disp <= 6; disp += 3)
    {
      float startRad = 0;
      for(int trialNum = 1; trialNum <= numTrials ; trialNum++)
	{
	       stConfigR2S newConfig;
	       
	       newConfig.x_ = roboCamConfig.x_ + disp * cos(trialNum * dRad);
	       newConfig.y_ = roboCamConfig.y_ + disp * sin(trialNum * dRad);
	       
	       point   ptNewCam(newConfig.x_,newConfig.y_);
	       if(motionManger.collisionChecker(ptNewCam) != 1 )
		 {
		   point transPt = ptTarget.translate(-ptNewCam.xcoord(),
						      -ptNewCam.ycoord());
		   segment transSeg(point(0,0),transPt);			  
		   newConfig.theta_ = transSeg.direction() * (180.0/3.14);
		   
		   segment newClosestSeg;
		   double newMinDist;
		   
		   trialCam.setConfig(newConfig);
		   minEscapeDistance(trialCam,ptTarget,lstMapSeg,
				     newClosestSeg,newMinDist);
	       
		   if(newMinDist >= MaxMinDist && newMinDist > 0.0)
		     {
		       
		       if(newMinDist == MaxMinDist)
			 {
			   point curEye = bestCam.center();
			   point trialEye = trialCam.center();
			   if(ptTarget.distance(curEye) >
			      ptTarget.distance(trialEye))
			     {
			       win.draw_ctext("NEW EQUAL DISTNACE ");
			       closestSeg = newClosestSeg;
			       MaxMinDist = newMinDist;
			       bestCam   = trialCam;
			     }
			
			 }
		       else
			 {
			   closestSeg = newClosestSeg;
			   MaxMinDist = newMinDist;
			   bestCam   = trialCam;
			 }
		     }
		   
		 }
	       startRad += dRad;
	}
    }
   
}

double stMEDTargetTracker :: minEscapeDistance(const  stCLCamera   & roboCam,
					       const point   & ptTarget,
					       const list<segment> & lstMapSeg,
					       segment & closestSeg,
					       double & minDist)const
{

  list<segment> lstVisSeg   ,lstGapSeg;
  list<point>   lstVisPolyPt;
  
  roboCam.visibilityPolygon( lstMapSeg, 
			     lstVisSeg,
			     lstGapSeg,
			     lstVisPolyPt);

     
  /* Need to check if the Target is inside the Visibility */

  if ( roboCam.contains(ptTarget,lstVisPolyPt,lstVisSeg))
    {
      
      minDist = -INT_MAX;
      if(!lstGapSeg.empty())
	{
	  closestSeg = lstGapSeg.front();
	  minDist = closestSeg.distance(ptTarget);
	 
	  segment gapSeg;
	  double dist;
	  forall(gapSeg , lstGapSeg)
	    {
	      dist = gapSeg.distance(ptTarget);
	      if(dist < minDist)
		{
		  closestSeg = gapSeg;
		  minDist = dist;
		}
	      
	    }
	 
	}
      
    }
  else
    {
    
      minDist = -INT_MAX;
      
    }
   
return minDist;
}