#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>
#include <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;
	typedef Links::const_iterator LinkIter;
	typedef std::map<int, Links> Nodes;
	typedef Nodes::const_iterator NodeIter;
	typedef std::map<int, int> LeaveTable;
	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;
};

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

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;
	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;
	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::vector<int> done;
	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
void IKTool::computeIHateIK()
{
	std::vector<TStageObject *> objs;
	for (int i = 0; i < m_skeleton->getBoneCount(); i++)
		objs.push_back(m_skeleton->getBone(i)->getStageObject());
	int n = (int)objs.size();
	int frame = TTool::getApplication()->getCurrentFrame()->getFrame();
	m_foot = m_firstFoot = 0;
	m_IHateIK = false;

	int i;
	for (i = 0; i < n && !objs[i]->getPinnedRangeSet()->isPinned(frame); i++) {
	}
	if (i == n)
		return;
	m_foot = objs[i];
	const TPinnedRangeSet::Range *range = m_foot->getPinnedRangeSet()->getRange(frame);
	if (!range || range->first != frame)
		return;

	m_IHateIK = true;
	int firstFrame = frame - 1;
	m_firstFoot = m_foot;
	for (;;) {
		for (i = 0; i < n && !objs[i]->getPinnedRangeSet()->isPinned(firstFrame); i++) {
		}
		if (i == n)
			break;
		m_firstFoot = objs[i];
		range = m_firstFoot->getPinnedRangeSet()->getRange(firstFrame);
		if (!range)
			break;
		firstFrame = range->first - 1;
		if (firstFrame < 0)
			break;
	}
	m_footPlacement = m_foot->getPlacement(frame);
	m_firstFootPlacement = m_firstFoot->getPinnedRangeSet()->getPlacement();
}

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

void IKTool::setAngleOffsets()
{
	int frame = TTool::getApplication()->getCurrentFrame()->getFrame();
	for (int i = 0; i < (int)m_joints.size(); i++) {
		double angle = m_joints[i].m_bone->getStageObject()->getParam(TStageObject::T_Angle, frame);
		double theta0 = TConsts::pi_180 * angle;
		double theta1 = m_joints[i].m_sign * m_engine.getJointAngle(i);
		m_joints[i].m_angleOffset = theta1 - theta0;
	}
}

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

void IKTool::storeOldValues()
{
	for (int i = 0; i < (int)m_joints.size(); i++) {

		TStageObjectValues values(m_joints[i].m_bone->getStageObject()->getId(), TStageObject::T_Angle);
		if (getTool()->isGlobalKeyframesEnabled()) {
			values.add(TStageObject::T_X);
			values.add(TStageObject::T_Y);
			values.add(TStageObject::T_Z);
			values.add(TStageObject::T_SO);
			values.add(TStageObject::T_ScaleX);
			values.add(TStageObject::T_ScaleY);
			values.add(TStageObject::T_Scale);
			values.add(TStageObject::T_Path);
			values.add(TStageObject::T_ShearX);
			values.add(TStageObject::T_ShearY);
		}
		TTool::Application *app = TTool::getApplication();
		values.setFrameHandle(app->getCurrentFrame());
		values.setXsheetHandle(app->getCurrentXsheet());
		values.updateValues();
		m_joints[i].m_oldValues = values;
	}
}
//------------------------------------------------------------

