/* SPDX-License-Identifier: GPL-2.0-or-later
 * Copyright 2001-2002 NaN Holding BV. All rights reserved. */

/** \file
 * \ingroup ikplugin
 */

#include <cmath>
#include <cstdlib>
#include <cstring>
#include <vector>

/* iTaSC headers */
#ifdef WITH_IK_ITASC
#  include "Armature.hpp"
#  include "Cache.hpp"
#  include "CopyPose.hpp"
#  include "Distance.hpp"
#  include "MovingFrame.hpp"
#  include "Scene.hpp"
#  include "WDLSSolver.hpp"
#  include "WSDLSSolver.hpp"
#endif

#include "MEM_guardedalloc.h"

#include "BIK_api.h"
#include "BLI_blenlib.h"
#include "BLI_math.h"
#include "BLI_utildefines.h"

#include "BKE_action.h"
#include "BKE_armature.h"
#include "BKE_constraint.h"
#include "BKE_global.h"
#include "DNA_action_types.h"
#include "DNA_armature_types.h"
#include "DNA_constraint_types.h"
#include "DNA_object_types.h"
#include "DNA_scene_types.h"

#include "itasc_plugin.h"

/* default parameters */
static bItasc DefIKParam;

/* in case of animation mode, feedback and timestep is fixed */
// #define ANIM_TIMESTEP   1.0
#define ANIM_FEEDBACK 0.8
// #define ANIM_QMAX       0.52

/* Structure pointed by bPose.ikdata
 * It contains everything needed to simulate the armatures
 * There can be several simulation islands independent to each other */
struct IK_Data {
  struct IK_Scene *first;
};

using Vector3 = float[3];
using Vector4 = float[4];
struct IK_Target;
using ErrorCallback = void (*)(const iTaSC::ConstraintValues *values,
                               uint nvalues,
                               IK_Target *iktarget);

/* one structure for each target in the scene */
struct IK_Target {
  struct Depsgraph *bldepsgraph;
  struct Scene *blscene;
  iTaSC::MovingFrame *target;
  iTaSC::ConstraintSet *constraint;
  struct bConstraint *blenderConstraint;
  struct bPoseChannel *rootChannel;
  Object *owner; /* for auto IK */
  ErrorCallback errorCallback;
  std::string targetName;
  std::string constraintName;
  ushort controlType;
  short channel;      /* index in IK channel array of channel on which this target is defined */
  short ee;           /* end effector number */
  bool simulation;    /* true when simulation mode is used (update feedback) */
  bool eeBlend;       /* end effector affected by enforce blending */
  float eeRest[4][4]; /* end effector initial pose relative to armature */

  IK_Target()
  {
    bldepsgraph = nullptr;
    blscene = nullptr;
    target = nullptr;
    constraint = nullptr;
    blenderConstraint = nullptr;
    rootChannel = nullptr;
    owner = nullptr;
    controlType = 0;
    channel = 0;
    ee = 0;
    eeBlend = true;
    simulation = true;
    targetName.reserve(32);
    constraintName.reserve(32);
  }
  ~IK_Target()
  {
    delete constraint;
    delete target;
  }
};

struct IK_Channel {
  bPoseChannel *pchan; /* channel where we must copy matrix back */
  KDL::Frame frame;    /* frame of the bone relative to object base, not armature base */
  std::string tail;    /* segment name of the joint from which we get the bone tail */
  std::string head;    /* segment name of the joint from which we get the bone head */
  int parent;          /* index in this array of the parent channel */
  short jointType;     /* type of joint, combination of IK_SegmentFlag */
  char ndof;           /* number of joint angles for this channel */
  char jointValid;     /* set to 1 when jointValue has been computed */
  /* for joint constraint */
  Object *owner;        /* for pose and IK param */
  double jointValue[4]; /* computed joint value */

  IK_Channel()
  {
    pchan = nullptr;
    parent = -1;
    jointType = 0;
    ndof = 0;
    jointValid = 0;
    owner = nullptr;
    jointValue[0] = 0.0;
    jointValue[1] = 0.0;
    jointValue[2] = 0.0;
    jointValue[3] = 0.0;
  }
};

struct IK_Scene {
  struct Depsgraph *bldepsgraph;
  struct Scene *blscene;
  IK_Scene *next;
  int numchan;  /* number of channel in pchan */
  int numjoint; /* number of joint in jointArray */
  /* array of bone information, one per channel in the tree */
  IK_Channel *channels;
  iTaSC::Armature *armature;
  iTaSC::Cache *cache;
  iTaSC::Scene *scene;
  iTaSC::MovingFrame *base; /* armature base object */
  KDL::Frame baseFrame;     /* frame of armature base relative to blArmature */
  KDL::JntArray jointArray; /* buffer for storing temporary joint array */
  iTaSC::Solver *solver;
  Object *blArmature;
  float blScale;    /* scale of the Armature object (assume uniform scaling) */
  float blInvScale; /* inverse of Armature object scale */
  struct bConstraint *polarConstraint;
  std::vector<IK_Target *> targets;

  IK_Scene()
  {
    bldepsgraph = nullptr;
    blscene = nullptr;
    next = nullptr;
    channels = nullptr;
    armature = nullptr;
    cache = nullptr;
    scene = nullptr;
    base = nullptr;
    solver = nullptr;
    blScale = blInvScale = 1.0f;
    blArmature = nullptr;
    numchan = 0;
    numjoint = 0;
    polarConstraint = nullptr;
  }

  ~IK_Scene()
  {
    /* delete scene first */
    delete scene;
    for (IK_Target *target : targets) {
      delete target;
    }
    targets.clear();
    delete[] channels;
    delete solver;
    delete armature;
    delete base;
    /* delete cache last */
    delete cache;
  }
};

/* type of IK joint, can be combined to list the joints corresponding to a bone */
enum IK_SegmentFlag {
  IK_XDOF = 1,
  IK_YDOF = 2,
  IK_ZDOF = 4,
  IK_SWING = 8,
  IK_REVOLUTE = 16,
  IK_TRANSY = 32,
};

enum IK_SegmentAxis {
  IK_X = 0,
  IK_Y = 1,
  IK_Z = 2,
  IK_TRANS_X = 3,
  IK_TRANS_Y = 4,
  IK_TRANS_Z = 5,
};

