#include "CbmSttGeomCircle.h"

using namespace std;

CbmSttGeomCircle::CbmSttGeomCircle()
{
    fRadius = 0.;
    fXcenter = 0.;
    fYcenter = 0.;
}
    
CbmSttGeomCircle::CbmSttGeomCircle(Double_t xcenter, Double_t ycenter, Double_t radius)
{
  
    fRadius = radius;
    fXcenter = xcenter;
    fYcenter = ycenter;
}
    
CbmSttGeomCircle::CbmSttGeomCircle(CbmSttGeomPoint center, Double_t radius)
{
  if (!center.Is2D())
    {
      cerr << "circle only available in 2D" << endl;
      
      fRadius = 0.;
      fXcenter = 0.;
      fYcenter = 0.;
    }
  else
    {
      fRadius = radius;
      fXcenter = center.GetX();
      fYcenter = center.GetY();
    }
}

CbmSttGeomCircle::CbmSttGeomCircle(CbmSttGeomPoint edgePoint, Double_t radius, Double_t phi)
{
    if (!edgePoint.Is2D())
    {
	cerr << "circle only available in 2D" << endl;
	
	fRadius = 0.;
	fXcenter = 0.;
	fYcenter = 0.;
    }
    else
    {
	CbmSttGeomLine
	    myLine(0., 0., 0., cos(phi), sin(phi), 0.);
	
	CbmSttGeomPoint
	    centerPoint = myLine.ClosestPointTo(edgePoint);
	
	fRadius = radius;
	fXcenter = centerPoint.GetX();
	fYcenter = centerPoint.GetY();
    }
}

CbmSttGeomCircle::~CbmSttGeomCircle()
{
}
    
Int_t CbmSttGeomCircle::IntersectWith(CbmSttGeomLine line, CbmSttGeomPoint &answer1, CbmSttGeomPoint &answer2)
{
  Int_t
    retval = 0;

  line.Transform(-fXcenter, -fYcenter, 0.);
  
  // taken from wolfram math
  Double_t
    d_x = line.GetX2() - line.GetX1(),
    d_y = line.GetY2() - line.GetY1(),
    d_r = sqrt(d_x * d_x + d_y * d_y),
    r = fRadius,
    sgn,
    D = line.GetX1() * line.GetY2() - line.GetX2() * line.GetY1(),  
    Delta = r * r * d_r * d_r - D * D;
  
  if (d_y < 0)
    sgn = -1.;
  else
    sgn = 1.;

  if (Delta < 0)
  {
      retval = 0;
      
      CbmSttGeomPoint
	  circleCenter(0., 0., 0.), 
	  closestPoint = line.ClosestPointTo(circleCenter);
      
      answer1.SetX(closestPoint.GetX() + fXcenter);
      answer1.SetY(closestPoint.GetY() + fYcenter);
      
      answer2.SetX(closestPoint.GetX() + fXcenter);
      answer2.SetY(closestPoint.GetY() + fYcenter);
  }
  else
  {
      Double_t
	  xIntersect1 = (D * d_y + sgn * d_x * sqrt(Delta)) / (d_r * d_r),
	  yIntersect1 = (-1 * D * d_x + fabs(d_y) * sqrt(Delta)) / (d_r * d_r),
	  xIntersect2 = (D * d_y - sgn * d_x * sqrt(Delta)) / (d_r * d_r),
	  yIntersect2 = (-1 * D * d_x - fabs(d_y) * sqrt(Delta)) / (d_r * d_r);
      
      answer1.SetX(xIntersect1 + fXcenter);
      answer1.SetY(yIntersect1 + fYcenter);
      
      answer2.SetX(xIntersect2 + fXcenter);
      answer2.SetY(yIntersect2 + fYcenter);
      
      if (Delta == 0)
	  retval = 1;
      else
	  retval = 2;
  }    
  return retval;
}

void CbmSttGeomCircle::Transform(Double_t x, Double_t y)
{
  //Transforms circle to the origin
    fXcenter += x;
    fYcenter += y;
}

