//-
// ==========================================================================
// Copyright 1995,2006,2008 Autodesk, Inc. All rights reserved.
//
// Use of this software is subject to the terms of the Autodesk
// license agreement provided at the time of installation or download,
// or which otherwise accompanies this software in either electronic
// or hard copy form.
// ==========================================================================
//+

//////////////////////////////////////////////////////////////////
//
// Single-bone, single-plane ik-solver.
//
// This plugin demonstrates how to create and register an ik-solver.
// Due to the complex nature of ik solvers, this plugin only 
// works with 2-joint skeletons (1 bone) in the x-y plane.
//
// To use the solver, create a single bone (joint tool).
// Then type the following in the command window:
//
//   ikHandle -sol simpleSolverNode -sj joint1 -ee joint2
//
// This creates a handle that can be dragged around in the x-y
// plane.
//
//////////////////////////////////////////////////////////////////

#include <maya/MIOStream.h>

#include <maya/MPxIkSolverNode.h>
#include <maya/MString.h>
#include <maya/MFnPlugin.h>
#include <maya/MObject.h>
#include <maya/MIkHandleGroup.h>
#include <maya/MFnIkHandle.h>
#include <maya/MDagPath.h>
#include <maya/MVector.h>
#include <maya/MPoint.h>
#include <maya/MDoubleArray.h>


#define MAX_ITERATIONS 1
#define kSolverType "simpleSolverNode"

//////////////////////////////////////////////////////////////////
//
// Class declaration
//
//////////////////////////////////////////////////////////////////
class simpleSolverNode : public MPxIkSolverNode {

public:
					simpleSolverNode();
    virtual			~simpleSolverNode();

	virtual MStatus	doSolve();
	virtual MString	solverTypeName() const;

	static	void*	creator();
	static  MStatus         initialize();

	static	MTypeId	id;

private:
	MStatus			doSimpleSolver();

};

MTypeId simpleSolverNode::id( 0x80100 );


//////////////////////////////////////////////////////////////////
//
// Implementation
//
//////////////////////////////////////////////////////////////////
simpleSolverNode::simpleSolverNode()
	: MPxIkSolverNode()
{
	// setMaxIterations( MAX_ITERATIONS );
}

simpleSolverNode::~simpleSolverNode() {}

void* simpleSolverNode::creator()
{
	return new simpleSolverNode;
}

MStatus simpleSolverNode::initialize()
{ 
	return MS::kSuccess;
}

MString simpleSolverNode::solverTypeName() const
//
// This method returns the type name used to identify this solver.
//
{
	return MString( kSolverType );
}

MStatus simpleSolverNode::doSolve()
//
// This is the core solver.
//
{
	doSimpleSolver();
	return MS::kSuccess;
}

MStatus simpleSolverNode::doSimpleSolver()
//
// Solve single joint in the x-y plane
//
// - first it calculates the angle between the handle and the end-effector.
// - then it determines which way to rotate the joint.
//
{
	MStatus stat;

	// Get the handle and create a function set for it
	//	
	MIkHandleGroup * handle_group = handleGroup();
	if (NULL == handle_group) {
		return MS::kFailure;
	}
	MObject handle = handle_group->handle( 0 );
	MDagPath handlePath = MDagPath::getAPathTo( handle );
	MFnIkHandle fnHandle(handlePath, &stat);


	// Get the position of the end_effector
	//
	MDagPath end_effector;
	fnHandle.getEffector(end_effector);
	MFnTransform tran( end_effector );
	MPoint effector_position = tran.rotatePivot( MSpace::kWorld );


	// Get the position of the handle
	//
	MPoint handle_position = fnHandle.rotatePivot( MSpace::kWorld );


	// Get the start joint position
	//
	MDagPath start_joint;
	fnHandle.getStartJoint( start_joint );
	MFnTransform start_transform( start_joint );
	MPoint start_position = start_transform.rotatePivot( MSpace::kWorld );


	// Calculate the rotation angle
	//
	MVector v1 = start_position - effector_position;
	MVector v2 = start_position - handle_position;
	double angle = v1.angle( v2 );



	// -------- Figure out which way to rotate --------
	//
	//  define two vectors U and V as follows
	//  U   =   EndEffector(E) - StartJoint(S)
	//  N   =   Normal to U passing through EndEffector
	//
	//  Clip handle_position to half-plane U to determine the region it
	//  lies in. Use the region to determine  the rotation direction.
	//
	//             U
	//             ^              Region      Rotation
	//             |  B           
	//            (E)---N            A          C-C-W
	//         A   |                 B           C-W
	//             |  B
	//             |
	//            (S)
	//
	#define CW		 rot = -1.0 * angle;	// clockwise
	#define CCW		 rot = angle;			// counter-clockwise

	double rot = 0.0;	// Rotation about Z-axis


	// U and N define a half-plane to clip the handle against
	//
	MVector U = effector_position - start_position;
	U.normalize();
	

	// Get a normal to U
	//
	MVector zAxis( 0.0, 0.0, 1.0 );
	MVector N = U ^ zAxis;	// Cross product
	N.normalize();


	// P is the handle position vector
	//
	MVector P = handle_position - effector_position;


	// Determine the rotation direction
	//
	double PdotN = P[0]*N[0] + P[1]*N[1];
	if ( PdotN < 0 ) {
		CCW;
	} else {
		CW;
	}


	// get and set the Joint Angles 
	//
	MDoubleArray jointAngles;
	if ( getJointAngles( jointAngles ) ) {
		jointAngles.set( jointAngles[0] + rot, 0 );
		setJointAngles( jointAngles );
	}
	return MS::kSuccess;
}


/////////////////////////////////////////////////////////
//
// Register the solver
//
/////////////////////////////////////////////////////////
MStatus initializePlugin( MObject obj )
{
	MStatus			status;
	MFnPlugin	plugin( obj, PLUGIN_COMPANY, "3.0", "Any");

	status = plugin.registerNode("simpleSolverNode", 
								 simpleSolverNode::id,
								 &simpleSolverNode::creator,
								 &simpleSolverNode::initialize,
								 MPxNode::kIkSolverNode);
	if (!status) {
		status.perror("registerNode");
		return status;
	}

	return status;
}

MStatus uninitializePlugin( MObject obj )
{
	MStatus   		status;
	MFnPlugin	plugin( obj );

	status = plugin.deregisterNode(simpleSolverNode::id);
	if (!status) {
		status.perror("deregisterNode");
		return status;
	}

	return status;
}