static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con)
{
  bPoseChannel *curchan, *pchan_root = nullptr, *chanlist[256], **oldchan;
  PoseTree *tree;
  PoseTarget *target;
  bKinematicConstraint *data;
  int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount;

  data = (bKinematicConstraint *)con->data;

  /* exclude tip from chain? */
  if (!(data->flag & CONSTRAINT_IK_TIP)) {
    pchan_tip = pchan_tip->parent;
  }

  rootbone = data->rootbone;
  /* Find the chain's root & count the segments needed */
  for (curchan = pchan_tip; curchan; curchan = curchan->parent) {
    pchan_root = curchan;

    if (++segcount > 255) { /* 255 is weak */
      break;
    }

    if (segcount == rootbone) {
      /* reached this end of the chain but if the chain is overlapping with a
       * previous one, we must go back up to the root of the other chain */
      if ((curchan->flag & POSE_CHAIN) && BLI_listbase_is_empty(&curchan->iktree)) {
        rootbone++;
        continue;
      }
      break;
    }

    if (BLI_listbase_is_empty(&curchan->iktree) == false) {
      /* Oh, there is already a chain starting from this channel and our chain is longer.
       * Should handle this by moving the previous chain up to the beginning of our chain
       * For now we just stop here. */
      break;
    }
  }
  if (!segcount) {
    return 0;
  }
  /* we reached a limit and still not the end of a previous chain, quit */
  if ((pchan_root->flag & POSE_CHAIN) && BLI_listbase_is_empty(&pchan_root->iktree)) {
    return 0;
  }

  /* now that we know how many segment we have, set the flag */
  for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone;
       segcount++, curchan = curchan->parent) {
    chanlist[segcount] = curchan;
    curchan->flag |= POSE_CHAIN;
  }

  /* setup the chain data */
  /* create a target */
  target = MEM_cnew<PoseTarget>("posetarget");
  target->con = con;
  /* by construction there can be only one tree per channel
   * and each channel can be part of at most one tree. */
  tree = (PoseTree *)pchan_root->iktree.first;

  if (tree == nullptr) {
    /* make new tree */
    tree = MEM_cnew<PoseTree>("posetree");

    tree->iterations = data->iterations;
    tree->totchannel = segcount;
    tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);

    tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan");
    tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent");
    for (a = 0; a < segcount; a++) {
      tree->pchan[a] = chanlist[segcount - a - 1];
      tree->parent[a] = a - 1;
    }
    target->tip = segcount - 1;

    /* AND! link the tree to the root */
    BLI_addtail(&pchan_root->iktree, tree);
    /* new tree */
    treecount = 1;
  }
  else {
    tree->iterations = MAX2(data->iterations, tree->iterations);
    tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);

    /* Skip common pose channels and add remaining. */
    size = MIN2(segcount, tree->totchannel);
    a = t = 0;
    while (a < size && t < tree->totchannel) {
      /* locate first matching channel */
      for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) {
        /* pass */
      }
      if (t >= tree->totchannel) {
        break;
      }
      for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1];
           a++, t++) {
        /* pass */
      }
    }

    segcount = segcount - a;
    target->tip = tree->totchannel + segcount - 1;

    if (segcount > 0) {
      for (parent = a - 1; parent < tree->totchannel; parent++) {
        if (tree->pchan[parent] == chanlist[segcount - 1]->parent) {
          break;
        }
      }

      /* shouldn't happen, but could with dependency cycles */
      if (parent == tree->totchannel) {
        parent = a - 1;
      }

      /* resize array */
      newsize = tree->totchannel + segcount;
      oldchan = tree->pchan;
      oldparent = tree->parent;

      tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan");
      tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent");
      memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
      memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
      MEM_freeN(oldchan);
      MEM_freeN(oldparent);

      /* add new pose channels at the end, in reverse order */
      for (a = 0; a < segcount; a++) {
        tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
        tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
      }
      tree->parent[tree->totchannel] = parent;

      tree->totchannel = newsize;
    }
    /* reusing tree */
    treecount = 0;
  }

  /* add target to the tree */
  BLI_addtail(&tree->targets, target);
  /* mark root channel having an IK tree */
  pchan_root->flag |= POSE_IKTREE;
  return treecount;
}

static bool is_cartesian_constraint(bConstraint *con)
{
  // bKinematicConstraint* data=(bKinematicConstraint *)con->data;

  return true;
}

static bool constraint_valid(bConstraint *con)
{
  bKinematicConstraint *data = (bKinematicConstraint *)con->data;

  if (data->flag & CONSTRAINT_IK_AUTO) {
    return true;
  }
  if (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) {
    return false;
  }
  if (is_cartesian_constraint(con)) {
    /* cartesian space constraint */
    if (data->tar == nullptr) {
      return false;
    }
    if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) {
      return false;
    }
  }
  return true;
}

static int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
{
  bConstraint *con;
  int treecount;

  /* find all IK constraints and validate them */
  treecount = 0;
  for (con = (bConstraint *)pchan_tip->constraints.first; con; con = (bConstraint *)con->next) {
    if (con->type == CONSTRAINT_TYPE_KINEMATIC) {
      if (constraint_valid(con)) {
        treecount += initialize_chain(ob, pchan_tip, con);
      }
    }
  }
  return treecount;
}

static IK_Data *get_ikdata(bPose *pose)
{
  if (pose->ikdata) {
    return (IK_Data *)pose->ikdata;
  }
  pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata");
  /* here init ikdata if needed
   * now that we have scene, make sure the default param are initialized */
  if (!DefIKParam.iksolver) {
    BKE_pose_itasc_init(&DefIKParam);
  }

  return (IK_Data *)pose->ikdata;
}
static double EulerAngleFromMatrix(const KDL::Rotation &R, int axis)
{
  double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));

  if (t > 16.0 * KDL::epsilon) {
    if (axis == 0) {
      return -KDL::atan2(R(1, 2), R(2, 2));
    }
    if (axis == 1) {
      return KDL::atan2(-R(0, 2), t);
    }

    return -KDL::atan2(R(0, 1), R(0, 0));
  }

  if (axis == 0) {
    return -KDL::atan2(-R(2, 1), R(1, 1));
  }
  if (axis == 1) {
    return KDL::atan2(-R(0, 2), t);
  }

  return 0.0f;
}

static double ComputeTwist(const KDL::Rotation &R)
{
  /* qy and qw are the y and w components of the quaternion from R */
  double qy = R(0, 2) - R(2, 0);
  double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;

  double tau = 2 * KDL::atan2(qy, qw);

  return tau;
}

static void RemoveEulerAngleFromMatrix(KDL::Rotation &R, double angle, int axis)
{
  /* compute twist parameter */
  KDL::Rotation T;
  switch (axis) {
    case 0:
      T = KDL::Rotation::RotX(-angle);
      break;
    case 1:
      T = KDL::Rotation::RotY(-angle);
      break;
    case 2:
      T = KDL::Rotation::RotZ(-angle);
      break;
    default:
      return;
  }
  /* remove angle */
  R = R * T;
}

#if 0
static void GetEulerXZY(const KDL::Rotation &R, double &X, double &Z, double &Y)
{
  if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) {
    X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0));
    Z = -KDL::sign(R(0, 1)) * KDL::PI / 2;
    Y = 0.0;
  }
  else {
    X = KDL::atan2(R(2, 1), R(1, 1));
    Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
    Y = KDL::atan2(R(0, 2), R(0, 0));
  }
}

static void GetEulerXYZ(const KDL::Rotation &R, double &X, double &Y, double &Z)
{
  if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) {
    X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1));
    Y = KDL::sign(R(0, 2)) * KDL::PI / 2;
    Z = 0.0;
  }
  else {
    X = KDL::atan2(-R(1, 2), R(2, 2));
    Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1))));
    Z = KDL::atan2(-R(0, 1), R(0, 0));
  }
}
#endif

static void GetJointRotation(KDL::Rotation &boneRot, int type, double *rot)
{
  switch (type & ~IK_TRANSY) {
    default:
      /* fixed bone, no joint */
      break;
    case IK_XDOF:
      /* RX only, get the X rotation */
      rot[0] = EulerAngleFromMatrix(boneRot, 0);
      break;
    case IK_YDOF:
      /* RY only, get the Y rotation */
      rot[0] = ComputeTwist(boneRot);
      break;
    case IK_ZDOF:
      /* RZ only, get the Z rotation */
      rot[0] = EulerAngleFromMatrix(boneRot, 2);
      break;
    case IK_XDOF | IK_YDOF:
      rot[1] = ComputeTwist(boneRot);
      RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
      rot[0] = EulerAngleFromMatrix(boneRot, 0);
      break;
    case IK_SWING:
      /* RX+RZ */
      boneRot.GetXZRot().GetValue(rot);
      break;
    case IK_YDOF | IK_ZDOF:
      /* RZ+RY */
      rot[1] = ComputeTwist(boneRot);
      RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
      rot[0] = EulerAngleFromMatrix(boneRot, 2);
      break;
    case IK_SWING | IK_YDOF:
      rot[2] = ComputeTwist(boneRot);
      RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
      boneRot.GetXZRot().GetValue(rot);
      break;
    case IK_REVOLUTE:
      boneRot.GetRot().GetValue(rot);
      break;
  }
}