void IKTool::apply()
{
	if (!m_valid)
		return;
	TStageObject *rootObj = m_skeleton->getRootBone()->getStageObject();
	if (!m_undo) {
		m_undo = new IKToolUndo();
		for (int i = 0; i < (int)m_joints.size(); i++)
			m_undo->addNode(m_joints[i].m_bone->getStageObject()->getId());

		if (m_IHateIK && m_firstFoot) {
			m_undo->setFirstFootId(m_firstFoot->getId());
			m_undo->setFirstFootOldPlacement(m_firstFoot->getPinnedRangeSet()->getPlacement());
		}
	}

	int frame = TTool::getApplication()->getCurrentFrame()->getFrame();
	for (int i = 0; i < (int)m_joints.size(); i++) {
		TStageObject *obj = m_joints[i].m_bone->getStageObject();
		TDoubleParam *param = obj->getParam(TStageObject::T_Angle);
		double theta = m_joints[i].m_sign * m_engine.getJointAngle(i) - m_joints[i].m_angleOffset;
		theta *= TConsts::invOf_pi_180;
		double oldTheta = param->getValue(frame);
		double delta = theta - oldTheta;
		if (fabs(delta) > 180)
			theta += (theta < oldTheta ? 1 : -1) * 360;
		param->setValue(frame, theta);
	}
	m_skeleton->getRootBone()->getStageObject()->invalidate();
	if (m_IHateIK) {

		TStageObject *rootObj = m_skeleton->getRootBone()->getStageObject();
		rootObj->setStatus(TStageObject::XY);
		rootObj->invalidate();
		TAffine rootBasePlacement = rootObj->getPlacement(frame);
		rootObj->setStatus(TStageObject::IK);
		rootObj->invalidate();

		TPinnedRangeSet *rangeSet = m_firstFoot->getPinnedRangeSet();

		TAffine oldFootPlacement = m_foot->getPlacement(frame);
		TAffine relativeOldFootPlacement = rootBasePlacement.inv() * oldFootPlacement;
		TAffine correction =
			rootBasePlacement.inv() * m_footPlacement * oldFootPlacement.inv() * rootBasePlacement;

		rangeSet->setPlacement(correction * rangeSet->getPlacement());
		rootObj->invalidate();

		TAffine currentFootPlacement = m_foot->getPlacement(frame);
		TAffine check = m_footPlacement.inv() * currentFootPlacement;

		assert(check.isIdentity(0.01));
	}
}

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

void IKTool::leftButtonDown(const TPointD &p, const TMouseEvent &e)
{
	TPointD pos = m_viewer->winToWorld(e.m_pos);
	m_pos = pos;
	initEngine(pos);
	storeOldValues();
}

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

void IKTool::leftButtonDrag(const TPointD &p, const TMouseEvent &e)
{
	if (!m_valid)
		return;
	if (m_engine.getJointCount() > 0) {
		TPointD tmp = m_viewer->winToWorld(e.m_pos);
		m_engine.drag(tmp);
		apply();
	}
}
//------------------------------------------------------------

void IKTool::leftButtonUp(const TPointD &p, const TMouseEvent &e)
{
	if (m_undo) {
		if (m_IHateIK && m_firstFoot)
			m_undo->setFirstFootNewPlacement(m_firstFoot->getPinnedRangeSet()->getPlacement());
		TUndoManager::manager()->add(m_undo);
		m_undo = 0;
	}

	m_valid = false;
	m_engine.clear();
}

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

void IKTool::draw()
{
	int err = glGetError();
	assert(err == GL_NO_ERROR);
	TTool::Application *app = TTool::getApplication();
	int frame = app->getCurrentFrame()->getFrame();

	double unit = TTool::getApplication()->getCurrentTool()->getTool()->getPixelSize();

	if (m_engine.getJointCount() > 0) {
		glColor3d(1, 0, 1);
		for (int i = 0; i < m_engine.getJointCount(); i++) {
			TPointD pa = m_engine.getJoint(i);
			tglDrawDisk(pa, 5 * unit);
			if (i > 0) {
				int j = m_engine.getJointParent(i);
				tglDrawSegment(pa, m_engine.getJoint(j));
			}
		}
	}
}

//============================================================
//
// ChangeDrawingTool
//
//------------------------------------------------------------

class ChangeDrawingUndo : public TUndo
{
	int m_row, m_col;
	TFrameId m_oldFid, m_newFid;

public:
	ChangeDrawingUndo(int row, int col)
		: m_row(row), m_col(col)
	{
		m_oldFid = getDrawing();
	}

	TFrameId getDrawing() const
	{
		TTool::Application *app = TTool::getApplication();
		TXsheet *xsh = app->getCurrentScene()->getScene()->getXsheet();
		TXshCell cell = xsh->getCell(m_row, m_col);
		return cell.m_frameId;
	}

	void setDrawing(const TFrameId &fid) const
	{
		TTool::Application *app = TTool::getApplication();
		TXsheet *xsh = app->getCurrentScene()->getScene()->getXsheet();
		TXshCell cell = xsh->getCell(m_row, m_col);
		cell.m_frameId = fid;
		xsh->setCell(m_row, m_col, cell);
		TStageObject *pegbar = xsh->getStageObject(TStageObjectId::ColumnId(m_col));
		pegbar->setOffset(pegbar->getOffset());
		// solo per invalidare l'imp. TODO da fare meglio
		// l'idea e' che cambiando drawing corrente, se ci sono hook la posizione
		// della pegbar (o delle pegbar figlie) puo' cambiare
		app->getCurrentXsheet()->notifyXsheetChanged();
	}

