//-
// ==========================================================================
// Copyright (C) 1995 - 2006 Autodesk, Inc. and/or its licensors.  All 
// rights reserved.
//
// The coded instructions, statements, computer programs, and/or related 
// material (collectively the "Data") in these files contain unpublished 
// information proprietary to Autodesk, Inc. ("Autodesk") and/or its 
// licensors, which is protected by U.S. and Canadian federal copyright 
// law and by international treaties.
//
// The Data is provided for use exclusively by You. You have the right 
// to use, modify, and incorporate this Data into other products for 
// purposes authorized by the Autodesk software license agreement, 
// without fee.
//
// The copyright notices in the Software and this entire statement, 
// including the above license grant, this restriction and the 
// following disclaimer, must be included in all copies of the 
// Software, in whole or in part, and all derivative works of 
// the Software, unless such copies or derivative works are solely 
// in the form of machine-executable object code generated by a 
// source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND. 
// AUTODESK DOES NOT MAKE AND HEREBY DISCLAIMS ANY EXPRESS OR IMPLIED 
// WARRANTIES INCLUDING, BUT NOT LIMITED TO, THE WARRANTIES OF 
// NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR 
// PURPOSE, OR ARISING FROM A COURSE OF DEALING, USAGE, OR 
// TRADE PRACTICE. IN NO EVENT WILL AUTODESK AND/OR ITS LICENSORS 
// BE LIABLE FOR ANY LOST REVENUES, DATA, OR PROFITS, OR SPECIAL, 
// DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES, EVEN IF AUTODESK 
// AND/OR ITS LICENSORS HAS BEEN ADVISED OF THE POSSIBILITY 
// OR PROBABILITY OF SUCH DAMAGES.
//
// ==========================================================================
//+

//////////////////////////////////////////////////////////////////
//
// 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;
}