static bool target_callback(const iTaSC::Timestamp &timestamp,
                            const iTaSC::Frame &current,
                            iTaSC::Frame &next,
                            void *param)
{
  IK_Target *target = (IK_Target *)param;
  /* compute next target position
   * get target matrix from constraint. */
  bConstraint *constraint = (bConstraint *)target->blenderConstraint;
  float tarmat[4][4];

  BKE_constraint_target_matrix_get(target->bldepsgraph,
                                   target->blscene,
                                   constraint,
                                   0,
                                   CONSTRAINT_OBTYPE_OBJECT,
                                   target->owner,
                                   tarmat,
                                   1.0);

  /* rootmat contains the target pose in world coordinate
   * if enforce is != 1.0, blend the target position with the end effector position
   * if the armature was in rest position. This information is available in eeRest */
  if (constraint->enforce != 1.0f && target->eeBlend) {
    /* eeRest is relative to the reference frame of the IK root
     * get this frame in world reference */
    float restmat[4][4];
    bPoseChannel *pchan = target->rootChannel;
    if (pchan->parent) {
      pchan = pchan->parent;
      float chanmat[4][4];
      copy_m4_m4(chanmat, pchan->pose_mat);
      copy_v3_v3(chanmat[3], pchan->pose_tail);
      mul_m4_series(restmat, target->owner->obmat, chanmat, target->eeRest);
    }
    else {
      mul_m4_m4m4(restmat, target->owner->obmat, target->eeRest);
    }
    /* blend the target */
    blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce);
  }
  next.setValue(&tarmat[0][0]);
  return true;
}

static bool base_callback(const iTaSC::Timestamp &timestamp,
                          const iTaSC::Frame &current,
                          iTaSC::Frame &next,
                          void *param)
{
  IK_Scene *ikscene = (IK_Scene *)param;
  /* compute next armature base pose
   * algorithm:
   * ikscene->pchan[0] is the root channel of the tree
   * if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail
   * then multiply by the armature matrix to get ikscene->armature base position */
  bPoseChannel *pchan = ikscene->channels[0].pchan;
  float rootmat[4][4];
  if (pchan->parent) {
    pchan = pchan->parent;
    float chanmat[4][4];
    copy_m4_m4(chanmat, pchan->pose_mat);
    copy_v3_v3(chanmat[3], pchan->pose_tail);
    /* save the base as a frame too so that we can compute deformation after simulation */
    ikscene->baseFrame.setValue(&chanmat[0][0]);
    /* iTaSC armature is scaled to object scale, scale the base frame too */
    ikscene->baseFrame.p *= ikscene->blScale;
    mul_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat);
  }
  else {
    copy_m4_m4(rootmat, ikscene->blArmature->obmat);
    ikscene->baseFrame = iTaSC::F_identity;
  }
  next.setValue(&rootmat[0][0]);
  /* If there is a polar target (only during solving otherwise we don't have end effector). */
  if (ikscene->polarConstraint && timestamp.update) {
    /* compute additional rotation of base frame so that armature follows the polar target */
    float imat[4][4];    /* IK tree base inverse matrix */
    float polemat[4][4]; /* polar target in IK tree base frame */
    float goalmat[4][4]; /* target in IK tree base frame */
    float mat[4][4];     /* temp matrix */
    bKinematicConstraint *poledata = (bKinematicConstraint *)ikscene->polarConstraint->data;

    invert_m4_m4(imat, rootmat);
    /* polar constraint imply only one target */
    IK_Target *iktarget = ikscene->targets[0];
    /* root channel from which we take the bone initial orientation */
    IK_Channel &rootchan = ikscene->channels[0];

    /* get polar target matrix in world space */
    BKE_constraint_target_matrix_get(ikscene->bldepsgraph,
                                     ikscene->blscene,
                                     ikscene->polarConstraint,
                                     1,
                                     CONSTRAINT_OBTYPE_OBJECT,
                                     ikscene->blArmature,
                                     mat,
                                     1.0);
    /* convert to armature space */
    mul_m4_m4m4(polemat, imat, mat);
    /* get the target in world space
     * (was computed before as target object are defined before base object). */
    iktarget->target->getPose().getValue(mat[0]);
    /* convert to armature space */
    mul_m4_m4m4(goalmat, imat, mat);
    /* take position of target, polar target, end effector, in armature space */
    KDL::Vector goalpos(goalmat[3]);
    KDL::Vector polepos(polemat[3]);
    KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p;
    /* get root bone orientation */
    KDL::Frame rootframe;
    ikscene->armature->getRelativeFrame(rootframe, rootchan.tail);
    KDL::Vector rootx = rootframe.M.UnitX();
    KDL::Vector rootz = rootframe.M.UnitZ();
    /* and compute root bone head */
    double q_rest[3], q[3], length;
    const KDL::Joint *joint;
    const KDL::Frame *tip;
    ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
    length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
    KDL::Vector rootpos = rootframe.p - length * rootframe.M.UnitY();

    /* compute main directions */
    KDL::Vector dir = KDL::Normalize(endpos - rootpos);
    KDL::Vector poledir = KDL::Normalize(goalpos - rootpos);
    /* compute up directions */
    KDL::Vector poleup = KDL::Normalize(polepos - rootpos);
    KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz * KDL::sin(poledata->poleangle);
    /* from which we build rotation matrix */
    KDL::Rotation endrot, polerot;
    /* for the armature, using the root bone orientation */
    KDL::Vector x = KDL::Normalize(dir * up);
    endrot.UnitX(x);
    endrot.UnitY(KDL::Normalize(x * dir));
    endrot.UnitZ(-dir);
    /* for the polar target */
    x = KDL::Normalize(poledir * poleup);
    polerot.UnitX(x);
    polerot.UnitY(KDL::Normalize(x * poledir));
    polerot.UnitZ(-poledir);
    /* the difference between the two is the rotation we want to apply */
    KDL::Rotation result(polerot * endrot.Inverse());
    /* apply on base frame as this is an artificial additional rotation */
    next.M = next.M * result;
    ikscene->baseFrame.M = ikscene->baseFrame.M * result;
  }
  return true;
}

static bool copypose_callback(const iTaSC::Timestamp &timestamp,
                              iTaSC::ConstraintValues *const _values,
                              uint _nvalues,
                              void *_param)
{
  IK_Target *iktarget = (IK_Target *)_param;
  bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
  iTaSC::ConstraintValues *values = _values;
  bItasc *ikparam = (bItasc *)iktarget->owner->pose->ikparam;

  /* we need default parameters */
  if (!ikparam) {
    ikparam = &DefIKParam;
  }

  if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
    if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
      values->alpha = 0.0;
      values->action = iTaSC::ACT_ALPHA;
      values++;
    }
    if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
      values->alpha = 0.0;
      values->action = iTaSC::ACT_ALPHA;
      values++;
    }
  }
  else {
    if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
      /* update error */
      values->alpha = condata->weight;
      values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
      values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
      values++;
    }
    if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
      /* update error */
      values->alpha = condata->orientweight;
      values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
      values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
      values++;
    }
  }
  return true;
}

static void copypose_error(const iTaSC::ConstraintValues *values,
                           uint nvalues,
                           IK_Target *iktarget)
{
  iTaSC::ConstraintSingleValue *value;
  double error;
  int i;

  if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
    /* update error */
    for (i = 0, error = 0.0, value = values->values; i < values->number; i++, value++) {
      error += KDL::sqr(value->y - value->yd);
    }
    iktarget->blenderConstraint->lin_error = float(KDL::sqrt(error));
    values++;
  }
  if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
    /* update error */
    for (i = 0, error = 0.0, value = values->values; i < values->number; i++, value++) {
      error += KDL::sqr(value->y - value->yd);
    }
    iktarget->blenderConstraint->rot_error = float(KDL::sqrt(error));
    values++;
  }
}

