#include "skeletonsubtools.h"

#include "skeletontool.h"

#include "tools/toolhandle.h"

#include "tools/toolutils.h"

#include "toonz/tscenehandle.h"

#include "toonz/tobjecthandle.h"

#include "toonz/tframehandle.h"

#include "toonz/tcolumnhandle.h"

#include "toonz/txsheethandle.h"

#include "toonz/txshlevelhandle.h"

#include "toonz/toonzscene.h"

#include "toonz/txshsimplelevel.h"

#include "toonz/tstageobjecttree.h"

#include "toonz/tstageobjectcmd.h"

#include "toonz/tpinnedrangeset.h"

#include "toonz/stage.h"

#include "toonz/hook.h"

#include "toonz/txshcell.h"

#include "toonz/skeleton.h"

#include "toonz/stageobjectutil.h"

#include "tgl.h"

#include "tconvert.h"

#include <tsystem.h></tsystem.h>

#include <math.h></math.h>

using namespace SkeletonSubtools;

//============================================================

//

// DragCenterTool

//

//------------------------------------------------------------

DragCenterTool::DragCenterTool(SkeletonTool *tool)

	: DragTool(tool), m_objId(TTool::getApplication()->getCurrentObject()->getObjectId()), m_frame(TTool::getApplication()->getCurrentFrame()->getFrame())

{

}

//------------------------------------------------------------

void DragCenterTool::leftButtonDown(const TPointD &pos, const TMouseEvent &)

{

	TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

	m_center = m_oldCenter = xsh->getCenter(m_objId, m_frame);

	m_firstPos = pos;

	m_affine = xsh->getPlacement(m_objId, m_frame).inv() * xsh->getParentPlacement(m_objId, m_frame);

	m_affine.a13 = m_affine.a23 = 0;

}

//------------------------------------------------------------

void DragCenterTool::leftButtonDrag(const TPointD &pos, const TMouseEvent &)

{

	double factor = 1.0 / Stage::inch;

	m_center = m_oldCenter + (m_affine * (pos - m_firstPos)) * factor;

	TTool::getApplication()->getCurrentXsheet()->getXsheet()->setCenter(m_objId, m_frame, m_center);

}

//------------------------------------------------------------

void DragCenterTool::leftButtonUp(const TPointD &pos, const TMouseEvent &)

{

	UndoStageObjectCenterMove *undo = new UndoStageObjectCenterMove(m_objId, m_frame, m_oldCenter, m_center);

	TTool::Application *app = TTool::getApplication();

	undo->setObjectHandle(app->getCurrentObject());

	undo->setXsheetHandle(app->getCurrentXsheet());

	TUndoManager::manager()->add(undo);

}

//============================================================

//

// DragChannelTool

//

//------------------------------------------------------------

DragChannelTool::DragChannelTool(SkeletonTool *tool, TStageObject::Channel a0)

	: DragTool(tool)

{

	TTool::Application *app = TTool::getApplication();

	m_before.setFrameHandle(app->getCurrentFrame());

	m_before.setObjectHandle(app->getCurrentObject());

	m_before.setXsheetHandle(app->getCurrentXsheet());

	m_before.add(a0);

	if (tool->isGlobalKeyframesEnabled()) {

		m_before.add(TStageObject::T_Angle);

		m_before.add(TStageObject::T_X);

		m_before.add(TStageObject::T_Y);

		m_before.add(TStageObject::T_Z);

		m_before.add(TStageObject::T_SO);

		m_before.add(TStageObject::T_ScaleX);

		m_before.add(TStageObject::T_ScaleY);

		m_before.add(TStageObject::T_Scale);

		m_before.add(TStageObject::T_Path);

		m_before.add(TStageObject::T_ShearX);

		m_before.add(TStageObject::T_ShearY);

	}

	m_after = m_before;

	m_dragged = false;

}

//------------------------------------------------------------

