// ===========================================================================
// Copyright 2018 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.
// ===========================================================================
//
//
//  Creation Date:  July 17, 1999
//
//  Description:
//      This is the action for snap 3 points to 3 points. It takes any 3 
//		points on an object (that define a plane) and transforms 
//		the object to match another set of 3 points. 
//		The point selections can include curve point, surface point, 
//		 edit point, poly vertex, etc.
//
//		The first 3 points must be on the same object (i.e. it is the 
//		object to be transformed). The second set of points define 
//		the position and plane to transform to.
//
//		Note: no scaling is done, only translation rotation.
//
//  Input Arguments:
//      None.
//
//  Return Value:
//      None.
//

proc string getObjectToMoveName( string $object )
//
//	Description:
//		Returns the transform name of the object with no components in the 
//		string e.g. curve1.u[0.5] will return curve1
//
{
	string $name;

	// get the string before any "." or "->" - this could be a shape name
	//
	string $buffer[];
	int $numTokens = `tokenize $object ".->" $buffer`;
	if ( $numTokens >= 1 ) $name = $buffer[0];

	return $name;
}

global proc snap3PointsTo3Points( int $moveParent )
{
	// $moveParent = 0: move the object defined by the first 3 points
	// $moveParent = 1: move the parent of the object
	// $moveParent = 2: move the grandparent of the object
	// $moveParent = 3: move the great-grandparent of the object

	// Get the list of nurbs curves/surfaces selected.
	//
	global int $gSelectCVsBit;
	global int $gSelectMeshVerts;
	global int $gSelectLatticePoints;
	global int $gSelectParticles;
	global int $gSelectEditPointsBit;
	global int $gSelectCurveParmPointsBit;
	global int $gSelectSurfaceParmPointsBit;
	global int $gSelectMeshUVs;
	global int $gSelectRotatePivots;
	global int $gSelectScalePivots;
	global int $gSelectJointPivots;
	global int $gSelectSelectHandles;
	global int $gSelectDimensions;
	global int $gSelectUVLocators;
	global int $gSelectSubdivMeshPoints;
	string $objectList[] = `filterExpand -ex true -sm $gSelectCVsBit -sm $gSelectMeshVerts -sm $gSelectLatticePoints -sm $gSelectParticles -sm $gSelectEditPointsBit -sm $gSelectSurfaceParmPointsBit -sm $gSelectCurveParmPointsBit -sm $gSelectMeshUVs -sm $gSelectRotatePivots -sm $gSelectScalePivots -sm $gSelectJointPivots -sm $gSelectSelectHandles -sm $gSelectDimensions -sm $gSelectUVLocators -sm $gSelectSubdivMeshPoints`;

	// make sure there are 6 objects selected
	//
	int $total = size($objectList);
	if ( $total == 0 )
	{
		error (uiRes("m_snap3PointsTo3Points.kNoPointsSelected"));
		return;
	}
	else if ( $total < 6 )
	{
		error (uiRes("m_snap3PointsTo3Points.kIncompleteSelection"));
		return;
	}
	else if ( $total > 6 )
	{
		warning (uiRes("m_snap3PointsTo3Points.kTooManyPoints"));
	}

	string $obj1 = $objectList[$total-6];
	string $obj2 = $objectList[$total-5];
	string $obj3 = $objectList[$total-4];
	string $obj4 = $objectList[$total-3];
	string $obj5 = $objectList[$total-2];
	string $obj6 = $objectList[$total-1];

	// get the name of the object to move (with no component part). 
	// The first 3 points should be on the same object?
	//
	string $objectToMove = getObjectToMoveName( $obj1 );
	string $tmpToMove = getObjectToMoveName( $obj2 );
	string $tmpToMove2 = getObjectToMoveName( $obj3 );

	string $tmp1 = getObjectToMoveName( $obj4 );
	string $tmp2 = getObjectToMoveName( $obj5 );
	string $tmp3 = getObjectToMoveName( $obj6 );

	if ( $objectToMove == $tmp1 || $objectToMove == $tmp2 ||
		 $objectToMove == $tmp3 )
	{
		error (uiRes("m_snap3PointsTo3Points.kErrorSameObject"));
		return;
	}
	else
	{
		string $fullPath1[] = `ls -l $objectToMove`;
		string $fullPath2[] = `ls -l $tmpToMove`;
		if ( strcmp($fullPath1[0], $fullPath2[0]) != 0 )
		{
			error (uiRes("m_snap3PointsTo3Points.kInvalidFirstTwoPoints"));
			return;
		} else {
			string $fullPath3[] = `ls -l $tmpToMove2`;
			if ( strcmp($fullPath2[0], $fullPath3[0]) != 0 )
			{
				error (uiRes("m_snap3PointsTo3Points.kInvalidFirstThreePoints"));
				return;
			}
		}
	}

	// If user wants to move a group, eg. he selects points on a
	// face of a cube and wants the entire cube (group) to move
	// $moveParent = 1: move the parent of the object
	// $moveParent = 2: move the grandparent of the object
	// $moveParent = 3: move the great-grandparent of the object
	//
	if( $moveParent > 0 ) {
		string $parent[];
		string $obj = $objectToMove;
	
		for( $i = 0; $i < $moveParent; $i ++ ) {
			$parent = `listRelatives -parent $obj`;
			if( size($parent[0]) > 0 ) {
				$obj = $parent[0];
			}
		}
		if( size($obj) > 0 ) {
			$objectToMove = $obj;
		}
	}

	// get the world space position of each selected point object
	//
	float $pos1[] = `pointPosition $obj1`;
	float $pos2[] = `pointPosition $obj2`;
	float $pos3[] = `pointPosition $obj3`;
	float $pos4[] = `pointPosition $obj4`;
	float $pos5[] = `pointPosition $obj5`;
	float $pos6[] = `pointPosition $obj6`;

	if ( $pos1[0] == $pos2[0] && 
		$pos1[1] == $pos2[1] && 
		$pos1[2] == $pos2[2] )
	{
		error (uiRes("m_snap3PointsTo3Points.kDupedFirstTwoPoints"));
		return;
	}
	if ( $pos2[0] == $pos3[0] && 
		$pos2[1] == $pos3[1] && 
		$pos2[2] == $pos3[2] )
	{
		error (uiRes("m_snap3PointsTo3Points.kDupedSecondThirdPoints"));
		return;
	}
	if ( $pos4[0] == $pos5[0] && 
		$pos4[1] == $pos5[1] && 
		$pos4[2] == $pos5[2] )
	{
		error (uiRes("m_snap3PointsTo3Points.kDupedFourthFifthPoints"));
		return;
	}
	if ( $pos5[0] == $pos6[0] && 
		$pos5[1] == $pos6[1] && 
		$pos5[2] == $pos6[2] )
	{
		error (uiRes("m_snap3PointsTo3Points.kDupedLastTwoPoints"));
		return;
	}

	// calculate the translation amount - the first point on each pair is the
	// point to use for translation
	//
	float $distance[];
	$distance[0] = $pos4[0] - $pos1[0];
	$distance[1] = $pos4[1] - $pos1[1];
	$distance[2] = $pos4[2] - $pos1[2];

 	// move the first object by that amount
	//
	evalEcho("move -r " + $distance[0] + " " + $distance[1] + " " + $distance[2] + " " + $objectToMove);

	// define the two vectors for each pair of points
	//
	float $axis1[];
	$axis1[0] = $pos2[0] - $pos1[0];
	$axis1[1] = $pos2[1] - $pos1[1];
	$axis1[2] = $pos2[2] - $pos1[2];
	float $axis2[];
	$axis2[0] = $pos5[0] - $pos4[0];
	$axis2[1] = $pos5[1] - $pos4[1];
	$axis2[2] = $pos5[2] - $pos4[2];

	// get the angle (in radians) between the two vectors and the axis of
	// rotation.  This is used to move axis1 to match axis2
	//
	float $dotProduct = dotProduct( $axis1, $axis2, 1 );
	$dotProduct = clamp(-1.0, 1.0, $dotProduct);
	float $angle = acos($dotProduct);
	float $crossProduct[] = crossProduct( $axis1, $axis2, 1, 1 );

 	// rotate the first object about the pivot point (the pivot is defined 
	// by the first point from the second pair of points  - 
	// i.e. point 3 from the inputs above)
	//
	float $rotation[];
	xyzRotation( $angle, $crossProduct, $rotation );
	evalEcho("rotate -r -p " + $pos4[0] + " " + $pos4[1] + " " + $pos4[2] + " " + $rotation[0] + "rad " + $rotation[1] + "rad " + $rotation[2] + "rad " + $objectToMove);

	// Get these points again since they may have moved
	//
	$pos3 = `pointPosition $obj3`;
	$pos6 = `pointPosition $obj6`;

	float $axis3[];
	$axis3[0] = $pos3[0] - $pos5[0];
	$axis3[1] = $pos3[1] - $pos5[1];
	$axis3[2] = $pos3[2] - $pos5[2];
	float $axis4[];
	$axis4[0] = $pos6[0] - $pos5[0];
	$axis4[1] = $pos6[1] - $pos5[1];
	$axis4[2] = $pos6[2] - $pos5[2];

	normalize( $axis2 );

	// Get the dot product of axis3 on axis2
	//
	$dotProduct = dotProduct( $axis3, $axis2, 0 );
	$newPos[0] = $pos5[0] + $dotProduct * $axis2[0];
	$newPos[1] = $pos5[1] + $dotProduct * $axis2[1];
	$newPos[2] = $pos5[2] + $dotProduct * $axis2[2];
	$axis3[0] = $pos3[0] - $newPos[0];
	$axis3[1] = $pos3[1] - $newPos[1];
	$axis3[2] = $pos3[2] - $newPos[2];

	// Get the dot product of axis4 on axis2
	//
	$dotProduct = dotProduct( $axis4, $axis2, 0 );
	$newPos[0] = $pos5[0] + $dotProduct * $axis2[0];
	$newPos[1] = $pos5[1] + $dotProduct * $axis2[1];
	$newPos[2] = $pos5[2] + $dotProduct * $axis2[2];
	$axis4[0] = $pos6[0] - $newPos[0];
	$axis4[1] = $pos6[1] - $newPos[1];
	$axis4[2] = $pos6[2] - $newPos[2];

	// rotate the first object again, this time about the 2nd axis
	// so that the 3rd point is in the same plane.
	// ie. match up axis3 with axis4
	//
	$dotProduct = dotProduct( $axis3, $axis4, 1 );
	$dotProduct = clamp(-1.0, 1.0, $dotProduct);
	$angle = acos($dotProduct);

	// reverse the angle if the cross product is in the -ve axis direction
	//
	$crossProduct = crossProduct( $axis3, $axis4, 1, 1 );
	$dotProduct = dotProduct( $crossProduct, $axis2, 0);
	if( $dotProduct < 0 ) {
		$angle = -$angle;
	}

	xyzRotation( $angle, $axis2, $rotation );
	evalEcho("rotate -r -p " + $pos5[0] + " " + $pos5[1] + " " + $pos5[2] + " " + $rotation[0] + "rad " + $rotation[1] + "rad " + $rotation[2] + "rad " + $objectToMove);
	
}