#include "SoTrunkNurbs.h"
#define WANT_NURBS_MAIN
#define OCTARM		// Otherwise, will get an Air-Octor

#include <math.h>

void phiKappaSToPneumaticLengthsRaw (
  const double *phi,
  const double *kappa,
  const double *s,
  const double *d,
  int i,
  double phiKappaSToPneumaticLengthsRawRet[3])
{
  double t5;
  double t4;
  double t6;
  double t18;
  double t3;
  double t12;
  double t1;
  t1 = s[i];
  t3 = t1 * d[i];
  t4 = phi[i];
  t5 = sin(t4);
  t6 = kappa[i];
  t12 = sin(0.314159265358979324e1 / 0.3e1 + t4);
  t18 = cos(0.314159265358979324e1 / 0.6e1 + t4);
  phiKappaSToPneumaticLengthsRawRet[0] = t1 - t3 * t5 * t6;
  phiKappaSToPneumaticLengthsRawRet[1] = t1 + t3 * t6 * t12;
  phiKappaSToPneumaticLengthsRawRet[2] = t1 - t3 * t6 * t18;
}

void SoTrunk::rotatedControlPts(float phi, float kappa, float theta,
  float thetaCtrl, float d, float rotatedControlPtsRet[][4])
{
  float t1;
  float t2;
  float t3;
  float t4;
  float t5;
  float t7;
  float t8;
  float t12;
  float t14;
  float t15;
  float t16;
  float t18;
  float t19;
  float t20;
  float t21;
  float t24;
  float t25;
  float t30;
  float t31;
  float t32;
  float t35;
  float t38;
  float t39;
  float t41;
  float t43;
  float t44;
  float t45;
  float t50;
  float t51;
  float t55;
  float t60;
  float t65;
  float t67;
  float t68;
  float t72;
  float t73;
  t1 = (float) cos(phi);
  t2 = (float) sin(phi);
  t3 = d * t2;
  t4 = (float) cos(thetaCtrl);
  t5 = t4 * kappa;
  t7 = (float) cos(theta);
  t8 = t7 * kappa;
  t12 = 0.1e1f / kappa;
  t14 = t1 * t1;
  t15 = d * t14;
  t16 = t15 * t5;
  t18 = d * t7 * kappa;
  t19 = t15 * t8;
  t20 = t2 * t7;
  t21 = t2 * t4;
  t24 = (float) sin(theta);
  t25 = t3 * kappa;
  t30 = (float) sqrt(0.3e1f);
  t31 = d * t30;
  t32 = t31 * t5;
  t35 = t31 * t14 * t4 * kappa;
  t38 = t31 * t14 * t7 * kappa;
  t39 = t4 * t1;
  t41 = t3 * t39 * kappa;
  t43 = t20 * kappa;
  t44 = d * t1 * t43;
  t45 = t1 * t7;
  t50 = t21 * kappa;
  t51 = t31 * t1 * t50;
  t55 = t31 * t7 * t1 * t2 * kappa;
  t60 = t31 * t1 * kappa;
  t65 = t4 / 0.2e1f;
  t67 = 0.2e1f * t45;
  t68 = 0.2e1f * t39;
  t72 = 0.2e1f * t20;
  t73 = 0.2e1f * t21;
  rotatedControlPtsRet[0][0] = t1 * (t3 * t5 - t3 * t8 - t7 + t4) * t12;
  rotatedControlPtsRet[0][1] = -(t16 + t18 - t19 + t20 - t21) * t12;
  rotatedControlPtsRet[0][2] = t24 * (0.1e1f + t25) * t12;
  rotatedControlPtsRet[0][3] = t4;
  rotatedControlPtsRet[1][0] = -(-t32 + t35 - t38 - t41 + t44 + t45 - t39) * t12 / 0.2e1f;
  rotatedControlPtsRet[1][1] = -(t51 - t55 + t16 + t18 - t19 + t20 - t21) * t12 / 0.2e1f;
  rotatedControlPtsRet[1][2] = -t24 * (-t25 - 0.1e1f + t60) * t12 / 0.2e1f;
  rotatedControlPtsRet[1][3] = t65;
  rotatedControlPtsRet[2][0] = -(-t32 + t35 - t38 + t41 - t44 + t67 - t68) * t12 / 0.2e1f;
  rotatedControlPtsRet[2][1] = (-t51 + t55 + t16 + t18 - t19 - t72 + t73) * t12 / 0.2e1f;
  rotatedControlPtsRet[2][2] = -t24 * (t25 - 0.2e1f + t60) * t12 / 0.2e1f;
  rotatedControlPtsRet[2][3] = t4;
  rotatedControlPtsRet[3][0] = -t1 * (t31 * t50 - t31 * t43 + t7 - t4) * t12 / 0.2e1f;
  rotatedControlPtsRet[3][1] = (t35 + t31 * t8 - t38 - t20 + t21) * t12 / 0.2e1f;
  rotatedControlPtsRet[3][2] = -t24 * (-0.1e1f + t31 * t2 * kappa) * t12 / 0.2e1f;
  rotatedControlPtsRet[3][3] = t65;
  rotatedControlPtsRet[4][0] = (-t32 + t35 - t38 - t41 + t44 - t67 + t68) * t12 / 0.2e1f;
  rotatedControlPtsRet[4][1] = (t51 - t55 + t16 + t18 - t19 - t72 + t73) * t12 / 0.2e1f;
  rotatedControlPtsRet[4][2] = t24 * (-t25 + 0.2e1f + t60) * t12 / 0.2e1f;
  rotatedControlPtsRet[4][3] = t4;
  rotatedControlPtsRet[5][0] = (-t32 + t35 - t38 + t41 - t44 - t45 + t39) * t12 / 0.2e1f;
  rotatedControlPtsRet[5][1] = -(-t51 + t55 + t16 + t18 - t19 + t20 - t21) * t12 / 0.2e1f;
  rotatedControlPtsRet[5][2] = t24 * (t25 + 0.1e1f + t60) * t12 / 0.2e1f;
  rotatedControlPtsRet[5][3] = t65;
  rotatedControlPtsRet[6][0] = rotatedControlPtsRet[0][0];
  rotatedControlPtsRet[6][1] = rotatedControlPtsRet[0][1];
  rotatedControlPtsRet[6][2] = rotatedControlPtsRet[0][2];
  rotatedControlPtsRet[6][3] = rotatedControlPtsRet[0][3];
}


