#include "toonz/ikengine.h"

#include "toonz/ikjacobian.h"

enum Method { JACOB_TRANS,

			  PURE_PSEUDO,

			  DLS,

			  SDLS,

			  COMPARE };

IKEngine::IKEngine()

{

}

int IKEngine::addJoint(const TPointD &pos, int indexParent)

{

	// TODO: evitare che si formino segmenti nulli

	//assert(m_joints.empty() || norm2(pos-m_joints.back())>0.000001);

	//assert(m_nodes[indexParent]);

	assert(m_skeleton.getNode(indexParent));

	m_skeleton.addNode(new IKNode());

	int index = m_skeleton.getNodeCount() - 1;

	m_skeleton.setNode(index, pos, IKNode::JOINT);

	m_skeleton.setParent(index, indexParent);

	return index;

}

//la root deve coincidere con un punto bloccato!

void IKEngine::setRoot(const TPointD &pos)

{

	m_skeleton.addNode(new IKNode());

	m_skeleton.setNode(0, pos, IKNode::JOINT);

	//m_skeleton.setParent(0,0);

	m_skeleton.setRoot(0);

}

void IKEngine::lock(int index)

{

	assert(index > -1 && index < m_skeleton.getNodeCount());

	m_skeleton.setPurpose(index, IKNode::EFFECTOR);

	IKNode *n = m_skeleton.getNode(index);

	TPointD pos = n->getPos();

	target.push_back(pos);

}

void IKEngine::unlock(int index)

{

	assert(index > -1 && index < m_skeleton.getNodeCount());

	m_skeleton.setPurpose(index, IKNode::JOINT);

}

bool IKEngine::isLocked(int index)

{

	assert(index > -1 && index < m_skeleton.getNodeCount());

	return m_skeleton.getNode(index)->IsEffector();

}

double IKEngine::getJointAngle(int index)

{

	assert(index > -1 && index < m_skeleton.getNodeCount());

	TPointD pos = m_skeleton.getNode(index)->getPos();

	TPointD u(1, 0);

	if (index != 0) {

		int parent = getJointParent(index);

		TPointD prevPos = m_skeleton.getNode(parent)->getPos();

		u = normalize(pos - prevPos);

	}

	TPointD v(-u.y, u.x);

	TPointD nextPos = m_skeleton.getNode(index + 1)->getPos();

	TPointD d = nextPos - pos;

	double theta = atan2(d * v, d * u);

	return theta;

}

void IKEngine::drag(TPointD &pos)

{

	//assert(index>-1 && index

	// se lo scheletro è vuoto non succede nulla

	if (m_skeleton.getNodeCount() == 0)

		return;

	// afferro l'ultimo punto della catena

	int indexDrag = m_skeleton.getNodeCount() - 1;

	if (m_skeleton.getNode(indexDrag)->getParent()->IsEffector())

		return;

	m_skeleton.setPurpose(indexDrag, IKNode::EFFECTOR);

	//assegno un indice alla sequenza dei giunti (nodi -end effectors)

	setSequenceJoints();

	target.push_back(pos);

	Jacobian jacobian(&m_skeleton, target);

	target.pop_back();

	for (int i = 0; i < 250; i++)

		doUpdateStep(jacobian);

}

void IKEngine::doUpdateStep(Jacobian &jacobian)

{

	jacobian.computeJacobian(); // calcolo Jacobiano e il vettore deltaS

	int WhichMethod = DLS;

	bool clampingDetected = true;

	while (clampingDetected) {

		// Calcolo i cambiamenti dell'angolo

		switch (WhichMethod) {

		case JACOB_TRANS:

			jacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method

			break;

		case DLS:

			jacobian.CalcDeltaThetasDLS(); // Damped least squares method

			break;

		case PURE_PSEUDO:

			jacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method

			break;

		case SDLS:

			jacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method

			break;

		default:

			jacobian.ZeroDeltaThetas();

			break;

		}

		jacobian.UpdateThetas(); // Aggiorno i valori di theta

		clampingDetected = jacobian.checkJointsLimit();

		//jacobian.UpdatedSClampValue();

	}

}

// Assegno gli indici nella sequenza dei giunti

void IKEngine::setSequenceJoints()

{

	int indexJoint = 0;

	for (int i = 0; i < (int)m_skeleton.getNodeCount(); i++) {

		IKNode *n = m_skeleton.getNode(i);

		IKNode::Purpose purpose = n->getPurpose();

		if (purpose != IKNode::EFFECTOR) {

			n->setSeqNumJoint(indexJoint);

			indexJoint++;

		}

	}

}