DragChannelTool::DragChannelTool(SkeletonTool *tool, TStageObject::Channel a0, TStageObject::Channel a1)

	: DragTool(tool)

{

	TTool::Application *app = TTool::getApplication();

	m_before.setFrameHandle(app->getCurrentFrame());

	m_before.setObjectHandle(app->getCurrentObject());

	m_before.setXsheetHandle(app->getCurrentXsheet());

	m_before.add(a0);

	m_before.add(a1);

	if (tool->isGlobalKeyframesEnabled()) {

		m_before.add(TStageObject::T_Angle);

		m_before.add(TStageObject::T_X);

		m_before.add(TStageObject::T_Y);

		m_before.add(TStageObject::T_Z);

		m_before.add(TStageObject::T_SO);

		m_before.add(TStageObject::T_ScaleX);

		m_before.add(TStageObject::T_ScaleY);

		m_before.add(TStageObject::T_Scale);

		m_before.add(TStageObject::T_Path);

		m_before.add(TStageObject::T_ShearX);

		m_before.add(TStageObject::T_ShearY);

	}

	m_after = m_before;

	m_dragged = false;

}

//------------------------------------------------------------

void DragChannelTool::start()

{

	m_before.updateValues();

	m_after = m_before;

}

//------------------------------------------------------------

TPointD DragChannelTool::getCenter()

{

	TTool::Application *app = TTool::getApplication();

	TStageObjectId objId = app->getCurrentObject()->getObjectId();

	int frame = app->getCurrentFrame()->getFrame();

	TXsheet *xsh = app->getCurrentXsheet()->getXsheet();

	return xsh->getParentPlacement(objId, frame).inv() * xsh->getPlacement(objId, frame) * (Stage::inch * xsh->getCenter(objId, frame));

}

//------------------------------------------------------------

void DragChannelTool::leftButtonUp(const TPointD &pos, const TMouseEvent &)

{

	if (m_dragged) {

		if (getTool()->isGlobalKeyframesEnabled())

			m_after.setGlobalKeyframe();

		TTool::Application *app = TTool::getApplication();

		UndoStageObjectMove *undo = new UndoStageObjectMove(m_before, m_after);

		undo->setObjectHandle(app->getCurrentObject());

		TUndoManager::manager()->add(undo);

		app->getCurrentScene()->setDirtyFlag(true);

		app->getCurrentXsheet()->notifyXsheetChanged();

		app->getCurrentObject()->notifyObjectIdChanged(false); // Keyframes navigator reads this

	}

	m_dragged = false;

}

//============================================================

//

// DragPositionTool

//

//------------------------------------------------------------

DragPositionTool::DragPositionTool(SkeletonTool *tool)

	: DragChannelTool(tool, TStageObject::T_X, TStageObject::T_Y)

{

}

//------------------------------------------------------------

void DragPositionTool::leftButtonDown(const TPointD &pos, const TMouseEvent &)

{

	start();

	m_firstPos = pos;

}

//------------------------------------------------------------

void DragPositionTool::leftButtonDrag(const TPointD &pos, const TMouseEvent &e)

{

	TPointD delta = pos - m_firstPos;

	if (e.isShiftPressed()) {

		if (fabs(delta.x) > fabs(delta.y))

			delta.y = 0;

		else

			delta.x = 0;

	}

	double factor = 1.0 / Stage::inch;

	setValues(getOldValue(0) + delta.x * factor, getOldValue(1) + delta.y * factor);

	m_dragged = true;

}

//============================================================

//

// DragRotationTool

//

//------------------------------------------------------------

DragRotationTool::DragRotationTool(SkeletonTool *tool, bool snapped)

	: DragChannelTool(tool, TStageObject::T_Angle), m_snapped(snapped)

{

}

//------------------------------------------------------------

void DragRotationTool::leftButtonDown(const TPointD &pos, const TMouseEvent &)

{

	m_lastPos = pos;

	m_center = getCenter();

	start();

}

//------------------------------------------------------------