	void onAdd()
	{
		m_newFid = getDrawing();
	}
	void undo() const { setDrawing(m_oldFid); }
	void redo() const { setDrawing(m_newFid); }
	int getSize() const { return sizeof(*this); }
	TFrameId getOldDrawing() const { return m_oldFid; }
};

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

ChangeDrawingTool::ChangeDrawingTool(SkeletonTool *tool, int dir)
	: DragTool(tool), m_oldY(0), m_dir(dir), m_undo(0)
{
}

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

void ChangeDrawingTool::leftButtonDown(const TPointD &, const TMouseEvent &e)
{
	m_oldY = e.m_pos.y;
	TTool::Application *app = TTool::getApplication();
	int row = app->getCurrentFrame()->getFrame();
	int col = app->getCurrentColumn()->getColumnIndex();
	m_undo = new ChangeDrawingUndo(row, col);
	if (m_dir > 0)
		changeDrawing(1);
	else if (m_dir < 0)
		changeDrawing(-1);
}

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

void ChangeDrawingTool::leftButtonDrag(const TPointD &, const TMouseEvent &e)
{
	if (m_dir != 0)
		return;
	int delta = e.m_pos.y - m_oldY;
	int h = 5;
	int increment = delta > 0 ? delta / h : -(-delta) / h;
	if (increment == 0)
		return;
	changeDrawing(-increment);
	m_oldY += h * increment;
}

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

void ChangeDrawingTool::leftButtonUp(const TPointD &, const TMouseEvent &e)
{
	ChangeDrawingUndo *u = dynamic_cast<ChangeDrawingUndo *>(m_undo);
	if (u) {
		if (u->getOldDrawing() != u->getDrawing())
			TUndoManager::manager()->add(u);
		else
			delete u;
		m_undo = 0;
	}
}

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

// cambia il frame della cella corrente, scorrendo (in maniera
// ciclica) i drawings del livello
// ritorna true se ci riesce
bool ChangeDrawingTool::changeDrawing(int delta)
{
	TTool::Application *app = TTool::getApplication();
	TXsheet *xsh = app->getCurrentScene()->getScene()->getXsheet();
	int row = app->getCurrentFrame()->getFrame();
	int col = app->getCurrentColumn()->getColumnIndex();
	TXshCell cell = xsh->getCell(row, col);
	if (!cell.m_level || !cell.m_level->getSimpleLevel())
		return false;
	std::vector<TFrameId> fids;
	cell.m_level->getSimpleLevel()->getFids(fids);
	int n = fids.size();
	if (n < 2)
		return false;
	std::vector<TFrameId>::iterator it;
	it = std::find(fids.begin(), fids.end(), cell.m_frameId);
	if (it == fids.end())
		return false;
	int index = std::distance(fids.begin(), it);
	while (delta < 0)
		delta += n;
	index = (index + delta) % n;

	ChangeDrawingUndo *u = dynamic_cast<ChangeDrawingUndo *>(m_undo);
	if (u)
		u->setDrawing(fids[index]);
	return true;
}

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

CommandHandler::CommandHandler()
	: m_skeleton(0), m_tempPinnedSet(0)
{
}

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

CommandHandler::~CommandHandler()
{
	delete m_skeleton;
}

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

void CommandHandler::setSkeleton(Skeleton *skeleton)
{
	if (m_skeleton != skeleton) {
		delete m_skeleton;
		m_skeleton = skeleton;
	}
}

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

void CommandHandler::clearPinnedRanges()
{
	if (m_skeleton) {
		TTool::Application *app = TTool::getApplication();
		m_skeleton->clearAllPinnedRanges();
		app->getCurrentScene()->setDirtyFlag(true);
		app->getCurrentXsheet()->notifyXsheetChanged();
		m_skeleton->getRootBone()->getStageObject()->setStatus(TStageObject::XY);
		delete m_skeleton;
		m_skeleton = 0;
	}
	if (m_tempPinnedSet)
		m_tempPinnedSet->clear();
}