static bool distance_callback(const iTaSC::Timestamp &timestamp,
                              iTaSC::ConstraintValues *const _values,
                              uint _nvalues,
                              void *_param)
{
  IK_Target *iktarget = (IK_Target *)_param;
  bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
  iTaSC::ConstraintValues *values = _values;
  bItasc *ikparam = (bItasc *)iktarget->owner->pose->ikparam;
  /* we need default parameters */
  if (!ikparam) {
    ikparam = &DefIKParam;
  }

  /* update weight according to mode */
  if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
    values->alpha = 0.0;
  }
  else {
    switch (condata->mode) {
      case LIMITDIST_INSIDE:
        values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0;
        break;
      case LIMITDIST_OUTSIDE:
        values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0;
        break;
      default:
        values->alpha = condata->weight;
        break;
    }
    if (!timestamp.substep) {
      /* only update value on first timestep */
      switch (condata->mode) {
        case LIMITDIST_INSIDE:
          values->values[0].yd = condata->dist * 0.95;
          break;
        case LIMITDIST_OUTSIDE:
          values->values[0].yd = condata->dist * 1.05;
          break;
        default:
          values->values[0].yd = condata->dist;
          break;
      }
      values->values[0].action = iTaSC::ACT_VALUE | iTaSC::ACT_FEEDBACK;
      values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
    }
  }
  values->action |= iTaSC::ACT_ALPHA;
  return true;
}

static void distance_error(const iTaSC::ConstraintValues *values,
                           uint _nvalues,
                           IK_Target *iktarget)
{
  iktarget->blenderConstraint->lin_error = float(values->values[0].y - values->values[0].yd);
}

static bool joint_callback(const iTaSC::Timestamp &timestamp,
                           iTaSC::ConstraintValues *const _values,
                           uint _nvalues,
                           void *_param)
{
  IK_Channel *ikchan = (IK_Channel *)_param;
  bItasc *ikparam = (bItasc *)ikchan->owner->pose->ikparam;
  bPoseChannel *chan = ikchan->pchan;
  int dof;

  /* a channel can be split into multiple joints, so we get called multiple
   * times for one channel (this callback is only for 1 joint in the armature)
   * the IK_JointTarget structure is shared between multiple joint constraint
   * and the target joint values is computed only once, remember this in jointValid
   * Don't forget to reset it before each frame */
  if (!ikchan->jointValid) {
    float rmat[3][3];

    if (chan->rotmode > 0) {
      /* Euler rotations (will cause gimbal lock, but this can be alleviated a bit with rotation
       * orders) */
      eulO_to_mat3(rmat, chan->eul, chan->rotmode);
    }
    else if (chan->rotmode == ROT_MODE_AXISANGLE) {
      /* axis-angle - stored in quaternion data,
       * but not really that great for 3D-changing orientations */
      axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]);
    }
    else {
      /* quats are normalized before use to eliminate scaling issues */
      normalize_qt(chan->quat);
      quat_to_mat3(rmat, chan->quat);
    }
    KDL::Rotation jointRot(rmat[0][0],
                           rmat[1][0],
                           rmat[2][0],
                           rmat[0][1],
                           rmat[1][1],
                           rmat[2][1],
                           rmat[0][2],
                           rmat[1][2],
                           rmat[2][2]);
    GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
    ikchan->jointValid = 1;
  }
  /* determine which part of jointValue is used for this joint
   * closely related to the way the joints are defined */
  switch (ikchan->jointType & ~IK_TRANSY) {
    case IK_XDOF:
    case IK_YDOF:
    case IK_ZDOF:
      dof = 0;
      break;
    case IK_XDOF | IK_YDOF:
      /* X + Y */
      dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
      break;
    case IK_SWING:
      /* XZ */
      dof = 0;
      break;
    case IK_YDOF | IK_ZDOF:
      /* Z + Y */
      dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
      break;
    case IK_SWING | IK_YDOF:
      /* XZ + Y */
      dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
      break;
    case IK_REVOLUTE:
      dof = 0;
      break;
    default:
      dof = -1;
      break;
  }
  if (dof >= 0) {
    for (uint i = 0; i < _nvalues; i++, dof++) {
      _values[i].values[0].yd = ikchan->jointValue[dof];
      _values[i].alpha = chan->ikrotweight;
      _values[i].feedback = ikparam->feedback;
    }
  }
  return true;
}

/* build array of joint corresponding to IK chain */
static int convert_channels(struct Depsgraph *depsgraph,
                            IK_Scene *ikscene,
                            PoseTree *tree,
                            float ctime)
{
  IK_Channel *ikchan;
  bPoseChannel *pchan;
  int a, flag, njoint;

  njoint = 0;
  for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; a++, ikchan++) {
    pchan = tree->pchan[a];
    ikchan->pchan = pchan;
    ikchan->parent = (a > 0) ? tree->parent[a] : -1;
    ikchan->owner = ikscene->blArmature;

    /* the constraint and channels must be applied before we build the iTaSC scene,
     * this is because some of the pose data (e.g. pose head) don't have corresponding
     * joint angles and can't be applied to the iTaSC armature dynamically */
    if (!(pchan->flag & POSE_DONE)) {
      BKE_pose_where_is_bone(depsgraph, ikscene->blscene, ikscene->blArmature, pchan, ctime, true);
    }
    /* tell blender that this channel was controlled by IK,
     * it's cleared on each BKE_pose_where_is() */
    pchan->flag |= (POSE_DONE | POSE_CHAIN);

    /* set DoF flag */
    flag = 0;
    if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
        (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.0f ||
         pchan->limitmax[0] > 0.0f)) {
      flag |= IK_XDOF;
    }
    if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
        (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.0f ||
         pchan->limitmax[1] > 0.0f)) {
      flag |= IK_YDOF;
    }
    if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
        (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.0f ||
         pchan->limitmax[2] > 0.0f)) {
      flag |= IK_ZDOF;
    }

    if (tree->stretch && (pchan->ikstretch > 0.0)) {
      flag |= IK_TRANSY;
    }
    /*
     * Logic to create the segments:
     * RX,RY,RZ = rotational joints with no length
     * RY(tip) = rotational joints with a fixed length arm = (0,length,0)
     * TY = translational joint on Y axis
     * F(pos) = fixed joint with an arm at position pos
     * Conversion rule of the above flags:
     * -   ==> F(tip)
     * X   ==> RX(tip)
     * Y   ==> RY(tip)
     * Z   ==> RZ(tip)
     * XY  ==> RX+RY(tip)
     * XZ  ==> RX+RZ(tip)
     * YZ  ==> RZ+RY(tip)
     * XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
     * In case of stretch, tip=(0,0,0) and there is an additional TY joint
     * The frame at last of these joints represents the tail of the bone.
     * The head is computed by a reverse translation on Y axis of the bone length
     * or in case of TY joint, by the frame at previous joint.
     * In case of separation of bones, there is an additional F(head) joint
     *
     * Computing rest pose and length is complicated: the solver works in world space
     * Here is the logic:
     * rest position is computed only from bone->bone_mat.
     * bone length is computed from bone->length multiplied by the scaling factor of
     * the armature. Non-uniform scaling will give bad result!
     */
    switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) {
      default:
        ikchan->jointType = 0;
        ikchan->ndof = 0;
        break;
      case IK_XDOF:
        /* RX only, get the X rotation */
        ikchan->jointType = IK_XDOF;
        ikchan->ndof = 1;
        break;
      case IK_YDOF:
        /* RY only, get the Y rotation */
        ikchan->jointType = IK_YDOF;
        ikchan->ndof = 1;
        break;
      case IK_ZDOF:
        /* RZ only, get the Zz rotation */
        ikchan->jointType = IK_ZDOF;
        ikchan->ndof = 1;
        break;
      case IK_XDOF | IK_YDOF:
        ikchan->jointType = IK_XDOF | IK_YDOF;
        ikchan->ndof = 2;
        break;
      case IK_XDOF | IK_ZDOF:
        /* RX+RZ */
        ikchan->jointType = IK_SWING;
        ikchan->ndof = 2;
        break;
      case IK_YDOF | IK_ZDOF:
        /* RZ+RY */
        ikchan->jointType = IK_ZDOF | IK_YDOF;
        ikchan->ndof = 2;
        break;
      case IK_XDOF | IK_YDOF | IK_ZDOF:
        /* spherical joint */
        if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT)) {
          /* decompose in a Swing+RotY joint */
          ikchan->jointType = IK_SWING | IK_YDOF;
        }
        else {
          ikchan->jointType = IK_REVOLUTE;
        }
        ikchan->ndof = 3;
        break;
    }
    if (flag & IK_TRANSY) {
      ikchan->jointType |= IK_TRANSY;
      ikchan->ndof++;
    }
    njoint += ikchan->ndof;
  }
  /* njoint is the joint coordinate, create the Joint Array */
  ikscene->jointArray.resize(njoint);
  ikscene->numjoint = njoint;
  return njoint;
}