void DragRotationTool::leftButtonDrag(const TPointD &pos, const TMouseEvent &)

{

	TPointD a = m_center - m_lastPos;

	TPointD b = m_center - pos;

	double a0 = norm2(pos - m_lastPos);

	if (a0 < 2 && !m_dragged)

		return;

	double a2 = norm2(a), b2 = norm2(b);

	const double eps = 1e-1;

	if (a2 < eps || b2 < eps)

		return;

	double dang = 180 * asin(cross(a, b) / sqrt(a2 * b2)) / TConsts::pi;

	if (m_snapped) {

		if (fabs(dang) < 2)

			return;

		m_snapped = false;

	}

	setValue(getValue(0) + dang);

	m_dragged = true;

	m_lastPos = pos;

}

//============================================================

//

// ParentChangeTool

//

//------------------------------------------------------------

ParentChangeTool::ParentChangeTool(SkeletonTool *tool, TTool::Viewer *viewer)

	: DragTool(tool), m_viewer(viewer), m_index(-1), m_snapped(true), m_pixelSize(tool->getPixelSize())

{

}

//------------------------------------------------------------

void ParentChangeTool::leftButtonDown(const TPointD &pos, const TMouseEvent &e)

{

	TTool::Application *app = TTool::getApplication();

	TXsheet *xsh = app->getCurrentScene()->getScene()->getXsheet();

	// colonna, id e frame corrente

	int currentColumnIndex = app->getCurrentColumn()->getColumnIndex();

	int currentFrame = app->getCurrentFrame()->getFrame();

	TStageObjectId id = TStageObjectId::ColumnId(currentColumnIndex);

	TStageObject *obj = xsh->getStageObject(id);

	obj->getCenterAndOffset(m_oldCenter, m_oldOffset);

	// trovo il centro della colonna corrente

	TAffine aff = xsh->getPlacement(id, currentFrame);

	TPointD center = Stage::inch * xsh->getCenter(id, currentFrame);

	m_center = aff * center;

	// mi serve in coordinate assolute (per questo non uso direttamente pos)

	m_lastPos = m_viewer->winToWorld(e.m_pos);

	m_lastPos2 = pos;

	// if(m_mode==1) getTool()->setParentProbe(pos);

	// cerco centri e hooks di tutte le altre colonne (non discendenti)

	int nextName = 10000;

	for (int col = 0; col < xsh->getColumnCount(); col++) {

		TXshCell cell = xsh->getCell(currentFrame, col);

		if (cell.isEmpty())

			continue;

		TStageObjectId colId = TStageObjectId::ColumnId(col);

		// se colId e' un discendente di id non e' un candidato a diventare

		// il padre di id.

		TStageObjectId tmpId = colId;

		while (tmpId.isColumn() && tmpId != id)

			tmpId = xsh->getStageObjectParent(tmpId);

		if (tmpId == id)

			continue;

		// registro alcune informazioni relative a colId:

		// placement, bbox, centro, hooks, name, ecc.

		Element element;

		element.m_columnIndex = col;

		TImageP img = cell.getImage(false);

		if (img)

			element.m_bbox = img->getBBox();

		element.m_aff = xsh->getPlacement(colId, currentFrame);

		Peer peer;

		peer.m_columnIndex = col;

		peer.m_handle = 0;

		peer.m_name = nextName++;

		peer.m_pos = element.m_aff * (Stage::inch * xsh->getCenter(colId, currentFrame));

		element.m_peers.push_back(peer);

		TXshLevel *xl = cell.m_level.getPointer();

		if (xl) {

			HookSet *hookSet = xl->getHookSet();

			TStageObject *pegbar = xsh->getStageObject(colId);

			if (hookSet && pegbar) {

				for (int i = 0; i < hookSet->getHookCount(); i++) {

					if (!hookSet->getHook(i))

						continue;

					peer.m_handle = 1 + i;

					peer.m_pos = element.m_aff * hookSet->getHook(i)->getAPos(cell.m_frameId);

					peer.m_name = nextName++;

					element.m_peers.push_back(peer);

				}

			}

		}

		m_elements[col] = element;

	}

	//m_newParentCenter = TPointD();

	m_firstWinPos = e.m_pos;

	m_affine = xsh->getParentPlacement(id, currentFrame).inv();

	m_objId = id;

}