SoTrunk::SoTrunk()
{
	controlPts = NULL;
	surface = NULL;
}


void SoTrunk::newTrunk(SoSeparator* root)
{
	// Create the So entities which compose a trunk
	controlPts = new SoCoordinate4;
	surface = new SoNurbsSurface;

	// Add them to the scene graph
	root->addChild(controlPts);
	root->addChild(surface);
}

void SoTrunk::setTrunk(float phi, float kappa, float s, float d)
{
	int i;
	float ctrlVals[7*7][4];
	float thetaCurrent, thetaInc;
	float vKnots[10] = {0, 0, 0};
	// Prevent numerical problems by limiting straightness of trunk
	if (fabs(kappa) < 1e-6f)
		kappa = (kappa >= 0.0f) ? 1e-6f : -1e-6f;
	// Compute the angle of wrap
	float theta = s*kappa;

/*
	// Hard-coded to draw a full circle
	rotatedControlPts(phi, kappa, theta, 0, d, &ctrlVals[7*0]);
	theta = 60.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 60.0*DEG2RAD, d, &ctrlVals[7*1]);
	theta = 120.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 0, d, &ctrlVals[7*2]);
	theta = 180.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 60.0*DEG2RAD, d, &ctrlVals[7*3]);
	theta = 240.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 0, d, &ctrlVals[7*4]);
	theta = 300.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 60.0*DEG2RAD, d, &ctrlVals[7*5]);
	theta = 360.0*DEG2RAD;
	rotatedControlPts(phi, kappa, theta, 0, d, &ctrlVals[7*6]);

//	float vKnots[] = {0, 0, 0, 1, 1, 2, 2, 3, 3, 3};
//	surface->vKnotVector.setValues(0, 10, vKnots);

	// Draw using straight lines
//	float uKnots[] = {0, 0, 1, 2, 3, 4, 5, 6, 6};
//	surface->uKnotVector.setValues(0, 9, uKnots);
//	float vKnots[] = {0, 0, 1, 2, 3, 4, 5, 6, 6};
//	surface->vKnotVector.setValues(0, 9, vKnots);
*/

	// Only draw first 360 degrees of the trunk, which draws all the visible trunk
	theta = min(theta, 360.0f*DEG2RAD);
	for (i = 0, thetaCurrent = 0; thetaCurrent < theta; thetaCurrent += thetaInc)
	{
		// Draw a max of 120 degrees of the trunk
		thetaInc = min(theta - thetaCurrent, 120.0f*DEG2RAD);
		// Compute regular points for this portion of the trunk
		rotatedControlPts(phi, kappa, thetaCurrent, 0, d, &ctrlVals[7*i++]);
		// Compute control points for this portion of the trunk
		rotatedControlPts(phi, kappa, thetaCurrent + thetaInc/2.0f, thetaInc/2.0f, d, &ctrlVals[7*i++]);
		// Update knots vector
		vKnots[1 + i] = (float) i/2;
		vKnots[2 + i] = (float) i/2;
	}
	// Add last knot vector
	vKnots[3 + i] = (float) i/2;
	// Add last (ending) regular points
	rotatedControlPts(phi, kappa, theta, 0, d, &ctrlVals[7*i++]);

	// Suck the control points in
	controlPts->point.setValues(0, 7*i, ctrlVals);

	// Define a nurbs surface
	surface->numUControlPoints = 7;
	surface->numVControlPoints = i;

	// Draw it using arc segments by defining the knots appropriately
	float uKnots[] = {0, 0, 0, 1, 1, 2, 2, 3, 3, 3};
	surface->uKnotVector.setValues(0, 10, uKnots);
	surface->vKnotVector.setValues(0, 3 + i, vKnots);
}