/* compute array of joint value corresponding to current pose */
static void convert_pose(IK_Scene *ikscene)
{
  KDL::Rotation boneRot;
  bPoseChannel *pchan;
  IK_Channel *ikchan;
  Bone *bone;
  float rmat[4][4]; /* rest pose of bone with parent taken into account */
  float bmat[4][4]; /* difference */
  float scale;
  double *rot;
  int a, joint;

  /* assume uniform scaling and take Y scale as general scale for the armature */
  scale = len_v3(ikscene->blArmature->obmat[1]);
  rot = ikscene->jointArray(0);
  for (joint = a = 0, ikchan = ikscene->channels;
       a < ikscene->numchan && joint < ikscene->numjoint;
       a++, ikchan++) {
    pchan = ikchan->pchan;
    bone = pchan->bone;

    if (pchan->parent) {
      unit_m4(bmat);
      mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat);
    }
    else {
      copy_m4_m4(bmat, bone->arm_mat);
    }
    invert_m4_m4(rmat, bmat);
    mul_m4_m4m4(bmat, rmat, pchan->pose_mat);
    normalize_m4(bmat);
    boneRot.setValue(bmat[0]);
    GetJointRotation(boneRot, ikchan->jointType, rot);
    if (ikchan->jointType & IK_TRANSY) {
      /* compute actual length */
      rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
    }
    rot += ikchan->ndof;
    joint += ikchan->ndof;
  }
}

/* compute array of joint value corresponding to current pose */
static void BKE_pose_rest(IK_Scene *ikscene)
{
  bPoseChannel *pchan;
  IK_Channel *ikchan;
  Bone *bone;
  float scale;
  double *rot;
  int a, joint;

  /* assume uniform scaling and take Y scale as general scale for the armature */
  scale = len_v3(ikscene->blArmature->obmat[1]);
  /* rest pose is 0 */
  SetToZero(ikscene->jointArray);
  /* except for transY joints */
  rot = ikscene->jointArray(0);
  for (joint = a = 0, ikchan = ikscene->channels;
       a < ikscene->numchan && joint < ikscene->numjoint;
       a++, ikchan++) {
    pchan = ikchan->pchan;
    bone = pchan->bone;

    if (ikchan->jointType & IK_TRANSY) {
      rot[ikchan->ndof - 1] = bone->length * scale;
    }
    rot += ikchan->ndof;
    joint += ikchan->ndof;
  }
}

static IK_Scene *convert_tree(
    struct Depsgraph *depsgraph, Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime)
{
  PoseTree *tree = (PoseTree *)pchan->iktree.first;
  PoseTarget *target;
  bKinematicConstraint *condata;
  bConstraint *polarcon;
  bItasc *ikparam;
  iTaSC::Armature *arm;
  iTaSC::Scene *scene;
  IK_Scene *ikscene;
  IK_Channel *ikchan;
  KDL::Frame initPose;
  Bone *bone;
  int a, numtarget;
  uint t;
  float length;
  bool ret = true;
  double *rot;
  float start[3];

  if (tree->totchannel == 0) {
    return nullptr;
  }

  ikscene = new IK_Scene;
  ikscene->blscene = blscene;
  ikscene->bldepsgraph = depsgraph;
  arm = new iTaSC::Armature();
  scene = new iTaSC::Scene();
  ikscene->channels = new IK_Channel[tree->totchannel];
  ikscene->numchan = tree->totchannel;
  ikscene->armature = arm;
  ikscene->scene = scene;
  ikparam = (bItasc *)ob->pose->ikparam;

  if (!ikparam) {
    /* you must have our own copy */
    ikparam = &DefIKParam;
  }

  if (ikparam->flag & ITASC_SIMULATION) {
    /* no cache in animation mode */
    ikscene->cache = new iTaSC::Cache();
  }

  switch (ikparam->solver) {
    case ITASC_SOLVER_SDLS:
      ikscene->solver = new iTaSC::WSDLSSolver();
      break;
    case ITASC_SOLVER_DLS:
      ikscene->solver = new iTaSC::WDLSSolver();
      break;
    default:
      delete ikscene;
      return nullptr;
  }
  ikscene->blArmature = ob;
  /* assume uniform scaling and take Y scale as general scale for the armature */
  ikscene->blScale = len_v3(ob->obmat[1]);
  ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale;

  std::string joint;
  std::string root("root");
  std::string parent;
  std::vector<double> weights;
  double weight[3];
  /* build the array of joints corresponding to the IK chain */
  convert_channels(depsgraph, ikscene, tree, ctime);
  /* in Blender, the rest pose is always 0 for joints */
  BKE_pose_rest(ikscene);
  rot = ikscene->jointArray(0);

