/* CONE - Limited Range Camera */
class stCLCamera : public stAVCamera
{
 public:

  
 
  stCLCamera(const  stConfigR2S & inConfig =0,
	     double inCamRange =10,
	     double inCamViewAngle =45) ;
  

  virtual int fileLoader(ifstream &inFile);
  virtual inline segment  farPlane()const;

  virtual inline void getConfig( stConfigR2S &outConfig) const ;
  virtual inline void setConfig(const stConfigR2S &inConfig);

  virtual inline const polygon & viewPolygon()const;
  virtual void visibilityPolygon(list<segment>  lstSeg, 
				 list<segment> &lstVisSeg,
				 list<segment> &lstVisGapSeg,
				 list<point> &lstVisPolyPt,
				 float minLength = 0.5)const;
  virtual inline char * className();
  virtual inline char * clippingType();
  

  virtual void clip(const list<segment>  &lstUnClippedSeg,
		    list<segment>  &lstClippedSeg,
		    segment &leftBound,
		    segment &rightBound)const;

  virtual inline int contains(const point &ptTarget,
			      const list<point>   &lstVisPolyPt,
			      const list<segment> &lstVisSeg)const;

  virtual void boundingSegments(const point &ptOrigin,
				const list<segment>  
				&lstClippedSeg, 
				segment & leftSeg,
				segment &rightSeg) const;

  virtual inline point center()const{ return point(camConfig_.x_,camConfig_.y_);}
 protected:
  virtual void buildPolygon();
 protected:
 
  polygon viewPolygon_;

  stConfigR2S   camConfig_;
  double  camRange_;
  double  camViewAngle_;
};


  stCLCamera :: stCLCamera(const  stConfigR2S& inConfig,
	     double inCamRange,
	     double inCamViewAngle)
{
  camConfig_    = inConfig;
  camRange_     = inCamRange;
  camViewAngle_ = inCamViewAngle;
  buildPolygon();
}

int stCLCamera :: fileLoader(ifstream &inFile)
{
  char title[40];
  inFile>>title;
  camConfig_.fileLoader(inFile);
  inFile>>camRange_ >>camViewAngle_;
  buildPolygon();
  if(inFile.fail())
    return -1;
  return 1;
}
 


segment stCLCamera :: farPlane()const
{
  point ptOrigin (camConfig_.x_,camConfig_.y_);
  double scale = 1;
  double ranExt = -0.0;
  double leftRad  = (camConfig_.theta_ + camViewAngle_*scale )* 0.01746;
  double rightRad = (camConfig_.theta_ - camViewAngle_*scale )* 0.01746;

  
  point leftPt = ptOrigin.translate_by_angle(leftRad,camRange_+ranExt );
  point rightPt= ptOrigin.translate_by_angle(rightRad,camRange_+ranExt);

  return segment(rightPt, leftPt);
}

void stCLCamera :: getConfig(stConfigR2S &outConfig) const 
{
  outConfig = camConfig_ ;  
}

void stCLCamera :: setConfig(const stConfigR2S &inConfig)  
{
  camConfig_ = inConfig ;


  buildPolygon();
}

const polygon & stCLCamera ::  viewPolygon()const
{
  return viewPolygon_;
}
void stCLCamera :: visibilityPolygon(list<segment>  lstSeg, 
				     list<segment> &lstVisSeg,
				     list<segment> &lstVisGapSeg,
				     list<point> &lstVisPolyPt,
				     float minLength )const
{
  point ptOrigin(camConfig_.x_, camConfig_.y_);
  list<segment>  lstClippedSeg;
  

  segment leftClipSeg, rightClipSeg;
  clip(lstSeg,lstClippedSeg,leftClipSeg, rightClipSeg);
  

  stVisibility :: visiblePolygon(ptOrigin,
				 lstClippedSeg,
				 lstVisSeg);

  

 
  list <segment> lstProjSeg;
  stVisibility ::projectVisiblity( ptOrigin,  lstVisSeg,
				   farPlane(),lstProjSeg);


  list <segment> lstVisPlusProj = lstVisSeg;

  list <segment> lstTemp;
  stVisibility ::angularSort(ptOrigin,lstProjSeg,lstTemp);
  lstProjSeg = lstTemp;

  lstVisPlusProj.conc(lstTemp);

   
  

  list<segment> lstAngularSortedSeg;
  stVisibility ::angularSort(ptOrigin,
			     lstVisPlusProj,
			     lstAngularSortedSeg);
  
 
  lstVisGapSeg.clear();
  stVisibility ::visibilityGaps( ptOrigin,
				 lstAngularSortedSeg,
				 lstVisGapSeg);
    
  

  list<segment> lstVis1,lstVis2,lstCompletVisiblitySeg;
  lstVis1 =  lstProjSeg;
  lstVisGapSeg.conc(lstProjSeg);// Adding the Far Plane Gaps
  lstVisGapSeg.push_back(leftClipSeg);
  lstVisGapSeg.push_back(rightClipSeg);
 
    lstVis2 = lstVisSeg;
  lstVis2.conc(lstVis1);
  lstVis2.push_back(leftClipSeg);
  lstVis2.push_back(rightClipSeg);
  point ptInside = ptOrigin.translate_by_angle(0.01746 *camConfig_.theta_, 1);
  stVisibility ::angularSort(ptInside,
			     lstVis2,
			     lstCompletVisiblitySeg);
  segment CVseg, prevSeg;
  list<point> lstPolyPt;
 
  prevSeg = lstCompletVisiblitySeg.tail();

  float minDist = 0.3;
  forall(CVseg,lstCompletVisiblitySeg)
   {
  
     lstPolyPt.push_back(CVseg.source());
     if(prevSeg.target().distance(CVseg.source()) > minDist)
       {
	 lstPolyPt.push_back(prevSeg.target());
       }
     prevSeg = CVseg;
   }
  point polyPt, prevPt;
  lstVisPolyPt.clear();
  
  prevPt = lstPolyPt.tail(); 
  forall(polyPt,lstPolyPt)
    {
      if(prevPt.distance(polyPt) > 2.0)
	lstVisPolyPt.push_back(polyPt);
      prevPt = polyPt;
      
    }
 

}

