//
// 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/MVectorArray.h>
#include "Force.h"
#include "math.h"
#include "Vec3f.h"

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

//this function is what Maya will process
MStatus process_linear_collision::doIt( const MArgList& args )
{
	MStatus stat = MS::kSuccess;
	
	//set x,y,z for simple array accessing
	int x = 0;
	int y = 1;
	int z = 2;
	
	//set the paramater variables
	const int three = 3;
	char temp1[10],temp2[10],temp3[10], temp4[10];
	unsigned index;
	
	MVector linearvelocity1;
	MVector linearvelocity2;
	MVector mayanormal;
	double cofr1;
	double cofr2;
	double mass1;
	double mass2;
	
	//set the 1st sphere's linear velocity to linearvelocity1
	index = args.flagIndex( "lv1", "lvelocity1") + 1;
	args.get(index, linearvelocity1, three);
	
	//set the 1st sphere's 
	index = args.flagIndex( "cofr1", "COFR1") + 1;
	args.get(index, cofr1);
	
	//set the 1st sphere's mass to mass1
	index = args.flagIndex( "m1", "Mass1") + 1;
	args.get(index, mass1);
	
	//set the 2nd sphere's linear velocity to linearvelocity2
	index = args.flagIndex( "lv2", "lvelocity2") + 1;
	args.get(index, linearvelocity2, three);
	
	//set the 2nd sphere's 
	index = args.flagIndex( "cofr2", "COFR2") + 1;
	args.get(index, cofr2);
	
	//set the 2nd sphere's mass to mass2
	index = args.flagIndex( "m2", "Mass2") + 1;
	args.get(index, mass2);
	
	//set the normal to mayanormal
	index = args.flagIndex( "n", "normal") + 1;
	args.get(index, mayanormal, three);
	
	//convert the maya api vectors to our written Vec3f vectors
	Vec3f LinearVelocity1(linearvelocity1[x], linearvelocity1[y], linearvelocity1[z]);
	Vec3f LinearVelocity2(linearvelocity2[x], linearvelocity2[y], linearvelocity2[z]);
	Vec3f normal(mayanormal[x], mayanormal[y], mayanormal[z]);
	

	/////////////////////////////////////////////////////////////////////////////////////////
	float V1pF, V2pF;
	float V1pI, V2pI;
	V1pI = LinearVelocity1.dot(normal);
	V2pI = LinearVelocity2.dot(normal);
	
	float AveE = (cofr1+cofr2)/2.0f;
	
	V1pF = ((mass1 - AveE*mass2)*V1pI + (1.0f + AveE)*mass2*V2pI)/(mass1 + mass2);
	V2pF = ((mass2 - AveE*mass1)*V2pI + (1.0f + AveE)*mass1*V1pI)/(mass2 + mass1);
	
	LinearVelocity1 = LinearVelocity1 + (V1pF - V1pI)*normal;
	LinearVelocity2 = LinearVelocity2 + (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] = LinearVelocity1.x;
	result[1] = LinearVelocity1.y;
	result[2] = LinearVelocity1.z;
	result[3] = LinearVelocity2.x;
	result[4] = LinearVelocity2.y;
	result[5] = LinearVelocity2.z;
	
	//convert the C++ double array to a Maya API double array
	MDoubleArray return_array(result, 5);
	
	//'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;
}