SoTrunkTrans::SoTrunkTrans() : SoTrunk()
{
	transform = NULL;
}

void SoTrunkTrans::newTrunk(SoSeparator* root)
{
	SoTrunk::newTrunk(root);

	transform = new SoTransform();
	root->addChild(transform);
}

void SoTrunkTrans::setTrunk(float phi, float kappa, float s, float d)
{
	SoTrunk::setTrunk(phi, kappa, s, d);
	setSoTrunkTrans(transform, phi, kappa, s);
}

void setSoTrunkTrans(SoTransform* trans, float phi, float kappa, float s)
{
	// Prevent numerical problems by limiting straightness of trunk
	if (fabs(kappa) < 1e-6f)
		kappa = (kappa >= 0.0f) ? 1e-6f : -1e-6f;
	// Compute the angle of wrap
	float theta = s*kappa;

	// Rotate and translate future objects so they begin at the trunk tip
	// First, determine the translation by copying from the final entries of
	// the A matrix.  From Maple:
	float transVal[3];
	float t5;
	float t10;
	float t7;
	float t3;
	float t1;
	float t2;
	t1 = (float) cos(theta);
	t2 = -0.1e1f + t1;
	t3 = (float) cos(phi);
	t5 = 0.1e1f / kappa;
	t7 = (float) sin(phi);
	t10 = (float) sin(theta);
	transVal[0] = -t2 * t3 * t5;
	transVal[1] = -t7 * t2 * t5;
	transVal[2] = t10 * t5;
	trans->translation.setValue(transVal);
	// Next, define the axis/angle rotation
	trans->rotation.setValue(SbVec3f((float) -sin(phi), (float) cos(phi), 0), theta);
}


