/*
 * ***** BEGIN GPL LICENSE BLOCK *****
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
 * All rights reserved.
 *
 * The Original Code is: all of this file.
 *
 * Original author: Benoit Bolsee
 * Contributor(s):
 *
 * ***** END GPL LICENSE BLOCK *****
 */

/** \file blender/ikplugin/intern/itasc_plugin.cpp
 *  \ingroup ikplugin
 */

#include <stdlib.h>
#include <string.h>
#include <vector>

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

#include "MEM_guardedalloc.h"

extern "C" {
#include "BIK_api.h"
#include "BLI_blenlib.h"
#include "BLI_math.h"
#include "BLI_utildefines.h"

#include "BKE_global.h"
#include "BKE_armature.h"
#include "BKE_action.h"
#include "BKE_constraint.h"
#include "DNA_object_types.h"
#include "DNA_action_types.h"
#include "DNA_constraint_types.h"
#include "DNA_armature_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;
};

typedef float Vector3[3];
typedef float Vector4[4];
struct IK_Target;
typedef void (*ErrorCallback)(const iTaSC::ConstraintValues *values, unsigned int 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;
	unsigned short			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 = NULL;
		blscene = NULL;
		target = NULL;
		constraint = NULL;
		blenderConstraint = NULL;
		rootChannel = NULL;
		owner = NULL;
		controlType = 0;
		channel = 0;
		ee = 0;
		eeBlend = true;
		simulation = true;
		targetName.reserve(32);
		constraintName.reserve(32);
	}
	~IK_Target() {
		if (constraint)
			delete constraint;
		if (target)
			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 = NULL;
		parent = -1;
		jointType = 0;
		ndof = 0;
		jointValid = 0;
		owner = NULL;
		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 = NULL;
		blscene = NULL;
		next = NULL;
		channels = NULL;
		armature = NULL;
		cache = NULL;
		scene = NULL;
		base = NULL;
		solver = NULL;
		blScale = blInvScale = 1.0f;
		blArmature = NULL;
		numchan = 0;
		numjoint = 0;
		polarConstraint = NULL;
	}

	~IK_Scene() {
		// delete scene first
		if (scene)
			delete scene;
		for (std::vector<IK_Target *>::iterator it = targets.begin(); it != targets.end(); ++it)
			delete (*it);
		targets.clear();
		if (channels)
			delete[] channels;
		if (solver)
			delete solver;
		if (armature)
			delete armature;
		if (base)
			delete base;
		// delete cache last
		if (cache)
			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 = NULL, *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 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 = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget");
	target->con = con;
	// by contruction 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 == NULL) {
		/* make new tree */
		tree = (PoseTree *)MEM_callocN(sizeof(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++) ;
			if (t >= tree->totchannel)
				break;
			for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ;
		}

		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 == NULL)
			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));
		else if (axis == 1) return KDL::atan2(-R(0, 2), t);
		else return -KDL::atan2(R(0, 1), R(0, 0));
	}
	else {
		if (axis == 0) return -KDL::atan2(-R(2, 1), R(1, 1));
		else if (axis == 1) return KDL::atan2(-R(0, 2), t);
		else 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 efffector)
	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, unsigned int _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, unsigned int 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, unsigned int _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, unsigned int _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, unsigned int _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 splitted 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 gimble 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 (unsigned int 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, 1);
		// 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.f || pchan->limitmax[0] > 0.f))
		{
			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.f || pchan->limitmax[1] > 0.f))
		{
			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.f || pchan->limitmax[2] > 0.f))
		{
			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;
	unsigned int t;
	float length;
	bool ret = true;
	double *rot;
	float start[3];

	if (tree->totchannel == 0)
		return NULL;

	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 NULL;
	}
	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 NULL;
	}
	// for each target, we need to add an end effector in the armature
	for (numtarget = 0, polarcon = NULL, 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 NULL;
	}
	// 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];
		unsigned int controltype, bonecnt;
		double bonelen;
		float mat[4][4];

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

		// 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, bonelen);
					// 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(bonelen);
				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 = NULL;
	}
	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 != NULL;
		     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 contraint 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, 1);
			// 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)(timestamp * 1000.0 + 0.5);
		if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || 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) {
			unsigned int 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
		;
	}
}

//---------------------------------------------------
// 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 != NULL && !(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 = NULL;
	}
}

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(NULL, 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 == NULL)
		return;

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