Int_t CbmSttGeomCircle::IntersectWith(CbmSttGeomCircle circle, CbmSttGeomPoint &answer1, CbmSttGeomPoint &answer2)
{
    // make sure this circle is the largest of the pair
    if (circle.GetRadius() > fRadius)
	return circle.IntersectWith(*this, answer1, answer2);
    
    cout << "circle1: " << fXcenter << "   " << fYcenter << "   " << fRadius << endl;
    cout << "circle2: " << circle.GetXcenter() << "   " << circle.GetYcenter() << "   " << circle.GetRadius() << endl;

    Int_t 
	retval = 0;
    
    circle.Transform(-fXcenter, -fYcenter);
    
    Double_t
	dx = circle.GetXcenter(),
	dy = circle.GetYcenter(),
	d = sqrt((dy * dy) + (dx * dx));
    
    // Check if circles are too far apart, or if one is contained in the other
    if (d > (fRadius + circle.GetRadius())) //Too far apart
      {
	Double_t 
	  scaleFactor = (((d - fRadius - circle.GetRadius()) / 2.) + fRadius);  //For normalization purposes
	
	answer1.SetX(((circle.GetXcenter() / d) * scaleFactor));  //x-comp of the vector to the point between the circles
	answer1.SetY(((circle.GetYcenter() / d) * scaleFactor));
	answer1.SetZ(0.);
	
	answer1.Transform(fXcenter, fYcenter, 0.);  //Transform back to initial position for circle
	
	answer2.SetX((circle.GetXcenter() * scaleFactor) + fXcenter);
	answer2.SetY((circle.GetYcenter() * scaleFactor) + fYcenter);
	answer2.SetZ(0.);
	
	retval = 0;
      }
    else if (d < TMath::Abs(fRadius - circle.GetRadius()))  //Circle contained in the other circle
      {
	Double_t
	  scaleFactor = (((fRadius - d - circle.GetRadius()) / 2.) + d + circle.GetRadius()) / d;
	
	answer1.SetX((circle.GetXcenter() * scaleFactor) + fXcenter);
	answer1.SetY((circle.GetYcenter() * scaleFactor) + fYcenter);
	answer1.SetZ(0.);
	
	answer2.SetX((circle.GetXcenter() * scaleFactor) + fXcenter);
	answer2.SetY((circle.GetYcenter() * scaleFactor) + fYcenter);
	answer2.SetZ(0.);
	
	retval = 0;
    }
    else   //Two intersection points
    {
	/* 'point 2' is the point where the line through the two circle
	 * intersection points crosses the line between the circle
	 * centers.  
	 */
	
	/* Determine the distance from point 0 to point 2 to get the angle. */
      Double_t 
	    a = ((fRadius * fRadius) - (circle.GetRadius() * circle.GetRadius()) + (d * d)) / (2.0 * d) ;
	
	/* Determine the coordinates of point 2. */
	Double_t
	    x2 = dx * a / d,
	    y2 = dy * a / d;
	
	/* Determine the distance from point 2 to either of the
	 * intersection points.
	 */
	Double_t
	    h = sqrt((fRadius * fRadius) - (a * a));
	
	/* Now determine the offsets of the intersection points from
	 * point 2.
	 */
	Double_t
	    rx = -dy * (h/d),
	    ry = dx * (h/d);
	
	answer1.SetX(x2 + rx + fXcenter);
	answer1.SetY(y2 + ry + fYcenter);
	
	answer2.SetX(x2 - rx + fXcenter);
	answer2.SetY(y2 - ry + fYcenter);

	cout << "answer1*: " << x2 + rx + fXcenter << "   " << y2 + ry + fYcenter << endl;
	cout << "answer2*: " << x2 - rx + fXcenter << "   " << y2 - ry + fYcenter << endl;

	if (d == (fRadius + circle.GetRadius()))
	    retval = 1;
	else
	    retval = 2;
    }
    
    return retval;
}

Double_t CbmSttGeomCircle::GetPhi()
{
    Double_t
	phi = atan(fYcenter / fXcenter);
    
    if (fXcenter < 0.)
    {
	if (fYcenter < 0.)
	    phi -= dPi;
	else
	    phi += dPi;
    }
}

Double_t CbmSttGeomCircle::GetD0()
{
    return sqrt(fXcenter * fXcenter + fYcenter * fYcenter) - fRadius;
}

Double_t CbmSttGeomCircle::DistanceTo(CbmSttGeomPoint myPoint)
{
  return sqrt((fXcenter - myPoint.GetX()) * (fXcenter - myPoint.GetX()) + (fYcenter - myPoint.GetY()) * (fYcenter - myPoint.GetY())) - fRadius;
}
    
ClassImp(CbmSttGeomCircle)