SoTrunkActuator::SoTrunkActuator()
{
	for (int i = 0; i < 3; i++)
		trans[i] = NULL;
	transform = NULL;
}

void SoTrunkActuator::newTrunkActuator(SoSeparator* root)
{
	for (int i = 0; i < 3; i++)
	{
		// Add a separator, under which goes a translation to place
		// each trunk then the trunk itself
		SoSeparator* sep = new SoSeparator;
		root->addChild(sep);
		trans[i] = new SoTranslation;
		sep->addChild(trans[i]);
		soTrunk[i].newTrunk(sep);
	}
	// Add a transformation to move to the end of the trunk
	transform = new SoTransform;
	root->addChild(transform);
}

void SoTrunkActuator::setTrunkActuator(float phi, float kappa, float s, float d, float r)
{
	// Convert float to double in order to call Maple-generated function
	double dPhi = phi;
	double dKappa = kappa;
	double ds = s;
	double dr = r;
	double l[3];
	// Determine length of each of the three actuators
	phiKappaSToPneumaticLengthsRaw(&dPhi, &dKappa, &ds, &dr, 0, l);

	// Update their parameters
	float tmp[3];
	float angle[3];
	int i;
	for (i = 0; i < 3; i++)
	{
		// Compute the angle at which each trunk lies (90, 210, 330 degrees)
		angle[i] = ((float) i)*2.0f*PIf/3.0f + PIf/2.0f;
		// Determine a translation which places the center of each 
		// actuator at this points
		trans[i]->translation.setValue(r*((float) cos(angle[i])),
			r*((float) sin(angle[i])), 0);
		// Compute the new curvature of this actuator, based on its location
		tmp[i] = kappa/( 1.0f - kappa*r*((float) cos(angle[i] - phi)) );
		// Bend the actuator
		soTrunk[i].setTrunk(phi, tmp[i], (float) l[i], d);
	}

	// Put in a transformation to move to the trunk tip
	setSoTrunkTrans(transform, phi, kappa, s);
}

