/***************************************************************
 * CSE455 Winter 2003 Project 3
 * File to be modified #2:
 * ImgView.inl (included from ImgView.cpp)
 *		contains routines for computing the 3D position of points
 ***************************************************************/

//
// TODO routine #1: sameZPlane
//		Compute the 3D position of newPoint using knownPoint
//		that lies on the same plane and whose 3D position is known.
//		See the man on the box lecture slide.
//		If newPoint is on the reference plane (Z==0), use homography instead.
//
void ImgView::sameZPlane()
{
	if (pntSelStack.size() < 2)
	{
		fl_alert("Not enough points on the stack.");
		return;
	}

	SVMPoint &newPoint = *pntSelStack[pntSelStack.size() - 1];
	SVMPoint &knownPoint = *pntSelStack[pntSelStack.size() - 2];

	if( !knownPoint.known() )
	{
		fl_alert("Can't compute relative values for unknown point.");
		return;
	}

	/******** BEGIN TODO ********/


	newPoint.Z = knownPoint.Z;

	//cross the two lines

	// find X, Y coordinates
	// use inverse homography




	/******** END TODO ********/

	newPoint.known(true);

	printf( "Calculated new coordinates for point: (%e, %e, %e)\n", newPoint.X, newPoint.Y, newPoint.Z );

	redraw();
}

//
// TODO routine #2: sameXY
//		Computes the 3D position of newPoint using knownPoint
//		that has the same X and Y coordinate, i.e. is directly
//		below or above newPoint.
//		See lecture slide on measuring heights.
//
void ImgView::sameXY()
{
	if (pntSelStack.size() < 2)
	{
		fl_alert("Not enough points on the stack.");
		return;
	}

	SVMPoint &newPoint = *pntSelStack[pntSelStack.size() - 1];
	SVMPoint &knownPoint = *pntSelStack[pntSelStack.size() - 2];

	if( !knownPoint.known() )
	{
		fl_alert("Can't compute relative values for unknown point.");
		return;
	}

	if( refPointOffPlane == NULL )
	{
		fl_alert("Need to specify the reference height first.");
		return;
	}

	/******** BEGIN TODO ********/

	// See the lecture note on measuring heights
	// using a known point directly below the new point.

	double magVzR,
		magRB,
		magTempX,
		magTempY,
		magTempZ,
		magTempTz,
		c,
		aCubic,
		bCubic,
		cCubic,
		dCubic;

	newPoint.X = knownPoint.X;
	newPoint.Y = knownPoint.Y;
	
	// need to find the Z coordinate of newPoint
	
	// solve the image cross ratio
	// for tz

	// magnitude of Vz-r
	magTempX = (zVanish.X - curRefPnt->X);
	magTempY = (zVanish.Y - curRefPnt->Y);
	magTempZ = (zVanish.Z - curRefPnt->Z);

	magVzR = magTempX*magTempX + magTempY*magTempY + magTempZ*magTempZ;

	// magnitude of r-b
	magTempX = (curRefPnt->X - knownPoint.X);
	magTempY = (curRefPnt->Y - knownPoint.Y);
	magTempZ = (curRefPnt->Z - knownPoint.Z);

	magRB= magTempX*magTempX + magTempY*magTempY + magTempZ*magTempZ;


	// now solve that messy equation
	c = referenceHeight * (magVzR/magRB);

	magTempTz = c * ( (knownPoint.X-curRefPnt->X)*(knownPoint.X-curRefPnt->X) + (knownPoint.Y-curRefPnt->Y)*(knownPoint.Y-curRefPnt->Y) );
	magTempTz = magTempTz / ( (zVanish.X-knownPoint.X)*(zVanish.X-knownPoint.X) + (zVanish.Y-knownPoint.Y)*(zVanish.Y-knownPoint.Y) );

	magTempTz = magTempTz - (c*(knownPoint.Z*knownPoint.Z));

	aCubic = 1;
	bCubic = (-2*zVanish.Z) - c;
	cCubic = (zVanish.Z*zVanish.Z) - (2*c*knownPoint.Z);
	dCubic = magTempTz;

	// computing cubics
	// taken from http://www.planet-source-code.com
	long double a1 = bCubic/aCubic, a2 = cCubic/aCubic, a3 = cCubic/aCubic;
    long double Q = (a1*a1 - 3.0*a2)/9.0;
    long double R = (2.0*a1*a1*a1 - 9.0*a1*a2 + 27.0*a3)/54.0;
	double *x;
    double R2_Q3 = R*R - Q*Q*Q;
	double theta;

    if (R2_Q3 <= 0)
    {
        theta = acos(R/sqrt(Q*Q*Q));
        x[0] = -2.0*sqrt(Q)*cos(theta/3.0) - a1/3.0;
        x[1] = -2.0*sqrt(Q)*cos((theta+2.0*3.1415926538)/3.0) - a1/3.0;
        x[2] = -2.0*sqrt(Q)*cos((theta+4.0*3.1415926538)/3.0) - a1/3.0;
    }
    else
     {
        x[0] = pow(sqrt(R2_Q3)+fabs(R), 1/3.0);
        x[0] += Q/x[0];
        x[0] *= (R < 0.0) ? 1 : -1;
        x[0] -= a1/3.0;
    }

	 newPoint.Z = x[0];


	/******** END TODO ********/

	newPoint.known(true);

	printf( "Calculated new coordinates for point: (%e, %e, %e)\n", newPoint.X, newPoint.Y, newPoint.Z );

	redraw();
}