#include "CbmSttGeomLine.h"
#include "CbmSttGeomCircle.h"
#include "CbmSttGeomHelix.h"
#include "TPolyLine3D.h"

CbmSttGeomLine::CbmSttGeomLine()
{
    fX1 = 0.;
    fY1 = 0.;
    fZ1 = 0.;
    fX2 = 0.;
    fY2 = 0.;
    fZ2 = 0.;
}
    
CbmSttGeomLine::CbmSttGeomLine(Double_t x1, Double_t y1, Double_t z1, Double_t x2, Double_t y2, Double_t z2)
{
    fX1 = x1;
    fY1 = y1;
    fZ1 = z1;
    fX2 = x2;
    fY2 = y2;
    fZ2 = z2;
}

void CbmSttGeomLine::Draw()
{
    Int_t
	steps = 100;

    Double_t
	xStep = (fX1 - fX2) / steps,
	yStep = (fY1 - fY2) / steps,
	zStep = (fZ1 - fZ2) / steps;

    for (Int_t teller = 0; teller < steps; teller++)
    {
	CbmSttGeomPoint
	    tmpPoint(fX2 + teller * xStep, fY2 + teller * yStep, fZ2 + teller * zStep);

	tmpPoint.Draw();
    }
}

CbmSttGeomLine::CbmSttGeomLine(CbmSttGeomPoint startPoint, CbmSttGeomPoint endPoint)
{
    fX1 = startPoint.GetX();
    fY1 = startPoint.GetY();
    fZ1 = startPoint.GetZ();
    fX2 = endPoint.GetX();
    fY2 = endPoint.GetY();
    fZ2 = endPoint.GetZ();
}

CbmSttGeomLine::~CbmSttGeomLine()
{
}

Double_t CbmSttGeomLine::GetD0() const
{
    return -1.;
}

Double_t CbmSttGeomLine::GetPhi() const
{
    return -1.;
}

Double_t CbmSttGeomLine::GetTheta() const
{
    return -1.;
}

Bool_t CbmSttGeomLine::IsParallelToZaxis() const
{
    Bool_t
	retval = kFALSE;

    TVector3
	zaxis(0., 0., -1.),
	l(fX2 - fX1, fY2 - fY1, fZ2 - fZ1);

    if (l.Angle(zaxis) < 0.000001)
    {
	retval = kTRUE;
    }
    else
    {
	retval = kFALSE;
    }

    return retval;
}

Bool_t CbmSttGeomLine::Is2D()
{
    Bool_t
	retval = kTRUE;
    
    if (fabs(fZ1) > 0. || fabs(fZ2) > 0.)
	retval = kFALSE;

    return retval;
}

Int_t CbmSttGeomLine::IntersectWith(CbmSttGeomCircle myCircle, CbmSttGeomPoint &answer1, CbmSttGeomPoint &answer2)
{
    return myCircle.IntersectWith(*this, answer1, answer2);
}

Double_t CbmSttGeomLine::DistanceTo(CbmSttGeomHelix myHelix)
{
    return myHelix.DistanceTo(*this);
}

Double_t CbmSttGeomLine::DistanceTo(CbmSttGeomPoint myPoint)
{
    CbmSttGeomPoint
	myLinePoint = ClosestPointTo(myPoint);

    return myPoint.DistanceTo(myLinePoint);
}

CbmSttGeomPoint CbmSttGeomLine::ClosestPointTo(CbmSttGeomPoint myPoint)
{
    Double_t
	vx = fX2 - fX1,
	vy = fY2 - fY1,
	vz = fZ2 - fZ1,

	wx = myPoint.GetX() - fX1,
	wy = myPoint.GetY() - fY1,
	wz = myPoint.GetZ() - fZ1;

    Double_t 
	c1 = wx * vx + wy * vy + wz * vz,
	c2 = vx * vx + vy * vy + vz * vz,
	b = c1 / c2;

    CbmSttGeomPoint
	myLinePoint(fX1 + b * vx, fY1 + b * vy, fZ1 + b * vz);

    return myLinePoint;
}

void CbmSttGeomLine::Transform(Double_t xtrans, Double_t ytrans, Double_t ztrans)
{
    fX1 += xtrans;
    fY1 += ytrans;
    fZ1 += ztrans;
    fX2 += xtrans;
    fY2 += ytrans;
    fZ2 += ztrans;
}

ClassImp(CbmSttGeomLine)