//------------------------------------------------------------

void ParentChangeTool::leftButtonDrag(const TPointD &pos, const TMouseEvent &e)

{

	if (m_snapped && norm2(e.m_pos - m_firstWinPos) > 200) {

		TTool::Application *app = TTool::getApplication();

		int currentColumnIndex = app->getCurrentColumn()->getColumnIndex();

		TStageObjectId objId = TStageObjectId::ColumnId(currentColumnIndex);

		TStageObjectCmd::setParent(objId, TStageObjectId::NoneId, "", app->getCurrentXsheet());

		m_snapped = false;

	}

	if (m_snapped)

		return;

	getTool()->setParentProbe(getTool()->getCurrentColumnParentMatrix() * pos);

	m_index = m_viewer->posToColumnIndex(e.m_pos, m_pixelSize * 5, false);

	m_lastPos = m_viewer->winToWorld(e.m_pos);

	m_handle = -1;

	if (m_index >= 0) {

		double minDist2 = 0;

		for (int i = 0; i < (int)m_elements[m_index].m_peers.size(); i++) {

			TPointD targetPos = m_elements[m_index].m_peers[i].m_pos;

			double dist2 = norm2(targetPos - m_lastPos);

			if (m_handle < 0 || dist2 < minDist2) {

				minDist2 = dist2;

				m_handle = m_elements[m_index].m_peers[i].m_handle;

			}

		}

	}

}

//------------------------------------------------------------

void ParentChangeTool::leftButtonUp(const TPointD &pos, const TMouseEvent &e)

{

	TTool::Application *app = TTool::getApplication();

	int currentColumnIndex = app->getCurrentColumn()->getColumnIndex();

	int currentFrame = app->getCurrentFrame()->getFrame();

	TXsheet *xsh = app->getCurrentScene()->getScene()->getXsheet();

	TStageObjectId objId = TStageObjectId::ColumnId(currentColumnIndex);

	getTool()->resetParentProbe();

	if (m_snapped) {

		xsh->getStageObject(objId)->setCenterAndOffset(m_oldCenter, m_oldOffset);

		return;

	}

	if (getTool()->getMagicLinkCount() > 0) {

		MagicLink magicLink = getTool()->getMagicLink(0);

		HookData h0 = magicLink.m_h0;

		HookData h1 = magicLink.m_h1;

		TStageObject *obj = xsh->getStageObject(objId);

		int parentColumnIndex = h1.m_columnIndex;

		TStageObjectId parentId = TStageObjectId::ColumnId(parentColumnIndex);

		std::string parentHandle = h1.getHandle();

		std::string handle = "";

		if (h0.m_columnIndex < 0) {

			handle = obj->getHandle();

		} else {

			handle = h0.getHandle();

		}

		// TUndoManager *undoManager = TUndoManager::manager();

		// undoManager->beginBlock();

		// TStageObjectCmd::resetPosition(id, app->getCurrentXsheet());

		TStageObjectCmd::setHandle(objId, handle, app->getCurrentXsheet());

		TStageObjectCmd::setParent(objId, parentId, parentHandle, app->getCurrentXsheet());

		// undoManager->endBlock();

	} else {

		TStageObjectCmd::setParent(objId, TStageObjectId::NoneId, "", app->getCurrentXsheet());

		xsh->getStageObject(objId)->setCenterAndOffset(m_oldCenter, m_oldOffset);

	}

}

//------------------------------------------------------------

void ParentChangeTool::draw()

{

	getTool()->drawHooks();

}

//============================================================

//

// IKTool helper functions

//

//------------------------------------------------------------