  for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; a++, ikchan++) {
    pchan = ikchan->pchan;
    bone = pchan->bone;

    KDL::Frame tip(iTaSC::F_identity);
    /* compute the position and rotation of the head from previous segment */
    Vector3 *fl = bone->bone_mat;
    KDL::Rotation brot(
        fl[0][0], fl[1][0], fl[2][0], fl[0][1], fl[1][1], fl[2][1], fl[0][2], fl[1][2], fl[2][2]);
    /* if the bone is disconnected, the head is movable in pose mode
     * take that into account by using pose matrix instead of bone
     * Note that pose is expressed in armature space, convert to previous bone space */
    {
      float R_parmat[3][3];
      float iR_parmat[3][3];
      if (pchan->parent) {
        copy_m3_m4(R_parmat, pchan->parent->pose_mat);
      }
      else {
        unit_m3(R_parmat);
      }
      if (pchan->parent) {
        sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
      }
      else {
        start[0] = start[1] = start[2] = 0.0f;
      }
      invert_m3_m3(iR_parmat, R_parmat);
      normalize_m3(iR_parmat);
      mul_m3_v3(iR_parmat, start);
    }
    KDL::Vector bpos(start[0], start[1], start[2]);
    bpos *= ikscene->blScale;
    KDL::Frame head(brot, bpos);

    /* rest pose length of the bone taking scaling into account */
    length = bone->length * ikscene->blScale;
    parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
    /* first the fixed segment to the bone head */
    if (!(ikchan->pchan->bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) {
      joint = bone->name;
      joint += ":H";
      ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
      parent = joint;
    }
    if (!(ikchan->jointType & IK_TRANSY)) {
      /* fixed length, put it in tip */
      tip.p[1] = length;
    }
    joint = bone->name;
    weight[0] = (1.0 - pchan->stiffness[0]);
    weight[1] = (1.0 - pchan->stiffness[1]);
    weight[2] = (1.0 - pchan->stiffness[2]);
    switch (ikchan->jointType & ~IK_TRANSY) {
      case 0:
        /* fixed bone */
        joint += ":F";
        ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
        break;
      case IK_XDOF:
        /* RX only, get the X rotation */
        joint += ":RX";
        ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
        weights.push_back(weight[0]);
        break;
      case IK_YDOF:
        /* RY only, get the Y rotation */
        joint += ":RY";
        ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
        weights.push_back(weight[1]);
        break;
      case IK_ZDOF:
        /* RZ only, get the Zz rotation */
        joint += ":RZ";
        ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
        weights.push_back(weight[2]);
        break;
      case IK_XDOF | IK_YDOF:
        joint += ":RX";
        ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
        weights.push_back(weight[0]);
        if (ret) {
          parent = joint;
          joint = bone->name;
          joint += ":RY";
          ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
          weights.push_back(weight[1]);
        }
        break;
      case IK_SWING:
        joint += ":SW";
        ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
        weights.push_back(weight[0]);
        weights.push_back(weight[2]);
        break;
      case IK_YDOF | IK_ZDOF:
        /* RZ+RY */
        joint += ":RZ";
        ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
        weights.push_back(weight[2]);
        if (ret) {
          parent = joint;
          joint = bone->name;
          joint += ":RY";
          ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
          weights.push_back(weight[1]);
        }
        break;
      case IK_SWING | IK_YDOF:
        /* decompose in a Swing+RotY joint */
        joint += ":SW";
        ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
        weights.push_back(weight[0]);
        weights.push_back(weight[2]);
        if (ret) {
          parent = joint;
          joint = bone->name;
          joint += ":RY";
          ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
          weights.push_back(weight[1]);
        }
        break;
      case IK_REVOLUTE:
        joint += ":SJ";
        ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
        weights.push_back(weight[0]);
        weights.push_back(weight[1]);
        weights.push_back(weight[2]);
        break;
    }
    if (ret && (ikchan->jointType & IK_TRANSY)) {
      parent = joint;
      joint = bone->name;
      joint += ":TY";
      ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]);
      const float ikstretch = pchan->ikstretch * pchan->ikstretch;
      /* why invert twice here? */
      weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f));
      weights.push_back(weight[1]);
    }
    if (!ret) {
      /* error making the armature?? */
      break;
    }
    /* joint points to the segment that correspond to the bone per say */
    ikchan->tail = joint;
    ikchan->head = parent;
    /* in case of error */
    ret = false;
    if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) {
      joint = bone->name;
      joint += ":RX";
      if (pchan->ikflag & BONE_IK_XLIMIT) {
        if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) {
          break;
        }
      }
      if (pchan->ikflag & BONE_IK_ROTCTL) {
        if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
          break;
        }
      }
    }
    if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) {
      joint = bone->name;
      joint += ":RY";
      if (pchan->ikflag & BONE_IK_YLIMIT) {
        if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0) {
          break;
        }
      }
      if (pchan->ikflag & BONE_IK_ROTCTL) {
        if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
          break;
        }
      }
    }
    if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
      joint = bone->name;
      joint += ":RZ";
      if (pchan->ikflag & BONE_IK_ZLIMIT) {
        if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0) {
          break;
        }
      }
      if (pchan->ikflag & BONE_IK_ROTCTL) {
        if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
          break;
        }
      }
    }
    if ((ikchan->jointType & IK_SWING) &&
        (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
      joint = bone->name;
      joint += ":SW";
      if (pchan->ikflag & BONE_IK_XLIMIT) {
        if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) {
          break;
        }
      }
      if (pchan->ikflag & BONE_IK_ZLIMIT) {
        if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0) {
          break;
        }
      }
      if (pchan->ikflag & BONE_IK_ROTCTL) {
        if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
          break;
        }
      }
    }
    if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) {
      joint = bone->name;
      joint += ":SJ";
      if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
        break;
      }
    }
    /* no error, so restore */
    ret = true;
    rot += ikchan->ndof;
  }
  if (!ret) {
    delete ikscene;
    return nullptr;
  }
  /* for each target, we need to add an end effector in the armature */
  for (numtarget = 0, polarcon = nullptr, ret = true, target = (PoseTarget *)tree->targets.first;
       target;
       target = (PoseTarget *)target->next) {
    condata = (bKinematicConstraint *)target->con->data;
    pchan = tree->pchan[target->tip];

    if (is_cartesian_constraint(target->con)) {
      /* add the end effector */
      IK_Target *iktarget = new IK_Target();
      ikscene->targets.push_back(iktarget);
      iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
      if (iktarget->ee == -1) {
        ret = false;
        break;
      }
      /* initialize all the fields that we can set at this time */
      iktarget->blenderConstraint = target->con;
      iktarget->channel = target->tip;
      iktarget->simulation = (ikparam->flag & ITASC_SIMULATION);
      iktarget->rootChannel = ikscene->channels[0].pchan;
      iktarget->owner = ob;
      iktarget->targetName = pchan->bone->name;
      iktarget->targetName += ":T:";
      iktarget->targetName += target->con->name;
      iktarget->constraintName = pchan->bone->name;
      iktarget->constraintName += ":C:";
      iktarget->constraintName += target->con->name;
      numtarget++;
      if (condata->poletar) {
        /* this constraint has a polar target */
        polarcon = target->con;
      }
    }
  }
  /* deal with polar target if any */
  if (numtarget == 1 && polarcon) {
    ikscene->polarConstraint = polarcon;
  }
  /* we can now add the armature
   * the armature is based on a moving frame.
   * initialize with the correct position in case there is no cache */
  base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
  ikscene->base = new iTaSC::MovingFrame(initPose);
  ikscene->base->setCallback(base_callback, ikscene);
  std::string armname;
  armname = ob->id.name;
  armname += ":B";
  ret = scene->addObject(armname, ikscene->base);
  armname = ob->id.name;
  armname += ":AR";
  if (ret) {
    ret = scene->addObject(armname, ikscene->armature, ikscene->base);
  }
  if (!ret) {
    delete ikscene;
    return nullptr;
  }
  /* set the weight */
  e_matrix &Wq = arm->getWq();
  assert(Wq.cols() == int(weights.size()));
  for (int q = 0; q < Wq.cols(); q++) {
    Wq(q, q) = weights[q];
  }
  /* get the inverse rest pose frame of the base to compute relative rest pose of end effectors
   * this is needed to handle the enforce parameter
   * ikscene->pchan[0] is the root channel of the tree
   * if it has no parent, then it's just the identify Frame */
  float invBaseFrame[4][4];
  pchan = ikscene->channels[0].pchan;
  if (pchan->parent) {
    /* it has a parent, get the pose matrix from it */
    float baseFrame[4][4];
    pchan = pchan->parent;
    copy_m4_m4(baseFrame, pchan->bone->arm_mat);
    /* move to the tail and scale to get rest pose of armature base */
    copy_v3_v3(baseFrame[3], pchan->bone->arm_tail);
    invert_m4_m4(invBaseFrame, baseFrame);
  }
  else {
    unit_m4(invBaseFrame);
  }
  /* finally add the constraint */
  for (t = 0; t < ikscene->targets.size(); t++) {
    IK_Target *iktarget = ikscene->targets[t];
    iktarget->blscene = blscene;
    iktarget->bldepsgraph = depsgraph;
    condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
    pchan = tree->pchan[iktarget->channel];
    uint controltype, bone_count;
    double bone_length;
    float mat[4][4];

    /* add the end effector
     * estimate the average bone length, used to clamp feedback error */
    for (bone_count = 0, bone_length = 0.0f, a = iktarget->channel; a >= 0;
         a = tree->parent[a], bone_count++) {
      bone_length += ikscene->blScale * tree->pchan[a]->bone->length;
    }
    bone_length /= bone_count;

    /* store the rest pose of the end effector to compute enforce target */
    copy_m4_m4(mat, pchan->bone->arm_mat);
    copy_v3_v3(mat[3], pchan->bone->arm_tail);
    /* get the rest pose relative to the armature base */
    mul_m4_m4m4(iktarget->eeRest, invBaseFrame, mat);
    iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ?
                            true :
                            false;
    /* use target_callback to make sure the initPose includes enforce coefficient */
    target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
    iktarget->target = new iTaSC::MovingFrame(initPose);
    iktarget->target->setCallback(target_callback, iktarget);
    ret = scene->addObject(iktarget->targetName, iktarget->target);
    if (!ret) {
      break;
    }

    switch (condata->type) {
      case CONSTRAINT_IK_COPYPOSE:
        controltype = 0;
        if (condata->flag & CONSTRAINT_IK_ROT) {
          if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X)) {
            controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
          }
          if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y)) {
            controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
          }
          if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z)) {
            controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
          }
        }
        if (condata->flag & CONSTRAINT_IK_POS) {
          if (!(condata->flag & CONSTRAINT_IK_NO_POS_X)) {
            controltype |= iTaSC::CopyPose::CTL_POSITIONX;
          }
          if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y)) {
            controltype |= iTaSC::CopyPose::CTL_POSITIONY;
          }
          if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z)) {
            controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
          }
        }
        if (controltype) {
          iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bone_length);
          /* set the gain */
          if (controltype & iTaSC::CopyPose::CTL_POSITION) {
            iktarget->constraint->setControlParameter(
                iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
          }
          if (controltype & iTaSC::CopyPose::CTL_ROTATION) {
            iktarget->constraint->setControlParameter(
                iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
          }
          iktarget->constraint->registerCallback(copypose_callback, iktarget);
          iktarget->errorCallback = copypose_error;
          iktarget->controlType = controltype;
          /* add the constraint */
          if (condata->flag & CONSTRAINT_IK_TARGETAXIS) {
            ret = scene->addConstraintSet(iktarget->constraintName,
                                          iktarget->constraint,
                                          iktarget->targetName,
                                          armname,
                                          "",
                                          ikscene->channels[iktarget->channel].tail);
          }
          else {
            ret = scene->addConstraintSet(iktarget->constraintName,
                                          iktarget->constraint,
                                          armname,
                                          iktarget->targetName,
                                          ikscene->channels[iktarget->channel].tail);
          }
        }
        break;
      case CONSTRAINT_IK_DISTANCE:
        iktarget->constraint = new iTaSC::Distance(bone_length);
        iktarget->constraint->setControlParameter(
            iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
        iktarget->constraint->registerCallback(distance_callback, iktarget);
        iktarget->errorCallback = distance_error;
        /* we can update the weight on each substep */
        iktarget->constraint->substep(true);
        /* add the constraint */
        ret = scene->addConstraintSet(iktarget->constraintName,
                                      iktarget->constraint,
                                      armname,
                                      iktarget->targetName,
                                      ikscene->channels[iktarget->channel].tail);
        break;
    }
    if (!ret) {
      break;
    }
  }
  if (!ret || !scene->addCache(ikscene->cache) || !scene->addSolver(ikscene->solver) ||
      !scene->initialize()) {
    delete ikscene;
    ikscene = nullptr;
  }
  return ikscene;
}

