#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 = asin(cross(a, b) / sqrt(a2 * b2)) * M_180_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() override {

    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 override {

    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 override {

    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 override {

    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

void IKTool::computeIHateIK() {

  std::vector<tstageobject *=""> objs;</tstageobject>

  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;