//
// Copyright (C) Jedrik D. Eliasen
// 
// File: rigidbody_pluginCmd.cpp
#pragma once
#include "stdio.h"
#include <maya/MPxCommand.h>
#include <maya/MFnPlugin.h>
#include <maya/MSimple.h>
#include <maya/MGlobal.h>
#include <maya/MDoubleArray.h>
#include <maya/MVector.h>
#include "Vec3f.h"
#include "Collision.h"
#include "math.h"

//Create a maya command called 'collision_reaction' that 
//will call the doIt function
DeclareSimpleCommand( collision_reaction, "Jedrik D. Eliasen", "7.0");

//this function is what Maya will process
MStatus collision_reaction::doIt( const MArgList& args )
{
	MStatus stat = MS::kSuccess;
	
	//set variables x,y,z to make array calls easier
	int x = 0;
	int y = 1;
	int z = 2;
	
	//set the paramater variables
	double radius1, radius2;
	double translationS1[3];
	double translationS2[3];
	MVector lvelocityS1;
	MVector lvelocityS2;
	double mass1, cofr1;
	double mass2, cofr2;
	char temp1[10],temp2[10],temp3[10], temp4[10];
	unsigned index;
	double three = 3;
	
	//set the 1st sphere's position x coord to translationS1[x]
	index = args.flagIndex( "tx1", "translateS1X");
	args.get(index + 1, translationS1[x]);
	
	//set the 1st sphere's position y coord to translationS1[y]
	index = args.flagIndex( "ty1", "translateS1Y");
	args.get(index + 1, translationS1[y]);
	
	//set the 1st sphere's position z coord to translationS1[z]
	index = args.flagIndex( "tz1", "translateS1Z");
	args.get(index + 1, translationS1[z]);
	
	//set the 1st sphere's radius to radius1
	index = args.flagIndex( "r1", "radius1");
	args.get(index + 1, radius1);
	
	//set the 1st sphere's coefficient of restitution to cofr1
	index = args.flagIndex( "cofr1", "COFRS1");
	args.get(index + 1, cofr1);
	
	//set the 1st sphere's mass to mass1
	index = args.flagIndex( "m1", "MassS1");
	args.get(index + 1, mass1);
	
	//set the 1st sphere's linear velocity to lvelocityS1
	index = args.flagIndex( "lv1", "lvelocity1") + 1;
	args.get(index, lvelocityS1, three);
	
	//set the 2nd sphere's position x coord to translationS2[x]
	index = args.flagIndex( "tx2", "translateS2X");
	args.get(index + 1, translationS2[x]);
	
	//set the 2nd sphere's position y coord to translationS2[y]
	index = args.flagIndex( "ty2", "translateS2Y");
	args.get(index + 1, translationS2[y]);
	
	//set the 2nd sphere's position z coord to translationS2[z]
	index = args.flagIndex( "tz2", "translateS2Z");
	args.get(index + 1, translationS2[z]);
	
	//set the 2nd sphere's radius to radius2
	index = args.flagIndex( "r2", "radius2");
	args.get(index + 1, radius2);
	
	//set the 2nd sphere's coefficient of restitution to cofr2
	index = args.flagIndex( "cofr2", "COFRS2");
	args.get(index + 1, cofr2);
	
	//set the 2nd sphere's mass to mass2
	index = args.flagIndex( "m2", "MassS2");
	args.get(index + 1, mass2);
	
	//set the 2nd sphere's linear velocity to lvelocityS2
	index = args.flagIndex( "lv2", "lvelocity2") + 1;
	args.get(index, lvelocityS2, three);

	//set the centers of the spheres to two vectors
	Vec3f vec1(translationS1[x],translationS1[y],translationS1[z]);
	Vec3f vec2(translationS2[x],translationS2[y],translationS2[z]);
	Vec3f pointXn(0,0,0);
	
	bool is_intersection = false;
	Sphere s1;
	Sphere s2;
	
	//Set Sphere 1 Data Attributes
	s1.COM_Position.x = translationS1[x];
	s1.COM_Position.y = translationS1[y];
	s1.COM_Position.z = translationS1[z];

	s1.Mass = mass1;

	s1.LinearVelocity.x = lvelocityS1[x];
	s1.LinearVelocity.y = lvelocityS1[y];
	s1.LinearVelocity.z = lvelocityS1[z];

	s1.COFR = cofr1;


	//Set Sphere 2 Data Attributes
	s2.COM_Position.x = translationS2[x];
	s2.COM_Position.y = translationS2[y];
	s2.COM_Position.z = translationS2[z];

	s2.Mass = mass2;

	s2.LinearVelocity.x = lvelocityS2[x];
	s2.LinearVelocity.y = lvelocityS2[y];
	s2.LinearVelocity.z = lvelocityS2[z];

	s2.COFR = cofr2;
		

	////////////////////////////////////////////////////////////////////////////////
	//Calculate Linear Component
	float V1pF, V2pF;
	float V1pI, V2pI;
	Vec3f normal = s2.COM_Position - s1.COM_Position;
	normal.normalize();
	V1pI = s1.LinearVelocity.dot(normal);
	V2pI = s2.LinearVelocity.dot(normal);
	
	float AveE = (s1.COFR+s2.COFR)/2.0f;
		
	V1pF = ((s1.Mass - AveE*s2.Mass)*V1pI + (1.0f + AveE)*s2.Mass*V2pI)/(s1.Mass + s2.Mass);
	V2pF = ((s2.Mass - AveE*s1.Mass)*V2pI + (1.0f + AveE)*s1.Mass*V1pI)/(s2.Mass + s1.Mass);
	Vec3f temp(0.0f,0.0f,0.0f);
	
	s1.LinearVelocity = s1.LinearVelocity + (V1pF - V1pI)*normal;
	s2.LinearVelocity = s2.LinearVelocity + (V2pF - V2pI)*normal;
	////////////////////////////////////////////////////////////////////////////////
	

	//cannot pass back a vector
	//create an array of double that will pass back the 2 vectors,
	//LinearVelocity1 & 2 as doubles
	double result[6];
	result[0] = s1.LinearVelocity.x;
	result[1] = s1.LinearVelocity.y;
	result[2] = s1.LinearVelocity.z;
	result[3] = s2.LinearVelocity.x;
	result[4] = s2.LinearVelocity.y;
	result[5] = s2.LinearVelocity.z;

	//convert the C++ double array to a Maya API double array
	MDoubleArray return_array(result, 6);
	
	//'return' the result to MEL
	clearResult();
	setResult( return_array );
	
	//this will return to MEL a true or false depending on whether the function
	//was successful or not
	return stat;
}