static void create_scene(struct Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime)
{
  bPoseChannel *pchan;

  /* create the IK scene */
  for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan;
       pchan = (bPoseChannel *)pchan->next) {
    /* by construction there is only one tree */
    PoseTree *tree = (PoseTree *)pchan->iktree.first;
    if (tree) {
      IK_Data *ikdata = get_ikdata(ob->pose);
      /* convert tree in iTaSC::Scene */
      IK_Scene *ikscene = convert_tree(depsgraph, scene, ob, pchan, ctime);
      if (ikscene) {
        ikscene->next = ikdata->first;
        ikdata->first = ikscene;
      }
      /* delete the trees once we are done */
      while (tree) {
        BLI_remlink(&pchan->iktree, tree);
        BLI_freelistN(&tree->targets);
        if (tree->pchan) {
          MEM_freeN(tree->pchan);
        }
        if (tree->parent) {
          MEM_freeN(tree->parent);
        }
        if (tree->basis_change) {
          MEM_freeN(tree->basis_change);
        }
        MEM_freeN(tree);
        tree = (PoseTree *)pchan->iktree.first;
      }
    }
  }
}

/* returns 1 if scaling has changed and tree must be reinitialized */
static int init_scene(Object *ob)
{
  /* check also if scaling has changed */
  float scale = len_v3(ob->obmat[1]);
  IK_Scene *scene;

  if (ob->pose->ikdata) {
    for (scene = ((IK_Data *)ob->pose->ikdata)->first; scene != nullptr; scene = scene->next) {
      if (fabs(scene->blScale - scale) > KDL::epsilon) {
        return 1;
      }
      scene->channels[0].pchan->flag |= POSE_IKTREE;
    }
  }
  return 0;
}