#ifdef WANT_NURBS_MAIN
int main(int argc, char** argv)
{
	// Initializes SoWin library (and implicitly also the Coin
	// library). Returns a top-level / shell window to use.
	HWND mainwin = SoWin::init(argc, argv, argv[0]);

	// Root of scene graph
	SoSeparator* root = new SoSeparator;
	root->ref();

	// Allow modification of number of polygons used to
	// render cylinders / NURBS surfaces (I think).
	// However, default when no SoComplexity node exists
	// is to render with value = 1.0.
	// A newly-constructed SoComplexity node has value = 0.5
	// as the default.
	SoComplexity* c = new SoComplexity;
	c->value = 1.0f;		// 0.0 = worst (fastest), 0.5 = default, 1.0 = best (slowest)
	root->addChild(c);

	// Set background to white
	SoVRMLBackground* b = new SoVRMLBackground;
	b->skyColor.setValue(1.0f, 1.0f, 1.0f);
	root->addChild(b);

	// Put in a base plate
	SoCube* basePlate = new SoCube;
	basePlate->depth = 1;
	basePlate->width = 20;
	basePlate->height = 20;
	root->addChild(basePlate);

	// Test code
//	SoTrunkActuator ta;
//	ta.newTrunkActuator(root);
//	ta.setTrunkActuator(0.0f, 0.1f, 10.0f*PIf/2.0f, 1.0f, 2.0f*SQRT3/3.0f);

#ifdef OCTARM
	// Setup for OctArm
	const int numSections = 3;
	SoTrunkActuator ta[numSections];
	SoMaterial* mat[numSections];
	float colors[numSections][3] = { { 1.0f, 0.0f, 0.0f}, { 0.0f, 1.0f, 0.0f }, { 0.0f, 0.0f, 1.0f } };
	float deadLengths[numSections] = { 6.28f, 6.28f, 3.04f };
	float d[numSections] = { 4.77f, 3.82f, 3.02f };
	float phi[numSections] = { 0.0f, -2.64233002320701f, -2.28655029143141f-20.0f*DEG2RAD };
	float kappa[numSections] = { 0.02560457772937f, 0.07284386628536f, 0.08981742323702f };
	float s[numSections] = { 28.43f, 32.403f, 39.587f };

#else
	// Setup for Air-Octor
	const int numSections = 2;
	SoTrunkTrans trunk[numSections];
	SoMaterial* mat[numSections];
	float colors[numSections][3] = { { 1.0f, 0.0f, 0.0f}, { 0.0f, 1.0f, 0.0f } };
	float deadLengths[numSections] = { 0.0f, 0.0f };
	float d[numSections] = { 4.5f, 4.5f };
	float phi[numSections] = { (-179.0f)*DEG2RAD, (180.0f-122.0f)*DEG2RAD };
	float kappa[numSections] = { 0.0358f, 0.0696f };
	float s[numSections] = { 35.9f, 41.9f };
#endif

	// Draw actuators
	for (int i = 0; i < numSections; i++)
	{
		// Add a color for this trunk section
		mat[i] = new SoMaterial;
		mat[i]->diffuseColor.setValue(colors[i][0], colors[i][1], colors[i][2]);
		root->addChild(mat[i]);

		// Add the actual trunk section
#ifdef OCTARM
		// OctArm
		ta[i].newTrunkActuator(root);
		// Note d is the per-actuator radius, but we measured the overall trunk
		//  radius. From geometry, per-actuator radius is
		//  (overall radius)*sqrt(3)/(2+sqrt(3)).
		//  Also from geometry, distance to trunk center is
		//  (overall radius)*2*sqrt(3)/3 
		ta[i].setTrunkActuator(phi[i], kappa[i], s[i],
			d[i]*SQRT3/(2.0f + SQRT3), d[i]*2.0f/(SQRT3 + 2.0f));
#else
		// Air-Octor
		trunk[i].newTrunk(root);
		trunk[i].setTrunk(phi[i], kappa[i], s[i], d[i]);
#endif

		// Add a dead section (cylinder) after each trunk section
		// First, translate along z by (cylinder height)/2, then rotate
		//  about the x axis since the cylinder extends along z
		SoTransform* trans = new SoTransform;
		trans->translation.setValue(0, 0, deadLengths[i]/2.0f);
		trans->rotation.setValue(SbVec3f(1.0f, 0, 0), PIf/2.0f);
		root->addChild(trans);
		// Color the cylinder differently than the trunk sections
		SoMaterial* m = new SoMaterial;
		m->diffuseColor.setValue(1.0f, 1.0f, 0.0f);
		root->addChild(m);
		// Add the cylinder
		SoCylinder* cyl = new SoCylinder;
		cyl->radius = d[i];
		cyl->height = deadLengths[i];
		root->addChild(cyl);
		// Now, translate along the local y (global z) again by
		//  (cylinder height)/2, then rotate back about x axis
		//  so rest of trunk extends along z.
		trans = new SoTransform;
		trans->translation.setValue(0, deadLengths[i]/2.0f, 0);
		trans->rotation.setValue(SbVec3f(1.0f, 0, 0), -PIf/2.0f);
		root->addChild(trans);
	}

	// Use one of the convenient SoWin viewer classes.
	SoWinExaminerViewer* eviewer = new SoWinExaminerViewer(mainwin);
	eviewer->setSceneGraph(root);
	eviewer->show();

	// Pop up the main window.
	SoWin::show(mainwin);
	// Loop until exit.
	SoWin::mainLoop();

	// Clean up resources.
	delete eviewer;
	root->unref();

	return 0;
}
#endif