char * stCLCamera :: className()
{
  return "stCLCamera";
}

char * stCLCamera :: clippingType()
{
  return "TriPolygon";
}

void stCLCamera :: clip(const list<segment>  &lstUnClippedSeg,
			list<segment>  &lstClippedSeg,
			segment &leftBound,
			segment &rightBound)const
{
  lstClippedSeg.clear();
 
  list<point> lstVert = viewPolygon_.vertices();

  rightBound = segment(lstVert[lstVert[0]],lstVert[lstVert[1]]);
  leftBound  = segment(lstVert[lstVert[2]],lstVert[lstVert[0]]);
  point polyCen = lstVert[lstVert[0]];
  segment UCsegment;  
  forall(UCsegment,lstUnClippedSeg)
    {
      list<point> lstInterPoint;
      segment clipSeg;
      lstInterPoint = viewPolygon_.intersection(UCsegment);

      int numInter =  lstInterPoint.size();
    
     
      if(numInter)
	{
	  point ptInter;
	  if(leftBound.intersection(UCsegment,ptInter))
	    {
	      leftBound = segment(ptInter, polyCen);
	    }
	  if(rightBound.intersection(UCsegment,ptInter))
	    {
	      rightBound = segment(polyCen,ptInter);
	    }
	}
      
      if(numInter == 2)
	{
	
	  point source = lstInterPoint.pop();
	  point target = lstInterPoint.pop();
	  lstClippedSeg.push_back(segment(source,target));
	 
	 
	}
      else 
	if(numInter == 1)
	  {/* numInter == 1 */
	  
	    if(viewPolygon_.contains(UCsegment.source()))
	      {
		lstClippedSeg.push_back(segment(UCsegment.source(),
						lstInterPoint.pop())
				       );
	      }
	    else
	      {
		lstClippedSeg.push_back(segment( lstInterPoint.pop(),
						 UCsegment.target())
					);
	      }
	  }
	else if(numInter == 0)
	  {
	    if(viewPolygon_.contains(UCsegment.source())
	       && viewPolygon_.contains(UCsegment.target()))
	      lstClippedSeg.push_back(UCsegment);
	  }

    }
 
}


void stCLCamera :: boundingSegments(const point &ptOrigin,
				    const list<segment>  
				    &lstClippedSeg, 
				    segment & leftSeg, 
				    segment &rightSeg) const
{
  segment inSeg;
  forall(inSeg, lstClippedSeg)
    {
      point ptInter;
      if(leftSeg.intersection(inSeg,ptInter))
	{
	  leftSeg = segment(ptInter,ptOrigin);
	}
      if(rightSeg.intersection(inSeg,ptInter))
	{
	  rightSeg = segment(ptOrigin,ptInter);
	}
	   
    }
}

void stCLCamera :: buildPolygon()
{
  point ptOrigin (camConfig_.x_,camConfig_.y_);

  double leftRad  = (camConfig_.theta_ + camViewAngle_ )* 0.01746;
  double rightRad = (camConfig_.theta_ - camViewAngle_ )* 0.01746;

  segment leftBoundSeg(ptOrigin ,
		       leftRad,
		       camRange_);
  
  segment rightBoundSeg(ptOrigin ,
			rightRad,
			camRange_);
  list<point> lstPolyPoint;
  lstPolyPoint.push_back(ptOrigin);
  lstPolyPoint.push_back(rightBoundSeg.target());
  lstPolyPoint.push_back(leftBoundSeg.target());
  
  viewPolygon_ = polygon(lstPolyPoint);
}

int stCLCamera :: contains(const point &ptTarget,
			   const list<point> &lstVisPolyPt,
			   const list<segment> &lstVisSeg) const
{

  if(viewPolygon_.contains(ptTarget))
   {
     segment obstSeg;
     point ptCamEye(camConfig_.x_ , camConfig_.y_);
     segment shootSeg(ptCamEye,ptTarget);
     
     forall(obstSeg,lstVisSeg)
       {
	 if(obstSeg.intersection(shootSeg))
	   return 0;
       }
   }
  else
    {
      return 0;
    }
  return 1;

 
}