namespace

{

class Graph

{

public:

	typedef std::set<int> Links;</int>

	typedef Links::const_iterator LinkIter;

	typedef std::map<int, links=""> Nodes;</int,>

	typedef Nodes::const_iterator NodeIter;

	typedef std::map<int, int=""> LeaveTable;</int,>

	typedef LeaveTable::const_iterator LeaveIter;

	Graph() {}

	int touch(int id)

	{

		if (m_nodes.count(id) == 0)

			m_nodes[id] = Links();

		return id;

	}

	bool isNode(int id) const { return m_nodes.count(id) > 0; }

	int getNodeCount() const { return (int)m_nodes.size(); }

	void link(int a, int b)

	{

		touch(a);

		touch(b);

		m_nodes[a].insert(b);

		m_nodes[b].insert(a);

	}

	bool areLinked(int a, int b) const

	{

		NodeIter it = m_nodes.find(a);

		return it == m_nodes.end() ? false : it->second.count(b);

	}

	const Links &getLinks(int id) const

	{

		static const Links empty;

		NodeIter it = m_nodes.find(id);

		return it == m_nodes.end() ? empty : it->second;

	}

	int getLinkCount(int id) const { return (int)getLinks(id).size(); }

	int getFirstLink(int id) const

	{

		const Links &links = getLinks(id);

		return links.empty() ? -1 : *(links.begin());

	}

	NodeIter begin() const { return m_nodes.begin(); }

	NodeIter end() const { return m_nodes.end(); }

	bool isLeave(int id) const { return m_leaves.count(id); }

	enum LeaveType { PINNED = 0x1,

					 TEMP = 0x2,

					 CHILD_END = 0x4,

					 PARENT_END = 0x8 };

	int getLeaveType(int id) const

	{

		LeaveIter it = m_leaves.find(id);

		return it == m_leaves.end() ? 0 : it->second;

	}

	void setLeaveType(int id, int type) { m_leaves[id] = type; }

	void remove(int id)

	{

		NodeIter it = m_nodes.find(id);

		if (it != m_nodes.end()) {

			for (LinkIter j = it->second.begin(); j != it->second.end(); ++j)

				m_nodes[*j].erase(id);

			m_nodes.erase(it->first);

		}

	}

	void clear()

	{

		m_nodes.clear();

		m_leaves.clear();

	}

private:

	Nodes m_nodes;

