#include "Controller.h"

/**
	Default constructor.
*/
Controller::Controller(Character* ch) : Observable() {
	simiteration = 0;
	//printf("controller::controller\n");
	this->character = ch;
	jointCount = ch->getJointCount();
	for (int i=0;i<jointCount;i++)
		torques.push_back(Vector3d());
	for (int i=0;i<jointCount;i++){
		character->joints[i]->id = i;
	}
}

/**
	Default destructor.
*/
Controller::~Controller(void) {
}


/**
	This method is used to apply the torques that are computed to the character that is controlled.
*/
void Controller::applyTorques(){
//printf("controller::applytorques\n");
	bool noOldTorqueInfo = false;
	if (oldTorques.size() != torques.size()){
		oldTorques.clear();
		noOldTorqueInfo = true;
	}

	Vector3d tmpTorque, deltaT;
	for (int i=0;i<jointCount;i++){
		if (noOldTorqueInfo)
			oldTorques.push_back(torques[i]);

		deltaT.setToDifference(torques[i], oldTorques[i]);

		double maxTorqueRateOfChange = 2000;

		deltaT.x = (deltaT.x<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.x);
		deltaT.x = (deltaT.x>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.x);
		deltaT.y = (deltaT.y<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.y);
		deltaT.y = (deltaT.y>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.y);
		deltaT.z = (deltaT.z<-maxTorqueRateOfChange)?(-maxTorqueRateOfChange):(deltaT.z);
		deltaT.z = (deltaT.z>maxTorqueRateOfChange)?(maxTorqueRateOfChange):(deltaT.z);


		tmpTorque.setToSum(oldTorques[i], deltaT);

		character->getJoint(i)->setTorque(tmpTorque);
		oldTorques[i].setValues(tmpTorque);
	}
}

/**
	This method is used to reset the torques that are to be applied.
*/
void Controller::resetTorques(){
	//printf("controller::resettorques\n");
	oldTorques.clear();
	for (int i=0;i<jointCount;i++)
		torques[i] = Vector3d(0,0,0);
}


void Controller::performPreTasks(double dt, DynamicArray<ContactPoint> *cfs) {
		/*char buffer[50];
		sprintf (buffer, "debugstatephysxpre%d.txt",simiteration);
		this->character->saveReducedStateToFile(buffer);*/
		computeTorques(cfs);
		/*printf("simulation step %d\n",simiteration);
		for(int i=0;i<jointCount;i++){
			printf("torques %d : %f, %f, %f\n",i,torques[i].x,torques[i].y,torques[i].z);
		}
		printf("======================================================================\n");*/
		applyTorques();
	}

bool Controller::performPostTasks(double dt, DynamicArray<ContactPoint> *cfs) { 
		/*char buffer[50];
		sprintf (buffer, "debugstatephysxpost%d.txt",simiteration);
		this->character->saveReducedStateToFile(buffer);
		simiteration++;*/
		//char lala;
		//scanf("%c",&lala);
		return false; }