static void execute_scene(struct Depsgraph *depsgraph,
                          Scene *blscene,
                          IK_Scene *ikscene,
                          bItasc *ikparam,
                          float ctime,
                          float frtime)
{
  int i;
  IK_Channel *ikchan;
  if (ikparam->flag & ITASC_SIMULATION) {
    for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
      /* In simulation mode we don't allow external constraint to change our bones,
       * mark the channel done also tell Blender that this channel is part of IK tree.
       * Cleared on each BKE_pose_where_is() */
      ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
      ikchan->jointValid = 0;
    }
  }
  else {
    /* in animation mode, we must get the bone position from action and constraints */
    for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
      if (!(ikchan->pchan->flag & POSE_DONE)) {
        BKE_pose_where_is_bone(
            depsgraph, blscene, ikscene->blArmature, ikchan->pchan, ctime, true);
      }
      /* tell blender that this channel was controlled by IK,
       * it's cleared on each BKE_pose_where_is() */
      ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
      ikchan->jointValid = 0;
    }
  }
  /* only run execute the scene if at least one of our target is enabled */
  for (i = ikscene->targets.size(); i > 0; i--) {
    IK_Target *iktarget = ikscene->targets[i - 1];
    if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) {
      break;
    }
  }
  if (i == 0 && ikscene->armature->getNrOfConstraints() == 0) {
    /* all constraint disabled */
    return;
  }

  /* compute timestep */
  double timestamp = ctime * frtime + 2147483.648;
  double timestep = frtime;
  bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false;
  int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep;
  bool simulation = true;

  if (ikparam->flag & ITASC_SIMULATION) {
    ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
  }
  else {
    /* in animation mode we start from the pose after action and constraint */
    convert_pose(ikscene);
    ikscene->armature->setJointArray(ikscene->jointArray);
    /* and we don't handle velocity */
    reiterate = true;
    simulation = false;
    /* time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
     * and choose 1s timestep to allow having velocity parameters in radiant */
    timestep = 1.0;
    /* use auto setup to let the solver test the variation of the joints */
    numstep = 0;
  }

  if (ikscene->cache && !reiterate && simulation) {
    iTaSC::CacheTS sts, cts;
    sts = cts = (iTaSC::CacheTS)std::round(timestamp * 1000.0);
    if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == nullptr || cts == 0) {
      /* the cache is empty before this time, reiterate */
      if (ikparam->flag & ITASC_INITIAL_REITERATION) {
        reiterate = true;
      }
    }
    else {
      /* can take the cache as a start point. */
      sts -= cts;
      timestep = sts / 1000.0;
    }
  }
  /* don't cache if we are reiterating because we don't want to destroy the cache unnecessarily */
  ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
  if (reiterate) {
    /* how many times do we reiterate? */
    for (i = 0; i < ikparam->numiter; i++) {
      if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
          ikscene->armature->getMaxEndEffectorChange() < ikparam->precision) {
        break;
      }
      ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation);
    }
    if (simulation) {
      /* one more fake iteration to cache */
      ikscene->scene->update(timestamp, 0.0, 1, true, true, true);
    }
  }
  /* compute constraint error */
  for (i = ikscene->targets.size(); i > 0; i--) {
    IK_Target *iktarget = ikscene->targets[i - 1];
    if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF) && iktarget->constraint) {
      uint nvalues;
      const iTaSC::ConstraintValues *values;
      values = iktarget->constraint->getControlParameters(&nvalues);
      iktarget->errorCallback(values, nvalues, iktarget);
    }
  }
  /* Apply result to bone:
   * walk the ikscene->channels
   * for each, get the Frame of the joint corresponding to the bone relative to its parent
   * combine the parent and the joint frame to get the frame relative to armature
   * a backward translation of the bone length gives the head
   * if TY, compute the scale as the ratio of the joint length with rest pose length */
  iTaSC::Armature *arm = ikscene->armature;
  KDL::Frame frame;
  double q_rest[3], q[3];
  const KDL::Joint *joint;
  const KDL::Frame *tip;
  bPoseChannel *pchan;
  float scale;
  float length;
  float yaxis[3];
  for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
    if (i == 0) {
      if (!arm->getRelativeFrame(frame, ikchan->tail)) {
        break;
      }
      /* this frame is relative to base, make it relative to object */
      ikchan->frame = ikscene->baseFrame * frame;
    }
    else {
      if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail)) {
        break;
      }
      /* combine with parent frame to get frame relative to object */
      ikchan->frame = ikscene->channels[ikchan->parent].frame * frame;
    }
    /* ikchan->frame is the tail frame relative to object
     * get bone length */
    if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip)) {
      break;
    }
    if (joint->getType() == KDL::Joint::TransY) {
      /* stretch bones have a TY joint, compute the scale */
      scale = float(q[0] / q_rest[0]);
      /* the length is the joint itself */
      length = float(q[0]);
    }
    else {
      scale = 1.0f;
      /* for fixed bone, the length is in the tip (always along Y axis) */
      length = tip->p(1);
    }
    /* ready to compute the pose mat */
    pchan = ikchan->pchan;
    /* tail mat */
    ikchan->frame.getValue(&pchan->pose_mat[0][0]);
    /* the scale of the object was included in the ik scene, take it out now
     * because the pose channels are relative to the object */
    mul_v3_fl(pchan->pose_mat[3], ikscene->blInvScale);
    length *= ikscene->blInvScale;
    copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]);
    /* shift to head */
    copy_v3_v3(yaxis, pchan->pose_mat[1]);
    mul_v3_fl(yaxis, length);
    sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis);
    copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]);
    /* add scale */
    mul_v3_fl(pchan->pose_mat[0], scale);
    mul_v3_fl(pchan->pose_mat[1], scale);
    mul_v3_fl(pchan->pose_mat[2], scale);
  }
  if (i < ikscene->numchan) {
    /* big problem */
  }
}

/* -------------------------------------------------------------------- */
/** \name Plugin Interface
 * \{ */

void itasc_initialize_tree(struct Depsgraph *depsgraph,
                           struct Scene *scene,
                           Object *ob,
                           float ctime)
{
  bPoseChannel *pchan;
  int count = 0;

  if (ob->pose->ikdata != nullptr && !(ob->pose->flag & POSE_WAS_REBUILT)) {
    if (!init_scene(ob)) {
      return;
    }
  }
  /* first remove old scene */
  itasc_clear_data(ob->pose);
  /* we should handle all the constraint and mark them all disabled
   * for blender but we'll start with the IK constraint alone */
  for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan;
       pchan = (bPoseChannel *)pchan->next) {
    if (pchan->constflag & PCHAN_HAS_IK) {
      count += initialize_scene(ob, pchan);
    }
  }
  /* if at least one tree, create the scenes from the PoseTree stored in the channels
   * postpone until execute_tree: this way the pose constraint are included */
  if (count) {
    create_scene(depsgraph, scene, ob, ctime);
  }
  itasc_update_param(ob->pose);
  /* make sure we don't rebuilt until the user changes something important */
  ob->pose->flag &= ~POSE_WAS_REBUILT;
}

void itasc_execute_tree(struct Depsgraph *depsgraph,
                        struct Scene *scene,
                        Object *ob,
                        bPoseChannel *pchan_root,
                        float ctime)
{
  if (ob->pose->ikdata) {
    IK_Data *ikdata = (IK_Data *)ob->pose->ikdata;
    bItasc *ikparam = (bItasc *)ob->pose->ikparam;
    /* we need default parameters */
    if (!ikparam) {
      ikparam = &DefIKParam;
    }

    for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
      if (ikscene->channels[0].pchan == pchan_root) {
        float timestep = scene->r.frs_sec_base / scene->r.frs_sec;
        execute_scene(depsgraph, scene, ikscene, ikparam, ctime, timestep);
        break;
      }
    }
  }
}

void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime)
{
  /* not used for iTaSC */
}

void itasc_clear_data(struct bPose *pose)
{
  if (pose->ikdata) {
    IK_Data *ikdata = (IK_Data *)pose->ikdata;
    for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) {
      ikdata->first = scene->next;
      delete scene;
    }
    MEM_freeN(ikdata);
    pose->ikdata = nullptr;
  }
}

void itasc_clear_cache(struct bPose *pose)
{
  if (pose->ikdata) {
    IK_Data *ikdata = (IK_Data *)pose->ikdata;
    for (IK_Scene *scene = ikdata->first; scene; scene = scene->next) {
      if (scene->cache) {
        /* clear all cache but leaving the timestamp 0 (=rest pose) */
        scene->cache->clearCacheFrom(nullptr, 1);
      }
    }
  }
}

void itasc_update_param(struct bPose *pose)
{
  if (pose->ikdata && pose->ikparam) {
    IK_Data *ikdata = (IK_Data *)pose->ikdata;
    bItasc *ikparam = (bItasc *)pose->ikparam;
    for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
      double armlength = ikscene->armature->getArmLength();
      ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax * armlength);
      ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps * armlength);
      if (ikparam->flag & ITASC_SIMULATION) {
        ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
        ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
        ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
        ikscene->armature->setControlParameter(
            CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback);
      }
      else {
        /* in animation mode timestep is 1s by convention => qmax becomes radiant and feedback
         * becomes fraction of error gap corrected in one iteration. */
        ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0);
        ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0);
        ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52);
        ikscene->armature->setControlParameter(
            CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8);
      }
    }
  }
}

void itasc_test_constraint(struct Object *ob, struct bConstraint *cons)
{
  struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data;

  /* only for IK constraint */
  if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == nullptr) {
    return;
  }

  switch (data->type) {
    case CONSTRAINT_IK_COPYPOSE:
    case CONSTRAINT_IK_DISTANCE:
      /* cartesian space constraint */
      break;
  }
}

/** \} */