	std::map<int, int=""> m_leaves;</int,>

};

//------------------------------------------------------------

bool hasPinned(const Skeleton::Bone *bone, const Skeleton::Bone *prevBone)

{

	if (!bone)

		return false;

	bool isHandle = prevBone == 0;

	bool isChild = prevBone != 0 && prevBone == bone->getParent();

	if (bone->getPinnedStatus() != Skeleton::Bone::FREE)

		return true;

	if (bone->getParent() && bone->getParent() != prevBone && hasPinned(bone->getParent(), bone))

		return true;

	for (int i = 0; i < bone->getChildCount(); i++)

		if (bone->getChild(i) != prevBone)

			if (hasPinned(bone->getChild(i), bone))

				return true;

	return false;

}

// create the tree from the skeleton. the tree contains the active chain only

// i.e. the nodes bounded by the pinned nodes and the handle

bool addToActiveChain(Graph &tree, const Skeleton::Bone *bone, const Skeleton::Bone *prevBone)

{

	if (!bone)

		return false;

	bool isHandle = prevBone == 0;

	bool isChild = prevBone != 0 && prevBone == bone->getParent();

	bool isParent = prevBone != 0 && prevBone->getParent() == bone;

	int pinnedStatus = bone->getPinnedStatus();

	bool isFree = pinnedStatus == Skeleton::Bone::FREE;

	// bool isPinned = pinnedStatus == Skeleton::Bone::PINNED;

	bool isTempPinned = pinnedStatus == Skeleton::Bone::TEMP_PINNED;

	bool propagate = false;

	if (!isChild && isFree)

		if (bone->getParent())

			propagate |= addToActiveChain(tree, bone->getParent(), bone);

	std::vector<int> children;</int>

	if (isHandle || isFree)

		for (int i = 0; i < bone->getChildCount(); i++)

			if (bone->getChild(i) != prevBone)

				propagate |= addToActiveChain(tree, bone->getChild(i), bone);

	bool insert = false;

	if (isHandle) // the handle must be added anyway

		insert = true;

	else if (isChild) // add child if it's pinned or if some gran-child has been added

		insert = !isFree || propagate;

	else if (isTempPinned) {

		// parent temp pinned are normally added, but if another branch is pinned we are not

		// able to handle the configuration: then consider it as pinned

		bool locked = false;

		// if(bone->getParent()) locked |= hasPinned(bone->getParent(), bone);

		for (int i = 0; i < bone->getChildCount() && !locked; i++)

			if (bone->getChild(i) != prevBone)

				locked |= hasPinned(bone->getChild(i), bone);

		insert = !locked;

	} else // add parent if free and grand-parent or some other child has been added; don't add pinned parent

		insert = isFree && propagate;

	if (insert) {

		int index = tree.touch(bone->getColumnIndex());

		if (!isFree)

			tree.setLeaveType(index, isTempPinned ? 2 : 1);

		else if (!isChild && bone->getParent() && !tree.isNode(bone->getParent()->getColumnIndex())) {

			// the parent has not been added. it was locked => this node act as pinned

			tree.setLeaveType(index, 2);

		}

		if (prevBone)

			tree.link(prevBone->getColumnIndex(), index);

	}

	return isHandle || !isFree || propagate;

}

//------------------------------------------------------------

} // namespace

//============================================================

//

// IKToolUndo

//

//------------------------------------------------------------

namespace SkeletonSubtools

{

class IKToolUndo : public TUndo

{

	struct Node {

		TStageObjectId m_id;

		double m_oldAngle, m_newAngle;

		bool m_wasKeyframe;

	};

	std::vector<node> m_nodes;</node>

	TStageObjectId m_firstFootId;

	TAffine m_oldFootPlacement, m_newFootPlacement;

	int m_frame;

public:

	IKToolUndo() {}

	void setFirstFootId(const TStageObjectId &firstFootId) { m_firstFootId = firstFootId; }

	void setFirstFootOldPlacement(const TAffine &oldPlacement) { m_oldFootPlacement = oldPlacement; }

	void setFirstFootNewPlacement(const TAffine &newPlacement) { m_newFootPlacement = newPlacement; }

	void addNode(const TStageObjectId &id)

	{

		m_nodes.push_back(Node());

		m_nodes.back().m_id = id;

		TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

		int frame = TTool::getApplication()->getCurrentFrame()->getFrame();

		TDoubleParam *param = xsh->getStageObject(id)->getParam(TStageObject::T_Angle);

		m_nodes.back().m_oldAngle = param->getValue(frame);

		m_nodes.back().m_wasKeyframe = param->isKeyframe(frame);

	}

	void onAdd()

	{

		TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

		m_frame = TTool::getApplication()->getCurrentFrame()->getFrame();

		for (int i = 0; i < (int)m_nodes.size(); i++) {

			TDoubleParam *param = xsh->getStageObject(m_nodes[i].m_id)->getParam(TStageObject::T_Angle);

			m_nodes[i].m_newAngle = param->getValue(m_frame);

		}

	}

	void setFootPlacement(const TAffine &placement) const

	{

		if (!m_firstFootId.isColumn())

			return;

		TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

		TStageObject *obj = xsh->getStageObject(m_firstFootId);

		TPinnedRangeSet *rangeSet = obj->getPinnedRangeSet();

		rangeSet->setPlacement(placement);

		while (obj->getParent().isColumn())

			obj = xsh->getStageObject(obj->getParent());

		obj->invalidate();

	}

	void undo() const

