#include "PullPoint.h"

#include <iostream>

#define STRONG_FORCE 500000
#define NORMAL_FORCE 5

using namespace std;

namespace dojo {

	PullPoint::PullPoint(const std::string& name, Entity::Part* l) : Entity(name, "Root", false) 
	{
		timeStep = .1f;
		inPosition = true;
		limbLock = true;
		move = false;
		limb = l;
		pullForce = NORMAL_FORCE;
		speed = 1.0f;
		destination = this->frame().translation;
	}

	void PullPoint::onSimulation(G3D::SimTime ts) 
	{
		timeStep = ts;
		Vector3 diff = -(limb->frame().translation - this->frame().translation);
		if (abs(diff.x) >= 2 || abs(diff.y) >= 2 || abs(diff.z) >= 2) {
			this->setFrame(limb->frame());
			destination = limb->frame().translation;
			//limb->setAngularVelocity(Vector3::ZERO);
			//limb->setLinearVelocity(Vector3::ZERO);
		}
		if (move) {// || diff.x >= 2 || diff.y >= 2 || diff.z >= 2) {
			Vector3 diff2 = -(frame().translation - destination);
			if (abs(diff2.x) > .01 || abs(diff2.y) > .01 || abs(diff2.z) > .01) {
				this->setFrame(CoordinateFrame(frame().translation + shift));
				inPosition = false;
			} else {
				inPosition = true;
				move = false;
				speed = 1.0f;
			}
		}

		//velocity = limb->linearVelocity();
		if (abs(limb->frame().translation.x - this->frame().translation.x) < 0.1
			&& abs(limb->frame().translation.y - this->frame().translation.y) < 0.1
			&& abs(limb->frame().translation.z - this->frame().translation.z) < 0.1
			&& pullForce == NORMAL_FORCE) {
			limb->setAngularVelocity(Vector3::ZERO);
			limb->setLinearVelocity(Vector3::ZERO);
		} else {
			float dampenedForce = (pullForce * ts);
			Vector3 force = -(limb->frame().translation - this->frame().translation) * dampenedForce;
			limb->applyForce(force);
			if (pullForce > NORMAL_FORCE) pullForce -= (limb->mass() * STRONG_FORCE) / 3;
			if (pullForce < NORMAL_FORCE) pullForce = NORMAL_FORCE; // / 5;
		}
	}

	void PullPoint::initPosition() {
		this->setFrame("Root", limb->frame());
	}

	void PullPoint::attackForce() {
		pullForce = (limb->mass() * STRONG_FORCE > 400) ? 400 : limb->mass() * STRONG_FORCE;
		//speed = 5.0f;
		this->setFrame(CoordinateFrame(destination));
	}

	void PullPoint::limpForce(void)
	{
		pullForce = 0.0f;
		speed = 1.0f;
	}

	void PullPoint::toggleLock() {
		limbLock = !limbLock;
	}

	bool PullPoint::isInPosition() {
		return inPosition;
	}

	void PullPoint::moveTo(Vector3 dest)
	{
		move = true;
		destination = dest;
		Vector3 diff2 = -(frame().translation - destination);
		float damp = speed * timeStep < 1 ? speed * timeStep : 1;
		shift = Vector3(
			abs(diff2.x) > .01 ? diff2.x * damp : 0,
			abs(diff2.y) > .01 ? diff2.y * damp : 0,
			abs(diff2.z) > .01 ? diff2.z * damp : 0);
	}

	void PullPoint::setSpeed(float s) {
		speed = s;
	}

	void PullPoint::setVelocity(Vector3 v)
	{
		velocity = v;
	}

}