#include "Driver.h"
#include "Water.h"

extern Water *water;

Driver::Driver() {
	forcemax = 30;
	forcestep = 1;
	accforce = 0;	
	brakeforce = 0;
	leftforce = 0; 
	rightforce = 0; 

	//Construct vectors
	center = new SbVec3f();
	frontcenter = new SbVec3f();
	backcenter = new SbVec3f();
	forward = new SbVec3f();
	right = new SbVec3f();
	upward = new SbVec3f();

}

void Driver::Setquader(Vertex* quader[8]) {
	for(int i=0; i< 8 ; i++)
		this->quader[i] = quader[i];
}

void Driver::UpdatePoints() {

	Recalc();
	//calc forces
	SbVec3f tmpacc(*forward);
	SbVec3f tmpbrk(*forward);
	SbVec3f tmprgt(*right);
	SbVec3f tmplft(*right);
	
	tmpacc *= accforce;
	tmpbrk *= -brakeforce;
	tmprgt *= rightforce;
	tmplft *= -leftforce;

	for(int i = 0; i < 4; i++) {
		//add acceleration forces
		quader[i]->addforce(tmpacc);
		//add brake forces
		quader[i+4]->addforce(tmpbrk);
	}
	for(int i = 0; i < 4; i = i + 2) {
		//add right forces to 0,2
		quader[i]->addforce(tmprgt);
		//add -right forces to 5,7
		quader[i+5]->addforce(-tmprgt);
		//add left forces to 1,3
		quader[i+1]->addforce(tmplft);
		//add -left forces to 4,6
		quader[i+4]->addforce(-tmplft);

	}
	//Water interaction: set 1 sphere
	water->EmbossSphere((float*)center->getValue());

}

void Driver::Accelerate() {
	if (accforce < forcemax)
		accforce += forcestep;
	brakeforce = 0;
}

void Driver::Brake() {
	if (brakeforce < forcemax)
		brakeforce += forcestep;
	accforce = 0;
}

void Driver::Left() {
	if (leftforce < forcemax)
		leftforce += forcestep;
	rightforce =0;
}

void Driver::Right() {
	if (rightforce < forcemax)
		rightforce += forcestep;
	leftforce = 0;
}

void Driver::DampForward() {
	accforce -= forcestep;
	brakeforce -= forcestep;
	if (accforce < 0) accforce = 0;
	if (brakeforce < 0) brakeforce = 0;
}

void Driver::DampSideward() {
	rightforce -= forcestep;
	leftforce -= forcestep;
	if (rightforce < 0) rightforce = 0;
	if (leftforce < 0) leftforce = 0;
}

SbVec3f Driver::GetForward() {		// returns forward vector (new instance of it?)
	return *forward;
}

SbVec3f Driver::GetPosition() {
	return *center;					// does this return a copy of the SbVec3f??
}

SbVec3f Driver::GetRight() {
	return *right;
}

void Driver::Recalc() {
	//Calculate centers
	*center = SbVec3f(0.0f,0.0f,0.0f);
	*frontcenter = SbVec3f(0.0f,0.0f,0.0f);
	*backcenter = SbVec3f(0.0f,0.0f,0.0f);
	
	for(int i = 0; i < 4; i++) {
		*frontcenter = *frontcenter + quader[i]->pos;
		*backcenter = *backcenter + quader[i+4]->pos;
	};

	*frontcenter /= 4.0f;
	*backcenter /= 4.0f;
	*center = (*frontcenter + *backcenter) /2.0f;

	//Calculate forward/upward/right direction
	*forward = *frontcenter - *backcenter;
	//set y coordinate to 0 to remain horizontal
	float x, y, z;
	forward->getValue(x, y, z);
	forward->setValue(x, 0.0f, z);
	forward->normalize();

	*upward = SbVec3f(0.0f, 1.0f, 0.0f);
	*right = forward->cross(*upward);
}