	{

		TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

		for (int i = 0; i < (int)m_nodes.size(); i++) {

			TDoubleParam *param = xsh->getStageObject(m_nodes[i].m_id)->getParam(TStageObject::T_Angle);

			if (m_nodes[i].m_wasKeyframe)

				param->setValue(m_frame, m_nodes[i].m_oldAngle);

			else

				param->deleteKeyframe(m_frame);

		}

		if (m_firstFootId.isColumn())

			setFootPlacement(m_oldFootPlacement);

		TTool::getApplication()->getCurrentXsheet()->notifyXsheetChanged();

		TTool::getApplication()->getCurrentObject()->notifyObjectIdChanged(false);

	}

	void redo() const

	{

		TXsheet *xsh = TTool::getApplication()->getCurrentXsheet()->getXsheet();

		for (int i = 0; i < (int)m_nodes.size(); i++) {

			TDoubleParam *param = xsh->getStageObject(m_nodes[i].m_id)->getParam(TStageObject::T_Angle);

			param->setValue(m_frame, m_nodes[i].m_newAngle);

		}

		if (m_firstFootId.isColumn())

			setFootPlacement(m_newFootPlacement);

		TTool::getApplication()->getCurrentXsheet()->notifyXsheetChanged();

		TTool::getApplication()->getCurrentObject()->notifyObjectIdChanged(false);

	}

	int getSize() const { return sizeof(*this) + (int)m_nodes.size() * sizeof(Node); }

};

} // namespace SkeletonSubtools

//============================================================

//

// IKTool

//

//------------------------------------------------------------

IKTool::IKTool(SkeletonTool *tool, TTool::Viewer *viewer, Skeleton *skeleton, int columnIndex)

	: DragTool(tool), m_viewer(viewer), m_skeleton(skeleton), m_pos(), m_columnIndex(columnIndex), m_valid(false), m_IHateIK(false), m_foot(0), m_firstFoot(0), m_undo(0)

{

}

//------------------------------------------------------------

IKTool::~IKTool()

{

	delete m_skeleton;

}

//------------------------------------------------------------

bool IKTool::isParentOf(int columnIndex, int childColumnIndex) const

{

	Skeleton::Bone *bone = m_skeleton->getBoneByColumnIndex(columnIndex);

	Skeleton::Bone *childBone = m_skeleton->getBoneByColumnIndex(childColumnIndex);

	return bone && childBone && childBone->getParent() == bone;

}

//------------------------------------------------------------

void IKTool::initEngine(const TPointD &pos)

{

	m_valid = false;

	m_engine.clear();

	m_joints.clear();

	// m_skeleton->getRootBone()->getStageObject()->setStatus(TStageObject::IK);

	// build the active chain (bounded by m_columnIndex and pinned nodes)

	Graph chain;

	addToActiveChain(chain, m_skeleton->getBoneByColumnIndex(m_columnIndex), 0);

	// mark leaves as child_end or parent_end: a leave is child/parent_end iff

	// all its link are parent/child

	for (Graph::NodeIter it = chain.begin(); it != chain.end(); ++it)

		if (chain.getLeaveType(it->first) & (Graph::PINNED | Graph::TEMP)) {

			int type = 0;

			for (Graph::LinkIter j = it->second.begin(); j != it->second.end(); ++j) {

				int f = isParentOf(*j, it->first) ? Graph::CHILD_END : Graph::PARENT_END;

				if (type == 0)

					type = f;

				else if (type != f) {

					type = 0;

					break;

				}

			}

			chain.setLeaveType(it->first, chain.getLeaveType(it->first) | type);

		}

	// search the foot (i.e. the pinned node). if there are no pinned node find the first

	// temp-pinned

	int foot = -1;

	for (Graph::NodeIter it = chain.begin(); it != chain.end(); ++it)

		if (chain.isLeave(it->first)) {

			if ((chain.getLeaveType(it->first) & Graph::PINNED) != 0 || foot < 0)

				foot = it->first;

		}

	if (foot < 0 || !chain.isNode(foot))

		return;

	// If the chain descend from the foot then the whole foot should be still (e.g. no rotation)

	// => chop the foot

	if (chain.getLinkCount(foot) == 1 && (chain.getLeaveType(foot) & Graph::PINNED)) {

		int nextNode = chain.getFirstLink(foot);

		if (isParentOf(foot, nextNode)) {

			chain.remove(foot);

			foot = nextNode;

			chain.setLeaveType(foot, 3);

		}

	}

	// if the handle is a terminal node (i.e. just one link) and the next node is a child

	// move the handle to the next node

	int handle = m_columnIndex;

	if (chain.getLinkCount(handle) == 1) {

		int nextNode = chain.getFirstLink(handle);

		if (isParentOf(handle, nextNode)) {

			chain.remove(handle);

			handle = nextNode;

		}

	}

	//

	// "reverse" the tree to suit the IKEngine convention

	std::vector<std::pair<int, *="" skeleton::bone="">> stack;</std::pair<int,>

	std::vector<int> done;</int>

	stack.push_back(std::make_pair(foot, (Skeleton::Bone *)0));

	while (!stack.empty()) {

		Skeleton::Bone *bone = m_skeleton->getBoneByColumnIndex(stack.back().first);

		Skeleton::Bone *prev = stack.back().second;

		Skeleton::Bone *prev2 = prev;

		stack.pop_back();

		for (;;) {

			int id = bone->getColumnIndex();

			done.push_back(id);

			double sign = 1;

			if (prev) {

				if (prev->getParent() == bone)

					sign = -1;

			} else if (done.size() == 1) {

				if (chain.getLeaveType(id) & Graph::CHILD_END)

					sign = -1;

			} else {

				if (chain.getLeaveType(id) & Graph::PARENT_END)

					sign = -1;

			}

			Graph::Links links = chain.getLinks(bone->getColumnIndex());

			for (int i = 0; i < (int)done.size(); i++)

				links.erase(done[i]);

			if (links.size() == 0) {

				// terminal point

				m_joints.push_back(Joint(bone, prev, sign));

				break;

			} else {

				Graph::LinkIter it = links.begin();

				Skeleton::Bone *next = m_skeleton->getBoneByColumnIndex(*it++);

				for (; it != links.end(); ++it)

					stack.push_back(std::make_pair(*it, prev));

				if (links.size() > 1 || (prev && next && (prev->getParent() == bone && next->getParent() == bone))) {

					bone = next;

				} else {

					m_joints.push_back(Joint(bone, prev, sign));

					prev = bone;

					bone = next;

				}

			}

		}

	}

	//if(m_joints.size()>=2 && m_joints[1].m_prevBone == m_joints[0].m_bone)

	//  m_joints[0].m_sign = m_joints[1].m_sign;

	// feed the engine

	m_engine.setRoot(m_joints[0].m_bone->getCenter());

	for (int i = 1; i < (int)m_joints.size(); i++) {

		int parent = -1;

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

			if (m_joints[j].m_bone == m_joints[i].m_prevBone) {

				parent = j;

				break;

			}

		if (parent < 0) {

			assert(0);

			break;

		}

		m_engine.addJoint(m_joints[i].m_bone->getCenter(), parent);

		if (m_joints[i].m_bone->getPinnedStatus() != Skeleton::Bone::FREE)

			m_engine.lock(i);

	}

	// add the handle

	bool handleFound = false;

	for (int i = 0; i < (int)m_joints.size(); i++)

		if (m_joints[i].m_bone->getColumnIndex() == handle) {

			m_engine.addJoint(pos, i);

			handleFound = true;

			break;

		}

	if (!handleFound)

		return;

	setAngleOffsets();

	computeIHateIK();

	m_valid = true;

}

//------------------------------------------------------------

// TODO cambiare questo nome; aggiungere due righe di spiegazione