editarmature_retarget.c

Go to the documentation of this file.

/*
 * ***** 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.
 *
 * Contributor(s): Martin Poirier
 *
 * ***** END GPL LICENSE BLOCK *****
 * autoarmature.c: Interface for automagically manipulating armature (retarget, created, ...)
 */

#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <math.h> 
#include <float.h>

#include "MEM_guardedalloc.h"

#include "PIL_time.h"

#include "DNA_armature_types.h"
#include "DNA_constraint_types.h"
#include "DNA_scene_types.h"
#include "DNA_object_types.h"

#include "BLI_blenlib.h"
#include "BLI_math.h"
#include "BLI_editVert.h"
#include "BLI_utildefines.h"
#include "BLI_ghash.h"
#include "BLI_graph.h"
#include "BLI_rand.h"
#include "BLI_threads.h"

//#include "BDR_editobject.h"

#include "BKE_constraint.h"
#include "BKE_armature.h"
#include "BKE_context.h"

#include "ED_armature.h"
#include "ED_util.h"

#include "BIF_retarget.h"


//#include "mydevice.h"
#include "reeb.h" // FIX ME
//#include "blendef.h"

#include "armature_intern.h"

/************ RIG RETARGET DATA STRUCTURES ***************/

typedef struct MemoNode {
    float   weight;
    int     next;
} MemoNode;

typedef struct RetargetParam {
    RigGraph    *rigg;
    RigArc      *iarc;
    RigNode     *inode_start;
    bContext    *context;
} RetargetParam;

typedef enum 
{
    RETARGET_LENGTH,
    RETARGET_AGGRESSIVE
} RetargetMode; 

typedef enum
{
    METHOD_BRUTE_FORCE = 0,
    METHOD_MEMOIZE = 1
} RetargetMethod;

typedef enum
{
    ARC_FREE = 0,
    ARC_TAKEN = 1,
    ARC_USED = 2
} ArcUsageFlags;

static RigGraph *GLOBAL_RIGG = NULL;

/*******************************************************************************************************/

void *exec_retargetArctoArc(void *param);

static void RIG_calculateEdgeAngles(RigEdge *edge_first, RigEdge *edge_second);
float rollBoneByQuat(EditBone *bone, float old_up_axis[3], float qrot[4]);

/* two levels */
#define SHAPE_LEVELS (SHAPE_RADIX * SHAPE_RADIX) 

/*********************************** EDITBONE UTILS ****************************************************/

static int countEditBoneChildren(ListBase *list, EditBone *parent)
{
    EditBone *ebone;
    int count = 0;
    
    for (ebone = list->first; ebone; ebone = ebone->next)
    {
        if (ebone->parent == parent)
        {
            count++;
        }
    }
    
    return count;
}

static EditBone* nextEditBoneChild(ListBase *list, EditBone *parent, int n)
{
    EditBone *ebone;
    
    for (ebone = list->first; ebone; ebone = ebone->next)
    {
        if (ebone->parent == parent)
        {
            if (n == 0)
            {
                return ebone;
            }
            n--;
        }
    }
    
    return NULL;
}

static void getEditBoneRollUpAxis(EditBone *bone, float roll, float up_axis[3])
{
    float mat[3][3], nor[3];

    sub_v3_v3v3(nor, bone->tail, bone->head);
    
    vec_roll_to_mat3(nor, roll, mat);
    copy_v3_v3(up_axis, mat[2]);
}

static float rollBoneByQuatAligned(EditBone *bone, float old_up_axis[3], float qrot[4], float qroll[4], float aligned_axis[3])
{
    float nor[3], new_up_axis[3], x_axis[3], z_axis[3];
    
    copy_v3_v3(new_up_axis, old_up_axis);
    mul_qt_v3(qrot, new_up_axis);
    
    sub_v3_v3v3(nor, bone->tail, bone->head);
    
    cross_v3_v3v3(x_axis, nor, aligned_axis);
    cross_v3_v3v3(z_axis, x_axis, nor);
    
    normalize_v3(new_up_axis);
    normalize_v3(x_axis);
    normalize_v3(z_axis);
    
    if (dot_v3v3(new_up_axis, x_axis) < 0)
    {
        negate_v3(x_axis);
    }
    
    if (dot_v3v3(new_up_axis, z_axis) < 0)
    {
        negate_v3(z_axis);
    }
    
    if (angle_normalized_v3v3(x_axis, new_up_axis) < angle_normalized_v3v3(z_axis, new_up_axis))
    {
        rotation_between_vecs_to_quat(qroll, new_up_axis, x_axis); /* set roll rotation quat */
        return ED_rollBoneToVector(bone, x_axis, FALSE);
    }
    else
    {
        rotation_between_vecs_to_quat(qroll, new_up_axis, z_axis); /* set roll rotation quat */
        return ED_rollBoneToVector(bone, z_axis, FALSE);
    }
}

static float rollBoneByQuatJoint(RigEdge *edge, RigEdge *previous, float qrot[4], float qroll[4], float up_axis[3])
{
    if (previous == NULL)
    {
        /* default to up_axis if no previous */
        return rollBoneByQuatAligned(edge->bone, edge->up_axis, qrot, qroll, up_axis);
    }
    else
    {
        float new_up_axis[3];
        float vec_first[3], vec_second[3], normal[3];
        
        if (previous->bone)
        {
            sub_v3_v3v3(vec_first, previous->bone->tail, previous->bone->head);
        } 
        else if (previous->prev->bone)
        {
            sub_v3_v3v3(vec_first, edge->bone->head, previous->prev->bone->tail);
        }
        else
        {
            /* default to up_axis if first bone in the chain is an offset */
            return rollBoneByQuatAligned(edge->bone, edge->up_axis, qrot, qroll, up_axis);
        }
        
        sub_v3_v3v3(vec_second, edge->bone->tail, edge->bone->head);
    
        normalize_v3(vec_first);
        normalize_v3(vec_second);
        
        cross_v3_v3v3(normal, vec_first, vec_second);
        normalize_v3(normal);
        
        axis_angle_to_quat(qroll, vec_second, edge->up_angle);
        
        mul_qt_v3(qroll, normal);
            
        copy_v3_v3(new_up_axis, edge->up_axis);
        mul_qt_v3(qrot, new_up_axis);
        
        normalize_v3(new_up_axis);
        
        /* real qroll between normal and up_axis */
        rotation_between_vecs_to_quat(qroll, new_up_axis, normal);

        return ED_rollBoneToVector(edge->bone, normal, FALSE);
    }
}

float rollBoneByQuat(EditBone *bone, float old_up_axis[3], float qrot[4])
{
    float new_up_axis[3];
    
    copy_v3_v3(new_up_axis, old_up_axis);
    mul_qt_v3(qrot, new_up_axis);
    
    return ED_rollBoneToVector(bone, new_up_axis, FALSE);
}

/************************************ DESTRUCTORS ******************************************************/

static void RIG_freeRigArc(BArc *arc)
{
    BLI_freelistN(&((RigArc*)arc)->edges);
}

void RIG_freeRigGraph(BGraph *rg)
{
    RigGraph *rigg = (RigGraph*)rg;
    BNode *node;
    BArc *arc;
    
#ifdef USE_THREADS
    BLI_destroy_worker(rigg->worker);
#endif
    
    if (rigg->link_mesh)
    {
        REEB_freeGraph(rigg->link_mesh);
    }
    
    for (arc = rg->arcs.first; arc; arc = arc->next)
    {
        RIG_freeRigArc(arc);
    }
    BLI_freelistN(&rg->arcs);
    
    for (node = rg->nodes.first; node; node = node->next)
    {
        BLI_freeNode(rg, (BNode*)node);
    }
    BLI_freelistN(&rg->nodes);
    
    BLI_freelistN(&rigg->controls);

    BLI_ghash_free(rigg->bones_map, NULL, NULL);
    BLI_ghash_free(rigg->controls_map, NULL, NULL);
    
    if (rigg->flag & RIG_FREE_BONELIST)
    {
        BLI_freelistN(rigg->editbones);
        MEM_freeN(rigg->editbones);
    }
    
    MEM_freeN(rg);
}

/************************************* ALLOCATORS ******************************************************/

static RigGraph *newRigGraph(void)
{
    RigGraph *rg;
    int totthread;
    
    rg = MEM_callocN(sizeof(RigGraph), "rig graph");
    
    rg->head = NULL;
    
    rg->bones_map = BLI_ghash_new(BLI_ghashutil_strhash, BLI_ghashutil_strcmp, "newRigGraph bones gh");
    rg->controls_map = BLI_ghash_new(BLI_ghashutil_strhash, BLI_ghashutil_strcmp, "newRigGraph cont gh");
    
    rg->free_arc = RIG_freeRigArc;
    rg->free_node = NULL;
    
#ifdef USE_THREADS
//  if(G.scene->r.mode & R_FIXED_THREADS)
//  {
//      totthread = G.scene->r.threads;
//  }
//  else
//  {
        totthread = BLI_system_thread_count();
//  }
    
    rg->worker = BLI_create_worker(exec_retargetArctoArc, totthread, 20); /* fix number of threads */
#endif
    
    return rg;
}

static RigArc *newRigArc(RigGraph *rg)
{
    RigArc *arc;
    
    arc = MEM_callocN(sizeof(RigArc), "rig arc");
    arc->count = 0;
    BLI_addtail(&rg->arcs, arc);
    
    return arc;
}

static RigControl *newRigControl(RigGraph *rg)
{
    RigControl *ctrl;
    
    ctrl = MEM_callocN(sizeof(RigControl), "rig control");
    
    BLI_addtail(&rg->controls, ctrl);
    
    return ctrl;
}

static RigNode *newRigNodeHead(RigGraph *rg, RigArc *arc, float p[3])
{
    RigNode *node;
    node = MEM_callocN(sizeof(RigNode), "rig node");
    BLI_addtail(&rg->nodes, node);

    copy_v3_v3(node->p, p);
    node->degree = 1;
    node->arcs = NULL;
    
    arc->head = node;
    
    return node;
}

static void addRigNodeHead(RigGraph *UNUSED(rg), RigArc *arc, RigNode *node)
{
    node->degree++;

    arc->head = node;
}

static RigNode *newRigNode(RigGraph *rg, float p[3])
{
    RigNode *node;
    node = MEM_callocN(sizeof(RigNode), "rig node");
    BLI_addtail(&rg->nodes, node);

    copy_v3_v3(node->p, p);
    node->degree = 0;
    node->arcs = NULL;
    
    return node;
}

static RigNode *newRigNodeTail(RigGraph *rg, RigArc *arc, float p[3])
{
    RigNode *node = newRigNode(rg, p);
    
    node->degree = 1;
    arc->tail = node;

    return node;
}

static void RIG_appendEdgeToArc(RigArc *arc, RigEdge *edge)
{
    BLI_addtail(&arc->edges, edge);

    if (edge->prev == NULL)
    {
        copy_v3_v3(edge->head, arc->head->p);
    }
    else
    {
        RigEdge *last_edge = edge->prev;
        copy_v3_v3(edge->head, last_edge->tail);
        RIG_calculateEdgeAngles(last_edge, edge);
    }
    
    edge->length = len_v3v3(edge->head, edge->tail);
    
    arc->length += edge->length;
    
    arc->count += 1;
}

static void RIG_addEdgeToArc(RigArc *arc, float tail[3], EditBone *bone)
{
    RigEdge *edge;

    edge = MEM_callocN(sizeof(RigEdge), "rig edge");

    copy_v3_v3(edge->tail, tail);
    edge->bone = bone;
    
    if (bone)
    {
        getEditBoneRollUpAxis(bone, bone->roll, edge->up_axis);
    }
    
    RIG_appendEdgeToArc(arc, edge);
}
/************************************** CLONING TEMPLATES **********************************************/

static void renameTemplateBone(char *name, char *template_name, ListBase *editbones, char *side_string, char *num_string)
{
    int i, j;
    
    for (i = 0, j = 0; template_name[i] != '\0' && i < (MAXBONENAME-1) && j < (MAXBONENAME-1); i++)
    {
        if (template_name[i] == '&')
        {
            if (template_name[i+1] == 'S' || template_name[i+1] == 's')
            {
                j += sprintf(name + j, "%s", side_string);
                i++;
            }
            else if (template_name[i+1] == 'N' || template_name[i+1] == 'n')
            {
                j += sprintf(name + j, "%s", num_string);
                i++;
            }
            else
            {
                name[j] = template_name[i];
                j++;
            }
        }
        else
        {
            name[j] = template_name[i];
            j++;
        }
    }
    
    name[j] = '\0';
    
    unique_editbone_name(editbones, name, NULL);
}

static RigControl *cloneControl(RigGraph *rg, RigGraph *src_rg, RigControl *src_ctrl, GHash *ptr_hash, char *side_string, char *num_string)
{
    RigControl *ctrl;
    char name[MAXBONENAME];
    
    ctrl = newRigControl(rg);
    
    copy_v3_v3(ctrl->head, src_ctrl->head);
    copy_v3_v3(ctrl->tail, src_ctrl->tail);
    copy_v3_v3(ctrl->up_axis, src_ctrl->up_axis);
    copy_v3_v3(ctrl->offset, src_ctrl->offset);
    
    ctrl->tail_mode = src_ctrl->tail_mode;
    ctrl->flag = src_ctrl->flag;

    renameTemplateBone(name, src_ctrl->bone->name, rg->editbones, side_string, num_string);
    ctrl->bone = duplicateEditBoneObjects(src_ctrl->bone, name, rg->editbones, src_rg->ob, rg->ob);
    ctrl->bone->flag &= ~(BONE_TIPSEL|BONE_SELECTED|BONE_ROOTSEL);
    BLI_ghash_insert(ptr_hash, src_ctrl->bone, ctrl->bone);
    
    ctrl->link = src_ctrl->link;
    ctrl->link_tail = src_ctrl->link_tail;
    
    return ctrl;
}

static RigArc *cloneArc(RigGraph *rg, RigGraph *src_rg, RigArc *src_arc, GHash *ptr_hash, char *side_string, char *num_string)
{
    RigEdge *src_edge;
    RigArc  *arc;
    
    arc = newRigArc(rg);
    
    arc->head = BLI_ghash_lookup(ptr_hash, src_arc->head);
    arc->tail = BLI_ghash_lookup(ptr_hash, src_arc->tail);
    
    arc->head->degree++;
    arc->tail->degree++;
    
    arc->length = src_arc->length;

    arc->count = src_arc->count;
    
    for (src_edge = src_arc->edges.first; src_edge; src_edge = src_edge->next)
    {
        RigEdge *edge;
    
        edge = MEM_callocN(sizeof(RigEdge), "rig edge");

        copy_v3_v3(edge->head, src_edge->head);
        copy_v3_v3(edge->tail, src_edge->tail);
        copy_v3_v3(edge->up_axis, src_edge->up_axis);
        
        edge->length = src_edge->length;
        edge->angle = src_edge->angle;
        edge->up_angle = src_edge->up_angle;
        
        if (src_edge->bone != NULL)
        {
            char name[MAXBONENAME];
            renameTemplateBone(name, src_edge->bone->name, rg->editbones, side_string, num_string);
            edge->bone = duplicateEditBoneObjects(src_edge->bone, name, rg->editbones, src_rg->ob, rg->ob);
            edge->bone->flag &= ~(BONE_TIPSEL|BONE_SELECTED|BONE_ROOTSEL);
            BLI_ghash_insert(ptr_hash, src_edge->bone, edge->bone);
        }

        BLI_addtail(&arc->edges, edge);
    }
    
    return arc;
}

static RigGraph *cloneRigGraph(RigGraph *src, ListBase *editbones, Object *ob, char *side_string, char *num_string)
{
    GHash   *ptr_hash;  
    RigNode *node;
    RigArc  *arc;
    RigControl *ctrl;
    RigGraph *rg;
    
    ptr_hash = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp, "cloneRigGraph gh");

    rg = newRigGraph();
    
    rg->ob = ob;
    rg->editbones = editbones;
    
    preEditBoneDuplicate(rg->editbones); /* prime bones for duplication */
    preEditBoneDuplicate(src->editbones); /* prime bones for duplication */
    
    /* Clone nodes */
    for (node = src->nodes.first; node; node = node->next)
    {
        RigNode *cloned_node = newRigNode(rg, node->p);
        BLI_ghash_insert(ptr_hash, node, cloned_node);
    }
    
    rg->head = BLI_ghash_lookup(ptr_hash, src->head);
    
    /* Clone arcs */
    for (arc = src->arcs.first; arc; arc = arc->next)
    {
        cloneArc(rg, src, arc, ptr_hash, side_string, num_string);
    }
    
    /* Clone controls */
    for (ctrl = src->controls.first; ctrl; ctrl = ctrl->next)
    {
        cloneControl(rg, src, ctrl, ptr_hash, side_string, num_string);
    }
    
    /* Relink bones properly */
    for (arc = rg->arcs.first; arc; arc = arc->next)
    {
        RigEdge *edge;
        
        for (edge = arc->edges.first; edge; edge = edge->next)
        {
            if (edge->bone != NULL)
            {
                EditBone *bone;
                
                updateDuplicateSubtargetObjects(edge->bone, src->editbones, src->ob, rg->ob);

                if (edge->bone->parent)
                {               
                    bone = BLI_ghash_lookup(ptr_hash, edge->bone->parent);
        
                    if (bone != NULL)
                    {
                        edge->bone->parent = bone;
                    }
                    else
                    {
                        /* disconnect since parent isn't cloned
                         * this will only happen when cloning from selected bones 
                         * */
                        edge->bone->flag &= ~BONE_CONNECTED;
                    }
                }
            }
        }
    }
    
    for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
    {
        EditBone *bone;
        
        updateDuplicateSubtargetObjects(ctrl->bone, src->editbones, src->ob, rg->ob);

        if (ctrl->bone->parent)
        {
            bone = BLI_ghash_lookup(ptr_hash, ctrl->bone->parent);
            
            if (bone != NULL)
            {
                ctrl->bone->parent = bone;
            }
            else
            {
                /* disconnect since parent isn't cloned
                 * this will only happen when cloning from selected bones 
                 * */
                ctrl->bone->flag &= ~BONE_CONNECTED;
            }
        }

        ctrl->link = BLI_ghash_lookup(ptr_hash, ctrl->link);
        ctrl->link_tail = BLI_ghash_lookup(ptr_hash, ctrl->link_tail);
    }
    
    BLI_ghash_free(ptr_hash, NULL, NULL);
    
    return rg;
}


/*******************************************************************************************************/

static void RIG_calculateEdgeAngles(RigEdge *edge_first, RigEdge *edge_second)
{
    float vec_first[3], vec_second[3];
    
    sub_v3_v3v3(vec_first, edge_first->tail, edge_first->head); 
    sub_v3_v3v3(vec_second, edge_second->tail, edge_second->head);

    normalize_v3(vec_first);
    normalize_v3(vec_second);
    
    edge_first->angle = angle_normalized_v3v3(vec_first, vec_second);
    
    if (edge_second->bone != NULL)
    {
        float normal[3];

        cross_v3_v3v3(normal, vec_first, vec_second);
        normalize_v3(normal);

        edge_second->up_angle = angle_normalized_v3v3(normal, edge_second->up_axis);
    }
}

/************************************ CONTROL BONES ****************************************************/

static void RIG_addControlBone(RigGraph *rg, EditBone *bone)
{
    RigControl *ctrl = newRigControl(rg);
    ctrl->bone = bone;
    copy_v3_v3(ctrl->head, bone->head);
    copy_v3_v3(ctrl->tail, bone->tail);
    getEditBoneRollUpAxis(bone, bone->roll, ctrl->up_axis);
    ctrl->tail_mode = TL_NONE;
    
    BLI_ghash_insert(rg->controls_map, bone->name, ctrl);
}

static int RIG_parentControl(RigControl *ctrl, EditBone *link)
{
    if (link)
    {
        float offset[3];
        int flag = 0;
        
        sub_v3_v3v3(offset, ctrl->bone->head, link->head);

        /* if root matches, check for direction too */      
        if (dot_v3v3(offset, offset) < 0.0001f)
        {
            float vbone[3], vparent[3];
            
            flag |= RIG_CTRL_FIT_ROOT;
            
            sub_v3_v3v3(vbone, ctrl->bone->tail, ctrl->bone->head);
            sub_v3_v3v3(vparent, link->tail, link->head);
            
            /* test for opposite direction */
            if (dot_v3v3(vbone, vparent) > 0)
            {
                float nor[3];
                float len;
                
                cross_v3_v3v3(nor, vbone, vparent);
                
                len = dot_v3v3(nor, nor);
                if (len < 0.0001f)
                {
                    flag |= RIG_CTRL_FIT_BONE;
                }
            }
        }
        
        /* Bail out if old one is automatically better */
        if (flag < ctrl->flag)
        {
            return 0;
        }
        
        /* if there's already a link
         *  overwrite only if new link is higher in the chain */
        if (ctrl->link && flag == ctrl->flag)
        {
            EditBone *bone = NULL;
            
            for (bone = ctrl->link; bone; bone = bone->parent)
            {
                /* if link is in the chain, break and use that one */
                if (bone == link)
                {
                    break;
                }
            }
            
            /* not in chain, don't update link */
            if (bone == NULL)
            {
                return 0;
            }
        }
        
        
        ctrl->link = link;
        ctrl->flag = flag;
        
        copy_v3_v3(ctrl->offset, offset);
        
        return 1;
    }
    
    return 0;
}

static void RIG_reconnectControlBones(RigGraph *rg)
{
    RigControl *ctrl;
    int change = 1;
    
    /* first pass, link to deform bones */
    for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
    {
        bPoseChannel *pchan;
        bConstraint *con;
        int found = 0;
        
        /* DO SOME MAGIC HERE */
        for (pchan= rg->ob->pose->chanbase.first; pchan; pchan= pchan->next)
        {
            for (con= pchan->constraints.first; con; con= con->next) 
            {
                bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
                ListBase targets = {NULL, NULL};
                bConstraintTarget *ct;
                
                /* constraint targets */
                if (cti && cti->get_constraint_targets)
                {
                    int target_index;
                    
                    cti->get_constraint_targets(con, &targets);
                    
                    for (target_index = 0, ct= targets.first; ct; target_index++, ct= ct->next)
                    {
                        if ((ct->tar == rg->ob) && strcmp(ct->subtarget, ctrl->bone->name) == 0)
                        {
                            /* SET bone link to bone corresponding to pchan */
                            EditBone *link = BLI_ghash_lookup(rg->bones_map, pchan->name);
                            
                            /* Making sure bone is in this armature */
                            if (link != NULL)
                            {
                                /* for pole targets, link to parent bone instead, if possible */
                                if (con->type == CONSTRAINT_TYPE_KINEMATIC && target_index == 1)
                                {
                                    if (link->parent && BLI_ghash_haskey(rg->bones_map, link->parent->name))
                                    {
                                        link = link->parent;
                                    }
                                }
                                
                                found = RIG_parentControl(ctrl, link);
                            }
                        }
                    }
                    
                    if (cti->flush_constraint_targets)
                        cti->flush_constraint_targets(con, &targets, 0);
                }
            }
        }

        /* if not found yet, check parent */
        if (found == 0)
        {       
            if (ctrl->bone->parent)
            {
                /* make sure parent is a deforming bone
                 * NULL if not
                 *  */
                EditBone *link = BLI_ghash_lookup(rg->bones_map, ctrl->bone->parent->name);
                
                found = RIG_parentControl(ctrl, link);
            }
            
            /* check if bone is not superposed on another one */
            {
                RigArc *arc;
                RigArc *best_arc = NULL;
                EditBone *link = NULL;
                
                for (arc = rg->arcs.first; arc; arc = arc->next)
                {
                    RigEdge *edge;
                    for (edge = arc->edges.first; edge; edge = edge->next)
                    {
                        if (edge->bone)
                        {
                            int fit = 0;
                            
                            fit = len_v3v3(ctrl->bone->head, edge->bone->head) < 0.0001f;
                            fit = fit || len_v3v3(ctrl->bone->tail, edge->bone->tail) < 0.0001f;
                            
                            if (fit)
                            {
                                /* pick the bone on the arc with the lowest symmetry level
                                 * means you connect control to the trunk of the skeleton */
                                if (best_arc == NULL || arc->symmetry_level < best_arc->symmetry_level)
                                {
                                    best_arc = arc;
                                    link = edge->bone;
                                }
                            }
                        }
                    }
                }
                
                found = RIG_parentControl(ctrl, link);
            }
        }
        
        /* if not found yet, check child */     
        if (found == 0)
        {
            RigArc *arc;
            RigArc *best_arc = NULL;
            EditBone *link = NULL;
            
            for (arc = rg->arcs.first; arc; arc = arc->next)
            {
                RigEdge *edge;
                for (edge = arc->edges.first; edge; edge = edge->next)
                {
                    if (edge->bone && edge->bone->parent == ctrl->bone)
                    {
                        /* pick the bone on the arc with the lowest symmetry level
                         * means you connect control to the trunk of the skeleton */
                        if (best_arc == NULL || arc->symmetry_level < best_arc->symmetry_level)
                        {
                            best_arc = arc;
                            link = edge->bone;
                        }
                    }
                }
            }
            
            found = RIG_parentControl(ctrl, link);
        }

    }
    
    
    /* second pass, make chains in control bones */
    while (change)
    {
        change = 0;
        
        for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
        {
            /* if control is not linked yet */
            if (ctrl->link == NULL)
            {
                bPoseChannel *pchan;
                bConstraint *con;
                RigControl *ctrl_parent = NULL;
                RigControl *ctrl_child;
                int found = 0;

                if (ctrl->bone->parent)
                {
                    ctrl_parent = BLI_ghash_lookup(rg->controls_map, ctrl->bone->parent->name);
                }

                /* check constraints first */
                
                /* DO SOME MAGIC HERE */
                for (pchan= rg->ob->pose->chanbase.first; pchan; pchan= pchan->next)
                {
                    for (con= pchan->constraints.first; con; con= con->next) 
                    {
                        bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
                        ListBase targets = {NULL, NULL};
                        bConstraintTarget *ct;
                        
                        /* constraint targets */
                        if (cti && cti->get_constraint_targets)
                        {
                            cti->get_constraint_targets(con, &targets);
                            
                            for (ct= targets.first; ct; ct= ct->next)
                            {
                                if ((ct->tar == rg->ob) && strcmp(ct->subtarget, ctrl->bone->name) == 0)
                                {
                                    /* SET bone link to ctrl corresponding to pchan */
                                    RigControl *link = BLI_ghash_lookup(rg->controls_map, pchan->name);

                                    /* if owner is a control bone, link with it */                                  
                                    if (link && link->link)
                                    {
                                        RIG_parentControl(ctrl, link->bone);
                                        found = 1;
                                        break;
                                    }
                                }
                            }
                            
                            if (cti->flush_constraint_targets)
                                cti->flush_constraint_targets(con, &targets, 0);
                        }
                    }
                }           

                if (found == 0)
                {
                    /* check if parent is already linked */
                    if (ctrl_parent && ctrl_parent->link)
                    {
                        RIG_parentControl(ctrl, ctrl_parent->bone);
                        change = 1;
                    }
                    else
                    {
                        /* check childs */
                        for (ctrl_child = rg->controls.first; ctrl_child; ctrl_child = ctrl_child->next)
                        {
                            /* if a child is linked, link to that one */
                            if (ctrl_child->link && ctrl_child->bone->parent == ctrl->bone)
                            {
                                RIG_parentControl(ctrl, ctrl_child->bone);
                                change = 1;
                                break;
                            }
                        }
                    }
                }
            }
        }
    }
    
    /* third pass, link control tails */
    for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
    {
        /* fit bone already means full match, so skip those */
        if ((ctrl->flag & RIG_CTRL_FIT_BONE) == 0)
        {
            GHashIterator ghi;
            
            /* look on deform bones first */
            BLI_ghashIterator_init(&ghi, rg->bones_map);
            
            for( ; !BLI_ghashIterator_isDone(&ghi); BLI_ghashIterator_step(&ghi))
            {
                EditBone *bone = (EditBone*)BLI_ghashIterator_getValue(&ghi);
                
                /* don't link with parent */
                if (bone->parent != ctrl->bone)
                {
                    if (len_v3v3(ctrl->bone->tail, bone->head) < 0.01f)
                    {
                        ctrl->tail_mode = TL_HEAD;
                        ctrl->link_tail = bone;
                        break;
                    }
                    else if (len_v3v3(ctrl->bone->tail, bone->tail) < 0.01f)
                    {
                        ctrl->tail_mode = TL_TAIL;
                        ctrl->link_tail = bone;
                        break;
                    }
                }
            }
            
            /* if we haven't found one yet, look in control bones */
            if (ctrl->tail_mode == TL_NONE)
            {
            }
        }
    }
    
}

/*******************************************************************************************************/

static void RIG_joinArcs(RigGraph *rg, RigNode *node, RigArc *joined_arc1, RigArc *joined_arc2)
{
    RigEdge *edge, *next_edge;
    
    /* ignore cases where joint is at start or end */
    if (joined_arc1->head == joined_arc2->head || joined_arc1->tail == joined_arc2->tail)
    {
        return;
    }
    
    /* swap arcs to make sure arc1 is before arc2 */
    if (joined_arc1->head == joined_arc2->tail)
    {
        RigArc *tmp = joined_arc1;
        joined_arc1 = joined_arc2;
        joined_arc2 = tmp;
    }
    
    for (edge = joined_arc2->edges.first; edge; edge = next_edge)
    {
        next_edge = edge->next;
        
        RIG_appendEdgeToArc(joined_arc1, edge);
    }
    
    joined_arc1->tail = joined_arc2->tail;
    
    joined_arc2->edges.first = joined_arc2->edges.last = NULL;
    
    BLI_removeArc((BGraph*)rg, (BArc*)joined_arc2);
    
    BLI_removeNode((BGraph*)rg, (BNode*)node);
}

static void RIG_removeNormalNodes(RigGraph *rg)
{
    RigNode *node, *next_node;
    
    for (node = rg->nodes.first; node; node = next_node)
    {
        next_node = node->next;
        
        if (node->degree == 2)
        {
            RigArc *arc, *joined_arc1 = NULL, *joined_arc2 = NULL;
            
            for (arc = rg->arcs.first; arc; arc = arc->next)
            {
                if (arc->head == node || arc->tail == node)
                {
                    if (joined_arc1 == NULL)
                    {
                        joined_arc1 = arc;
                    }
                    else
                    {
                        joined_arc2 = arc;
                        break;
                    }
                }
            }
            
            RIG_joinArcs(rg, node, joined_arc1, joined_arc2);
        }
    }
}

static void RIG_removeUneededOffsets(RigGraph *rg)
{
    RigArc *arc;
    
    for (arc = rg->arcs.first; arc; arc = arc->next)
    {
        RigEdge *first_edge, *last_edge;
        
        first_edge = arc->edges.first;
        last_edge = arc->edges.last;
        
        if (first_edge->bone == NULL)
        {
            if (first_edge->bone == NULL && len_v3v3(first_edge->tail, arc->head->p) <= 0.001f)
            {
                BLI_remlink(&arc->edges, first_edge);
                MEM_freeN(first_edge);
            }
            else if (arc->head->degree == 1)
            {
                RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, first_edge->tail, 0.001f);
                
                if (new_node)
                {
                    BLI_remlink(&arc->edges, first_edge);
                    MEM_freeN(first_edge);
                    BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->head);
                }
                else
                {
                    RigEdge *next_edge = first_edge->next;
    
                    if (next_edge)
                    {
                        BLI_remlink(&arc->edges, first_edge);
                        MEM_freeN(first_edge);
                        
                        copy_v3_v3(arc->head->p, next_edge->head);
                    }
                }
            }
            else
            {
                /* check if all arc connected start with a null edge */
                RigArc *other_arc;
                for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
                {
                    if (other_arc != arc)
                    {
                        RigEdge *test_edge;
                        if (other_arc->head == arc->head)
                        {
                            test_edge = other_arc->edges.first;
                            
                            if (test_edge->bone != NULL)
                            {
                                break;
                            }
                        }
                        else if (other_arc->tail == arc->head)
                        {
                            test_edge = other_arc->edges.last;
                            
                            if (test_edge->bone != NULL)
                            {
                                break;
                            }
                        }
                    }
                }
                
                if (other_arc == NULL)
                {
                    RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, first_edge->tail, 0.001);
                    
                    if (new_node)
                    {
                        /* remove null edge in other arcs too */
                        for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
                        {
                            if (other_arc != arc)
                            {
                                RigEdge *test_edge;
                                if (other_arc->head == arc->head)
                                {
                                    BLI_replaceNodeInArc((BGraph*)rg, (BArc*)other_arc, (BNode*)new_node, (BNode*)other_arc->head);
                                    test_edge = other_arc->edges.first;
                                    BLI_remlink(&other_arc->edges, test_edge);
                                    MEM_freeN(test_edge);
                                }
                                else if (other_arc->tail == arc->head)
                                {
                                    BLI_replaceNodeInArc((BGraph*)rg, (BArc*)other_arc, (BNode*)new_node, (BNode*)other_arc->tail);
                                    test_edge = other_arc->edges.last;
                                    BLI_remlink(&other_arc->edges, test_edge);
                                    MEM_freeN(test_edge);
                                }
                            }
                        }
                        
                        BLI_remlink(&arc->edges, first_edge);
                        MEM_freeN(first_edge);
                        BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->head);
                    }
                    else
                    {
                        RigEdge *next_edge = first_edge->next;
        
                        if (next_edge)
                        {
                            BLI_remlink(&arc->edges, first_edge);
                            MEM_freeN(first_edge);
                            
                            copy_v3_v3(arc->head->p, next_edge->head);
                            
                            /* remove null edge in other arcs too */
                            for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
                            {
                                if (other_arc != arc)
                                {
                                    RigEdge *test_edge;
                                    if (other_arc->head == arc->head)
                                    {
                                        test_edge = other_arc->edges.first;
                                        BLI_remlink(&other_arc->edges, test_edge);
                                        MEM_freeN(test_edge);
                                    }
                                    else if (other_arc->tail == arc->head)
                                    {
                                        test_edge = other_arc->edges.last;
                                        BLI_remlink(&other_arc->edges, test_edge);
                                        MEM_freeN(test_edge);
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
        
        if (last_edge->bone == NULL)
        {
            if (len_v3v3(last_edge->head, arc->tail->p) <= 0.001f)
            {
                BLI_remlink(&arc->edges, last_edge);
                MEM_freeN(last_edge);
            }
            else if (arc->tail->degree == 1)
            {
                RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, last_edge->head, 0.001f);
                
                if (new_node)
                {
                    RigEdge *previous_edge = last_edge->prev;
                    
                    BLI_remlink(&arc->edges, last_edge);
                    MEM_freeN(last_edge);
                    BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->tail);
                    
                    /* set previous angle to 0, since there's no following edges */
                    if (previous_edge)
                    {
                        previous_edge->angle = 0;
                    }
                }
                else
                {
                    RigEdge *previous_edge = last_edge->prev;
    
                    if (previous_edge)
                    {
                        BLI_remlink(&arc->edges, last_edge);
                        MEM_freeN(last_edge);
                        
                        copy_v3_v3(arc->tail->p, previous_edge->tail);
                        previous_edge->angle = 0;
                    }
                }
            }
        }
    }
}

static void RIG_arcFromBoneChain(RigGraph *rg, ListBase *list, EditBone *root_bone, RigNode *starting_node, int selected)
{
    EditBone *bone, *last_bone = root_bone;
    RigArc *arc = NULL;
    int contain_head = 0;
    
    for(bone = root_bone; bone; bone = nextEditBoneChild(list, bone, 0))
    {
        int nb_children;
        
        if (selected == 0 || (bone->flag & BONE_SELECTED))
        { 
            if ((bone->flag & BONE_NO_DEFORM) == 0)
            {
                BLI_ghash_insert(rg->bones_map, bone->name, bone);
            
                if (arc == NULL)
                {
                    arc = newRigArc(rg);
                    
                    if (starting_node == NULL)
                    {
                        starting_node = newRigNodeHead(rg, arc, root_bone->head);
                    }
                    else
                    {
                        addRigNodeHead(rg, arc, starting_node);
                    }
                }
                
                if (bone->parent && (bone->flag & BONE_CONNECTED) == 0)
                {
                    RIG_addEdgeToArc(arc, bone->head, NULL);
                }
                
                RIG_addEdgeToArc(arc, bone->tail, bone);
                
                last_bone = bone;
                
                if (strcmp(bone->name, "head") == 0)
                {
                    contain_head = 1;
                }
            }
            else if ((bone->flag & BONE_EDITMODE_LOCKED) == 0) /* ignore locked bones */
            {
                RIG_addControlBone(rg, bone);
            }
        }
        
        nb_children = countEditBoneChildren(list, bone);
        if (nb_children > 1)
        {
            RigNode *end_node = NULL;
            int i;
            
            if (arc != NULL)
            {
                end_node = newRigNodeTail(rg, arc, bone->tail);
            }
            else
            {
                end_node = newRigNode(rg, bone->tail);
            }

            for (i = 0; i < nb_children; i++)
            {
                root_bone = nextEditBoneChild(list, bone, i);
                RIG_arcFromBoneChain(rg, list, root_bone, end_node, selected);
            }
            
            /* arc ends here, break */
            break;
        }
    }
    
    /* If the loop exited without forking */
    if (arc != NULL && bone == NULL)
    {
        newRigNodeTail(rg, arc, last_bone->tail);
    }

    if (contain_head)
    {
        rg->head = arc->tail;
    }
}

/*******************************************************************************************************/
static void RIG_findHead(RigGraph *rg)
{
    if (rg->head == NULL)
    {
        if (BLI_countlist(&rg->arcs) == 1)
        {
            RigArc *arc = rg->arcs.first;
            
            rg->head = (RigNode*)arc->head;
        }
        else
        {
            RigArc *arc;
            
            for (arc = rg->arcs.first; arc; arc = arc->next)
            {
                RigEdge *edge = arc->edges.last;
                
                if (edge->bone->flag & (BONE_TIPSEL|BONE_SELECTED))
                {
                    rg->head = arc->tail;
                    break;
                }
            }
        }
        
        if (rg->head == NULL)
        {
            rg->head = rg->nodes.first;
        }
    }
}

/*******************************************************************************************************/

static void RIG_printNode(RigNode *node, const char name[])
{
    printf("%s %p %i <%0.3f, %0.3f, %0.3f>\n", name, (void *)node, node->degree, node->p[0], node->p[1], node->p[2]);
    
    if (node->symmetry_flag & SYM_TOPOLOGICAL)
    {
        if (node->symmetry_flag & SYM_AXIAL)
            printf("Symmetry AXIAL\n");
        else if (node->symmetry_flag & SYM_RADIAL)
            printf("Symmetry RADIAL\n");
            
        print_v3("symmetry axis", node->symmetry_axis);
    }
}

void RIG_printArcBones(RigArc *arc)
{
    RigEdge *edge;

    for (edge = arc->edges.first; edge; edge = edge->next)
    {
        if (edge->bone)
            printf("%s ", edge->bone->name);
        else
            printf("---- ");
    }
    printf("\n");
}

static void RIG_printCtrl(RigControl *ctrl, char *indent)
{
    char text[128];
    
    printf("%sBone: %s\n", indent, ctrl->bone->name);
    printf("%sLink: %s\n", indent, ctrl->link ? ctrl->link->name : "!NONE!");
    
    BLI_snprintf(text, sizeof(text), "%soffset", indent);
    print_v3(text, ctrl->offset);
    
    printf("%sFlag: %i\n", indent, ctrl->flag);
}

static void RIG_printLinkedCtrl(RigGraph *rg, EditBone *bone, int tabs)
{
    RigControl *ctrl;
    char indent[64];
    char *s = indent;
    int i;
    
    for (i = 0; i < tabs; i++)
    {
        s[0] = '\t';
        s++;
    }
    s[0] = 0;
    
    for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
    {
        if (ctrl->link == bone)
        {
            RIG_printCtrl(ctrl, indent);
            RIG_printLinkedCtrl(rg, ctrl->bone, tabs + 1);
        }
    }
}

void RIG_printArc(RigGraph *rg, RigArc *arc)
{
    RigEdge *edge;

    RIG_printNode((RigNode*)arc->head, "head");

    for (edge = arc->edges.first; edge; edge = edge->next)
    {
        printf("\tinner joints %0.3f %0.3f %0.3f\n", edge->tail[0], edge->tail[1], edge->tail[2]);
        printf("\t\tlength %f\n", edge->length);
        printf("\t\tangle %f\n", edge->angle * 180 / M_PI);
        if (edge->bone)
        {
            printf("\t\t%s\n", edge->bone->name);
            RIG_printLinkedCtrl(rg, edge->bone, 3);
        }
    }   
    printf("symmetry level: %i flag: %i group %i\n", arc->symmetry_level, arc->symmetry_flag, arc->symmetry_group);

    RIG_printNode((RigNode*)arc->tail, "tail");
}

void RIG_printGraph(RigGraph *rg)
{
    RigArc *arc;

    printf("---- ARCS ----\n");
    for (arc = rg->arcs.first; arc; arc = arc->next)
    {
        RIG_printArc(rg, arc);  
        printf("\n");
    }

    if (rg->head)
    {
        RIG_printNode(rg->head, "HEAD NODE:");
    }
    else
    {
        printf("HEAD NODE: NONE\n");
    }   
}

/*******************************************************************************************************/

RigGraph *RIG_graphFromArmature(const bContext *C, Object *ob, bArmature *arm)
{
    Object *obedit = CTX_data_edit_object(C);
    Scene *scene = CTX_data_scene(C);
    EditBone *ebone;
    RigGraph *rg;

    rg = newRigGraph();
    
    if (obedit == ob)
    {
        rg->editbones = ((bArmature *)obedit->data)->edbo;
    }
    else
    {
        rg->editbones = MEM_callocN(sizeof(ListBase), "EditBones");
        make_boneList(rg->editbones, &arm->bonebase, NULL, NULL);
        rg->flag |= RIG_FREE_BONELIST;
    }
    
    rg->ob = ob;

    /* Do the rotations */
    for (ebone = rg->editbones->first; ebone; ebone=ebone->next){
        if (ebone->parent == NULL)
        {
            RIG_arcFromBoneChain(rg, rg->editbones, ebone, NULL, 0);
        }
    }
    
    BLI_removeDoubleNodes((BGraph*)rg, 0.001);
    
    RIG_removeNormalNodes(rg);
    
    RIG_removeUneededOffsets(rg);
    
    BLI_buildAdjacencyList((BGraph*)rg);
    
    RIG_findHead(rg);

    BLI_markdownSymmetry((BGraph*)rg, (BNode*)rg->head, scene->toolsettings->skgen_symmetry_limit);
    
    RIG_reconnectControlBones(rg); /* after symmetry, because we use levels to find best match */
    
    if (BLI_isGraphCyclic((BGraph*)rg))
    {
        printf("armature cyclic\n");
    }
    
    return rg;
}

static RigGraph *armatureSelectedToGraph(bContext *C, Object *ob, bArmature *arm)
{
    Object *obedit = CTX_data_edit_object(C);
    Scene *scene = CTX_data_scene(C);
    EditBone *ebone;
    RigGraph *rg;

    rg = newRigGraph();
    
    if (obedit == ob)
    {
        rg->editbones = arm->edbo;
    }
    else
    {
        rg->editbones = MEM_callocN(sizeof(ListBase), "EditBones");
        make_boneList(rg->editbones, &arm->bonebase, NULL, NULL);
        rg->flag |= RIG_FREE_BONELIST;
    }

    rg->ob = ob;

    /* Do the rotations */
    for (ebone = rg->editbones->first; ebone; ebone=ebone->next){
        if (ebone->parent == NULL)
        {
            RIG_arcFromBoneChain(rg, rg->editbones, ebone, NULL, 1);
        }
    }
    
    BLI_removeDoubleNodes((BGraph*)rg, 0.001);
    
    RIG_removeNormalNodes(rg);
    
    RIG_removeUneededOffsets(rg);
    
    BLI_buildAdjacencyList((BGraph*)rg);
    
    RIG_findHead(rg);

    BLI_markdownSymmetry((BGraph*)rg, (BNode*)rg->head, scene->toolsettings->skgen_symmetry_limit);
    
    RIG_reconnectControlBones(rg); /* after symmetry, because we use levels to find best match */
    
    if (BLI_isGraphCyclic((BGraph*)rg))
    {
        printf("armature cyclic\n");
    }
    
    return rg;
}
/************************************ GENERATING *****************************************************/

#if 0
static EditBone *add_editbonetolist(char *name, ListBase *list)
{
    EditBone *bone= MEM_callocN(sizeof(EditBone), "eBone");
    
    BLI_strncpy(bone->name, name, sizeof(bone->name));
    unique_editbone_name(list, bone->name, NULL);
    
    BLI_addtail(list, bone);
    
    bone->flag |= BONE_TIPSEL;
    bone->weight= 1.0F;
    bone->dist= 0.25F;
    bone->xwidth= 0.1;
    bone->zwidth= 0.1;
    bone->ease1= 1.0;
    bone->ease2= 1.0;
    bone->rad_head= 0.10;
    bone->rad_tail= 0.05;
    bone->segments= 1;
    bone->layer=  1;//arm->layer;
    
    return bone;
}
#endif

#if 0 /* UNUSED */
static void generateMissingArcsFromNode(RigGraph *rigg, ReebNode *node, int multi_level_limit)
{
    while (node->multi_level > multi_level_limit && node->link_up)
    {
        node = node->link_up;
    }
    
    while (node->multi_level < multi_level_limit && node->link_down)
    {
        node = node->link_down;
    }
    
    if (node->multi_level == multi_level_limit)
    {
        int i;
        
        for (i = 0; i < node->degree; i++)
        {
            ReebArc *earc = node->arcs[i];
            
            if (earc->flag == ARC_FREE && earc->head == node)
            {
                ReebNode *other = BIF_otherNodeFromIndex(earc, node);
                
                earc->flag = ARC_USED;
                
                //generateBonesForArc(rigg, earc, node, other);
                generateMissingArcsFromNode(rigg, other, multi_level_limit);
            }
        }
    }
}

static void generateMissingArcs(RigGraph *rigg)
{
    ReebGraph *reebg;
    int multi_level_limit = 5;
    
    for (reebg = rigg->link_mesh; reebg; reebg = reebg->link_up)
    {
        ReebArc *earc;
        
        for (earc = reebg->arcs.first; earc; earc = earc->next)
        {
            if (earc->flag == ARC_USED)
            {
                generateMissingArcsFromNode(rigg, earc->head, multi_level_limit);
                generateMissingArcsFromNode(rigg, earc->tail, multi_level_limit);
            }
        }
    }
}
#endif

/************************************ RETARGETTING *****************************************************/

static void repositionControl(RigGraph *rigg, RigControl *ctrl, float head[3], float tail[3], float qrot[4], float resize);

static void repositionTailControl(RigGraph *rigg, RigControl *ctrl);

static void finalizeControl(RigGraph *rigg, RigControl *ctrl, float resize)
{
    if ((ctrl->flag & RIG_CTRL_DONE) == RIG_CTRL_DONE)
    {
        RigControl *ctrl_child;

#if 0       
        printf("CTRL: %s LINK: %s", ctrl->bone->name, ctrl->link->name);
        
        if (ctrl->link_tail)
        {
            printf(" TAIL: %s", ctrl->link_tail->name);
        }
        
        printf("\n");
#endif
        
        /* if there was a tail link: apply link, recalc resize factor and qrot */
        if (ctrl->tail_mode != TL_NONE)
        {
            float *tail_vec = NULL;
            float v1[3], v2[3], qtail[4];
            
            if (ctrl->tail_mode == TL_TAIL)
            {
                tail_vec = ctrl->link_tail->tail;
            }
            else if (ctrl->tail_mode == TL_HEAD)
            {
                tail_vec = ctrl->link_tail->head;
            }
            
            sub_v3_v3v3(v1, ctrl->bone->tail, ctrl->bone->head);
            sub_v3_v3v3(v2, tail_vec, ctrl->bone->head);
            
            copy_v3_v3(ctrl->bone->tail, tail_vec);
            
            rotation_between_vecs_to_quat(qtail, v1, v2);
            mul_qt_qtqt(ctrl->qrot, qtail, ctrl->qrot);
            
            resize = len_v3(v2) / len_v3v3(ctrl->head, ctrl->tail);
        }
        
        ctrl->bone->roll = rollBoneByQuat(ctrl->bone, ctrl->up_axis, ctrl->qrot);
    
        /* Cascade to connected control bones */
        for (ctrl_child = rigg->controls.first; ctrl_child; ctrl_child = ctrl_child->next)
        {
            if (ctrl_child->link == ctrl->bone)
            {
                repositionControl(rigg, ctrl_child, ctrl->bone->head, ctrl->bone->tail, ctrl->qrot, resize);
            }
            if (ctrl_child->link_tail == ctrl->bone)
            {
                repositionTailControl(rigg, ctrl_child);
            }
        }
    }   
}

static void repositionTailControl(RigGraph *rigg, RigControl *ctrl)
{
    ctrl->flag |= RIG_CTRL_TAIL_DONE;

    finalizeControl(rigg, ctrl, 1); /* resize will be recalculated anyway so we don't need it */
}

static void repositionControl(RigGraph *rigg, RigControl *ctrl, float head[3], float UNUSED(tail[3]), float qrot[4], float resize)
{
    float parent_offset[3], tail_offset[3];
    
    copy_v3_v3(parent_offset, ctrl->offset);
    mul_v3_fl(parent_offset, resize);
    mul_qt_v3(qrot, parent_offset);
    
    add_v3_v3v3(ctrl->bone->head, head, parent_offset); 

    ctrl->flag |= RIG_CTRL_HEAD_DONE;

    copy_qt_qt(ctrl->qrot, qrot);

    if (ctrl->tail_mode == TL_NONE)
    {
        sub_v3_v3v3(tail_offset, ctrl->tail, ctrl->head);
        mul_v3_fl(tail_offset, resize);
        mul_qt_v3(qrot, tail_offset);

        add_v3_v3v3(ctrl->bone->tail, ctrl->bone->head, tail_offset);
        
        ctrl->flag |= RIG_CTRL_TAIL_DONE;
    }

    finalizeControl(rigg, ctrl, resize);
}

static void repositionBone(bContext *C, RigGraph *rigg, RigEdge *edge, float vec0[3], float vec1[3], float up_axis[3])
{
    Scene *scene = CTX_data_scene(C);
    EditBone *bone;
    RigControl *ctrl;
    float qrot[4], resize;
    float v1[3], v2[3];
    float l1, l2;
    
    bone = edge->bone;
    
    sub_v3_v3v3(v1, edge->tail, edge->head);
    sub_v3_v3v3(v2, vec1, vec0);
    
    l1 = normalize_v3(v1);
    l2 = normalize_v3(v2);

    resize = l2 / l1;
    
    rotation_between_vecs_to_quat(qrot, v1, v2);
    
    copy_v3_v3(bone->head, vec0);
    copy_v3_v3(bone->tail, vec1);
    
    if (!is_zero_v3(up_axis))
    {
        float qroll[4];

        if (scene->toolsettings->skgen_retarget_roll == SK_RETARGET_ROLL_VIEW)
        {
            bone->roll = rollBoneByQuatAligned(bone, edge->up_axis, qrot, qroll, up_axis);
        }
        else if (scene->toolsettings->skgen_retarget_roll == SK_RETARGET_ROLL_JOINT)
        {
            bone->roll = rollBoneByQuatJoint(edge, edge->prev, qrot, qroll, up_axis);
        }
        else
        {
            unit_qt(qroll);
        }
        
        mul_qt_qtqt(qrot, qroll, qrot);
    }
    else
    {
        bone->roll = rollBoneByQuat(bone, edge->up_axis, qrot);
    }

    for (ctrl = rigg->controls.first; ctrl; ctrl = ctrl->next)
    {
        if (ctrl->link == bone)
        {
            repositionControl(rigg, ctrl, vec0, vec1, qrot, resize);
        }
        if (ctrl->link_tail == bone)
        {
            repositionTailControl(rigg, ctrl);
        }
    }
}

static RetargetMode detectArcRetargetMode(RigArc *arc);
static void retargetArctoArcLength(bContext *C, RigGraph *rigg, RigArc *iarc, RigNode *inode_start);


static RetargetMode detectArcRetargetMode(RigArc *iarc)
{
    RetargetMode mode = RETARGET_AGGRESSIVE;
    ReebArc *earc = iarc->link_mesh;
    RigEdge *edge;
    int large_angle = 0;
    float avg_angle = 0;
    /* float avg_length = 0; */ /* UNUSED */
    int nb_edges = 0;
    
    
    for (edge = iarc->edges.first; edge; edge = edge->next)
    {
        avg_angle += edge->angle;
        nb_edges++;
    }
    
    avg_angle /= nb_edges - 1; /* -1 because last edge doesn't have an angle */

    /* avg_length = iarc->length / nb_edges; */  /* UNUSED */
    
    
    if (nb_edges > 2)
    {
        for (edge = iarc->edges.first; edge; edge = edge->next)
        {
            if (fabs(edge->angle - avg_angle) > M_PI / 6)
            {
                large_angle = 1;
            }
        }
    }
    else if (nb_edges == 2 && avg_angle > 0)
    {
        large_angle = 1;
    }
        
    
    if (large_angle == 0)
    {
        mode = RETARGET_LENGTH;
    }
    
    if (earc->bcount <= (iarc->count - 1))
    {
        mode = RETARGET_LENGTH;
    }
    
    return mode;
}

#ifndef USE_THREADS
static void printMovesNeeded(int *positions, int nb_positions)
{
    int moves = 0;
    int i;
    
    for (i = 0; i < nb_positions; i++)
    {
        moves += positions[i] - (i + 1);
    }
    
    printf("%i moves needed\n", moves);
}

static void printPositions(int *positions, int nb_positions)
{
    int i;
    
    for (i = 0; i < nb_positions; i++)
    {
        printf("%i ", positions[i]);
    }
    printf("\n");
}
#endif

#define MAX_COST FLT_MAX /* FIX ME */

static float costDistance(BArcIterator *iter, float *vec0, float *vec1, int i0, int i1, float distance_weight)
{
    EmbedBucket *bucket = NULL;
    float max_dist = 0;
    float v1[3], v2[3], c[3];
    float v1_inpf;

    if (distance_weight > 0)
    {
        sub_v3_v3v3(v1, vec0, vec1);
        
        v1_inpf = dot_v3v3(v1, v1);
        
        if (v1_inpf > 0)
        {
            int j;
            for (j = i0 + 1; j < i1 - 1; j++)
            {
                float dist;
                
                bucket = IT_peek(iter, j);
    
                sub_v3_v3v3(v2, bucket->p, vec1);
        
                cross_v3_v3v3(c, v1, v2);
                
                dist = dot_v3v3(c, c) / v1_inpf;
                
                max_dist = dist > max_dist ? dist : max_dist;
            }
            
            return distance_weight * max_dist;
        }
        else
        {
            return MAX_COST;
        }
    }
    else
    {
        return 0;
    }
}

static float costAngle(float original_angle, float vec_first[3], float vec_second[3], float angle_weight)
{
    if (angle_weight > 0)
    {
        float current_angle;
        
        if (!is_zero_v3(vec_first) && !is_zero_v3(vec_second))
        {
            current_angle = saacos(dot_v3v3(vec_first, vec_second));

            return angle_weight * fabsf(current_angle - original_angle);
        }
        else
        {
            return angle_weight * (float)M_PI;
        }
    }
    else
    {
        return 0;
    }
}

static float costLength(float original_length, float current_length, float length_weight)
{
    if (current_length == 0)
    {
        return MAX_COST;
    }
    else
    {
        float length_ratio = fabs((current_length - original_length) / original_length);
        return length_weight * length_ratio * length_ratio;
    }
}

#if 0
static float calcCostLengthDistance(BArcIterator *iter, float **vec_cache, RigEdge *edge, float *vec1, float *vec2, int i1, int i2)
{
    float vec[3];
    float length;

    sub_v3_v3v3(vec, vec2, vec1);
    length = normalize_v3(vec);

    return costLength(edge->length, length) + costDistance(iter, vec1, vec2, i1, i2);
}
#endif

static float calcCostAngleLengthDistance(BArcIterator *iter, float **UNUSED(vec_cache), RigEdge *edge, float *vec0, float *vec1, float *vec2, int i1, int i2, float angle_weight, float length_weight, float distance_weight)
{
    float vec_second[3], vec_first[3];
    float length2;
    float new_cost = 0;

    sub_v3_v3v3(vec_second, vec2, vec1);
    length2 = normalize_v3(vec_second);


    /* Angle cost */    
    if (edge->prev)
    {
        sub_v3_v3v3(vec_first, vec1, vec0); 
        normalize_v3(vec_first);
        
        new_cost += costAngle(edge->prev->angle, vec_first, vec_second, angle_weight);
    }

    /* Length cost */
    new_cost += costLength(edge->length, length2, length_weight);

    /* Distance cost */
    new_cost += costDistance(iter, vec1, vec2, i1, i2, distance_weight);

    return new_cost;
}

static int indexMemoNode(int nb_positions, int previous, int current, int joints_left)
{
    return joints_left * nb_positions * nb_positions + current * nb_positions + previous;
}

static void copyMemoPositions(int *positions, MemoNode *table, int nb_positions, int joints_left)
{
    int previous = 0, current = 0;
    int i = 0;
    
    for (i = 0; joints_left > 0; joints_left--, i++)
    {
        MemoNode *node;
        node = table + indexMemoNode(nb_positions, previous, current, joints_left);
        
        positions[i] = node->next;
        
        previous = current;
        current = node->next;
    }
}

static MemoNode * solveJoints(MemoNode *table, BArcIterator *iter, float **vec_cache, int nb_joints, int nb_positions, int previous, int current, RigEdge *edge, int joints_left, float angle_weight, float length_weight, float distance_weight)
{
    MemoNode *node;
    int index = indexMemoNode(nb_positions, previous, current, joints_left);
    
    node = table + index;
    
    if (node->weight != 0)
    {
        return node;
    }
    else if (joints_left == 0)
    {
        float *vec0 = vec_cache[previous];
        float *vec1 = vec_cache[current];
        float *vec2 = vec_cache[nb_positions + 1];

        node->weight = calcCostAngleLengthDistance(iter, vec_cache, edge, vec0, vec1, vec2, current, iter->length, angle_weight, length_weight, distance_weight);

        return node;
    }
    else
    {
        MemoNode *min_node = NULL;
        float *vec0 = vec_cache[previous];
        float *vec1 = vec_cache[current];
        float min_weight= 0.0f;
        int min_next= 0;
        int next;
        
        for (next = current + 1; next <= nb_positions - (joints_left - 1); next++)
        {
            MemoNode *next_node;
            float *vec2 = vec_cache[next];
            float weight = 0.0f;
            
            /* ADD WEIGHT OF PREVIOUS - CURRENT - NEXT triple */
            weight = calcCostAngleLengthDistance(iter, vec_cache, edge, vec0, vec1, vec2, current, next, angle_weight, length_weight, distance_weight);
            
            if (weight >= MAX_COST)
            {
                continue;
            }
            
            /* add node weight */
            next_node = solveJoints(table, iter, vec_cache, nb_joints, nb_positions, current, next, edge->next, joints_left - 1, angle_weight, length_weight, distance_weight);
            weight += next_node->weight;
            
            if (min_node == NULL || weight < min_weight)
            {
                min_weight = weight;
                min_node = next_node;
                min_next = next;
            }
        }
        
        if (min_node)
        {
            node->weight = min_weight;
            node->next = min_next;
            return node;
        }
        else
        {
            node->weight = MAX_COST;
            return node;
        }
    }
    
}

static int testFlipArc(RigArc *iarc, RigNode *inode_start)
{
    ReebArc *earc = iarc->link_mesh;
    ReebNode *enode_start = BIF_NodeFromIndex(earc, inode_start->link_mesh);
    
    /* no flip needed if both nodes are the same */
    if ((enode_start == earc->head && inode_start == iarc->head) || (enode_start == earc->tail && inode_start == iarc->tail))
    {
        return 0;
    }
    else
    {
        return 1;
    }
}

static void retargetArctoArcAggresive(bContext *C, RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
{
    ReebArcIterator arc_iter;
    BArcIterator *iter = (BArcIterator*)&arc_iter;
    RigEdge *edge;
    EmbedBucket *bucket = NULL;
    ReebNode *node_start, *node_end;
    ReebArc *earc = iarc->link_mesh;
    float angle_weight = 1.0; // GET FROM CONTEXT
    float length_weight = 1.0;
    float distance_weight = 1.0;
#ifndef USE_THREADS
    float min_cost = FLT_MAX;
#endif
    float *vec0, *vec1;
    int *best_positions;
    int nb_edges = BLI_countlist(&iarc->edges);
    int nb_joints = nb_edges - 1;
    RetargetMethod method = METHOD_MEMOIZE;
    int i;
    
    if (nb_joints > earc->bcount)
    {
        printf("NOT ENOUGH BUCKETS!\n");
        return;
    }

    best_positions = MEM_callocN(sizeof(int) * nb_joints, "Best positions");
    
    if (testFlipArc(iarc, inode_start))
    {
        node_start = earc->tail;
        node_end = earc->head;
    }
    else
    {
        node_start = earc->head;
        node_end = earc->tail;
    }

    /* equal number of joints and potential position, just fill them in */
    if (nb_joints == earc->bcount)
    {
        int i;
        
        /* init with first values */
        for (i = 0; i < nb_joints; i++)
        {
            best_positions[i] = i + 1;
        }
    }
    if (method == METHOD_MEMOIZE)
    {
        int nb_positions = earc->bcount;
        int nb_memo_nodes = nb_positions * nb_positions * (nb_joints + 1);
        MemoNode *table = MEM_callocN(nb_memo_nodes * sizeof(MemoNode), "memoization table");
#ifndef USE_THREADS
        MemoNode *result;
#endif
        float **positions_cache = MEM_callocN(sizeof(float*) * (nb_positions + 2), "positions cache");
        int i;
        
        positions_cache[0] = node_start->p;
        positions_cache[nb_positions + 1] = node_end->p;
        
        initArcIterator(iter, earc, node_start);

        for (i = 1; i <= nb_positions; i++)
        {
            EmbedBucket *bucket = IT_peek(iter, i);
            positions_cache[i] = bucket->p;
        }

#ifndef USE_THREADS
        result = solveJoints(table, iter, positions_cache, nb_joints, earc->bcount, 0, 0, iarc->edges.first, nb_joints, angle_weight, length_weight, distance_weight);
        min_cost = result->weight;
#else
        solveJoints(table, iter, positions_cache, nb_joints, earc->bcount, 0, 0, iarc->edges.first, nb_joints, angle_weight, length_weight, distance_weight);
#endif

        copyMemoPositions(best_positions, table, earc->bcount, nb_joints);

        MEM_freeN(table);
        MEM_freeN(positions_cache);
    }

    vec0 = node_start->p;
    initArcIterator(iter, earc, node_start);
    
#ifndef USE_THREADS
    printPositions(best_positions, nb_joints);
    printMovesNeeded(best_positions, nb_joints);
    printf("min_cost %f\n", min_cost);
    printf("buckets: %i\n", earc->bcount);
#endif

    /* set joints to best position */
    for (edge = iarc->edges.first, i = 0;
         edge;
         edge = edge->next, i++)
    {
        float *no = NULL;
        if (i < nb_joints)
        {
            bucket = IT_peek(iter, best_positions[i]);
            vec1 = bucket->p;
            no = bucket->no;
        }
        else
        {
            vec1 = node_end->p;
            no = node_end->no;
        }
        
        if (edge->bone)
        {
            repositionBone(C, rigg, edge, vec0, vec1, no);
        }
        
        vec0 = vec1;
    }

    MEM_freeN(best_positions);
}

static void retargetArctoArcLength(bContext *C, RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
{
    ReebArcIterator arc_iter;
    BArcIterator *iter = (BArcIterator*)&arc_iter;
    ReebArc *earc = iarc->link_mesh;
    ReebNode *node_start, *node_end;
    RigEdge *edge;
    EmbedBucket *bucket = NULL;
    float embedding_length = 0;
    float *vec0 = NULL;
    float *vec1 = NULL;
    float *previous_vec = NULL;

    
    if (testFlipArc(iarc, inode_start))
    {
        node_start = (ReebNode*)earc->tail;
        node_end = (ReebNode*)earc->head;
    }
    else
    {
        node_start = (ReebNode*)earc->head;
        node_end = (ReebNode*)earc->tail;
    }
    
    initArcIterator(iter, earc, node_start);

    bucket = IT_next(iter);
    
    vec0 = node_start->p;
    
    while (bucket != NULL)
    {
        vec1 = bucket->p;
        
        embedding_length += len_v3v3(vec0, vec1);
        
        vec0 = vec1;
        bucket = IT_next(iter);
    }
    
    embedding_length += len_v3v3(node_end->p, vec1);
    
    /* fit bones */
    initArcIterator(iter, earc, node_start);

    bucket = IT_next(iter);

    vec0 = node_start->p;
    previous_vec = vec0;
    vec1 = bucket->p;
    
    for (edge = iarc->edges.first; edge; edge = edge->next)
    {
        float new_bone_length = edge->length / iarc->length * embedding_length;
        float *no = NULL;
        float length = 0;

        while (bucket && new_bone_length > length)
        {
            length += len_v3v3(previous_vec, vec1);
            bucket = IT_next(iter);
            previous_vec = vec1;
            vec1 = bucket->p;
            no = bucket->no;
        }
        
        if (bucket == NULL)
        {
            vec1 = node_end->p;
            no = node_end->no;
        }

        /* no need to move virtual edges (space between unconnected bones) */       
        if (edge->bone)
        {
            repositionBone(C, rigg, edge, vec0, vec1, no);
        }
        
        vec0 = vec1;
        previous_vec = vec1;
    }
}

static void retargetArctoArc(bContext *C, RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
{
#ifdef USE_THREADS
    RetargetParam *p = MEM_callocN(sizeof(RetargetParam), "RetargetParam");
    
    p->rigg = rigg;
    p->iarc = iarc;
    p->inode_start = inode_start;
    p->context = C;
    
    BLI_insert_work(rigg->worker, p);
#else
    RetargetParam p;

    p.rigg = rigg;
    p.iarc = iarc;
    p.inode_start = inode_start;
    p.context = C;
    
    exec_retargetArctoArc(&p);
#endif
}

void *exec_retargetArctoArc(void *param)
{
    RetargetParam *p = (RetargetParam*)param;
    RigGraph *rigg = p->rigg;
    RigArc *iarc = p->iarc;
    bContext *C = p->context;   
    RigNode *inode_start = p->inode_start;
    ReebArc *earc = iarc->link_mesh;
    
    if (BLI_countlist(&iarc->edges) == 1)
    {
        RigEdge *edge = iarc->edges.first;

        if (testFlipArc(iarc, inode_start))
        {
            repositionBone(C, rigg, edge, earc->tail->p, earc->head->p, earc->head->no);
        }
        else
        {
            repositionBone(C, rigg, edge, earc->head->p, earc->tail->p, earc->tail->no);
        }
    }
    else
    {
        RetargetMode mode = detectArcRetargetMode(iarc);
        
        if (mode == RETARGET_AGGRESSIVE)
        {
            retargetArctoArcAggresive(C, rigg, iarc, inode_start);
        }
        else
        {       
            retargetArctoArcLength(C, rigg, iarc, inode_start);
        }
    }

#ifdef USE_THREADS
    MEM_freeN(p);
#endif
    
    return NULL;
}

static void matchMultiResolutionNode(RigGraph *rigg, RigNode *inode, ReebNode *top_node)
{
    ReebNode *enode = top_node;
    ReebGraph *reebg = BIF_graphForMultiNode(rigg->link_mesh, enode);
    int ishape, eshape;
    
    ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)inode, NULL, 0) % SHAPE_LEVELS;
    eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
    
    inode->link_mesh = enode;

    while (ishape == eshape && enode->link_down)
    {
        inode->link_mesh = enode;

        enode = enode->link_down;
        reebg = BIF_graphForMultiNode(rigg->link_mesh, enode); /* replace with call to link_down once that exists */
        eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
    } 
}

static void markMultiResolutionChildArc(ReebNode *end_enode, ReebNode *enode)
{
    int i;
    
    for(i = 0; i < enode->degree; i++)
    {
        ReebArc *earc = (ReebArc*)enode->arcs[i];
        
        if (earc->flag == ARC_FREE)
        {
            earc->flag = ARC_TAKEN;
            
            if (earc->tail->degree > 1 && earc->tail != end_enode)
            {
                markMultiResolutionChildArc(end_enode, earc->tail);
            }
            break;
        }
    }
}

static void markMultiResolutionArc(ReebArc *start_earc)
{
    if (start_earc->link_up)
    {
        ReebArc *earc;
        for (earc = start_earc->link_up ; earc; earc = earc->link_up)
        {
            earc->flag = ARC_TAKEN;
            
            if (earc->tail->index != start_earc->tail->index)
            {
                markMultiResolutionChildArc(earc->tail, earc->tail);
            }
        }
    }
}

static void matchMultiResolutionArc(RigGraph *rigg, RigNode *start_node, RigArc *next_iarc, ReebArc *next_earc)
{
    ReebNode *enode = next_earc->head;
    ReebGraph *reebg = BIF_graphForMultiNode(rigg->link_mesh, enode);
    int ishape, eshape;

    ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)start_node, (BArc*)next_iarc, 1) % SHAPE_LEVELS;
    eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, (BArc*)next_earc, 1) % SHAPE_LEVELS;
    
    while (ishape != eshape && next_earc->link_up)
    {
        next_earc->flag = ARC_TAKEN; // mark previous as taken, to prevent backtrack on lower levels
        
        next_earc = next_earc->link_up;
        reebg = reebg->link_up;
        enode = next_earc->head;
        eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, (BArc*)next_earc, 1) % SHAPE_LEVELS;
    } 

    next_earc->flag = ARC_USED;
    next_iarc->link_mesh = next_earc;
    
    /* mark all higher levels as taken too */
    markMultiResolutionArc(next_earc);
//  while (next_earc->link_up)
//  {
//      next_earc = next_earc->link_up;
//      next_earc->flag = ARC_TAKEN;
//  }
}

static void matchMultiResolutionStartingNode(RigGraph *rigg, ReebGraph *reebg, RigNode *inode)
{
    ReebNode *enode;
    int ishape, eshape;
    
    enode = reebg->nodes.first;
    
    ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)inode, NULL, 0) % SHAPE_LEVELS;
    eshape = BLI_subtreeShape((BGraph*)rigg->link_mesh, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
    
    while (ishape != eshape && reebg->link_up)
    {
        reebg = reebg->link_up;
        
        enode = reebg->nodes.first;
        
        eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
    } 

    inode->link_mesh = enode;
}

static void findCorrespondingArc(RigGraph *rigg, RigArc *start_arc, RigNode *start_node, RigArc *next_iarc, int root)
{
    ReebNode *enode = start_node->link_mesh;
    ReebArc *next_earc;
    int symmetry_level = next_iarc->symmetry_level;
    int symmetry_group = next_iarc->symmetry_group;
    int symmetry_flag = next_iarc->symmetry_flag;
    int i;
    
    next_iarc->link_mesh = NULL;
        
//  if (root)
//  {
//      printf("-----------------------\n");
//      printf("MATCHING LIMB\n");
//      RIG_printArcBones(next_iarc);
//  }
    
    for(i = 0; i < enode->degree; i++)
    {
        next_earc = (ReebArc*)enode->arcs[i];
        
//      if (next_earc->flag == ARC_FREE)
//      {
//          printf("candidate (level %i ?= %i) (flag %i ?= %i) (group %i ?= %i)\n",
//          symmetry_level, next_earc->symmetry_level,
//          symmetry_flag, next_earc->symmetry_flag, 
//          symmetry_group, next_earc->symmetry_flag);
//      }
        
        if (next_earc->flag == ARC_FREE &&
            next_earc->symmetry_flag == symmetry_flag &&
            next_earc->symmetry_group == symmetry_group &&
            next_earc->symmetry_level == symmetry_level)
        {
//          printf("CORRESPONDING ARC FOUND\n");
//          printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
            
            matchMultiResolutionArc(rigg, start_node, next_iarc, next_earc);
            break;
        }
    }
    
    /* not found, try at higher nodes (lower node might have filtered internal arcs, messing shape of tree */
    if (next_iarc->link_mesh == NULL)
    {
//      printf("NO CORRESPONDING ARC FOUND - GOING TO HIGHER LEVELS\n");
        
        if (enode->link_up)
        {
            start_node->link_mesh = enode->link_up;
            findCorrespondingArc(rigg, start_arc, start_node, next_iarc, 0);
        }
    }

    /* still not found, print debug info */
    if (root && next_iarc->link_mesh == NULL)
    {
        start_node->link_mesh = enode; /* linking back with root node */
        
//      printf("NO CORRESPONDING ARC FOUND\n");
//      RIG_printArcBones(next_iarc);
//      
//      printf("ON NODE %i, multilevel %i\n", enode->index, enode->multi_level);
//      
//      printf("LOOKING FOR\n");
//      printf("flag %i -- level %i -- flag %i -- group %i\n", ARC_FREE, symmetry_level, symmetry_flag, symmetry_group);
//      
//      printf("CANDIDATES\n");
//      for(i = 0; i < enode->degree; i++)
//      {
//          next_earc = (ReebArc*)enode->arcs[i];
//          printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
//      }
        
        /* Emergency matching */
        for(i = 0; i < enode->degree; i++)
        {
            next_earc = (ReebArc*)enode->arcs[i];
            
            if (next_earc->flag == ARC_FREE && next_earc->symmetry_level == symmetry_level)
            {
//              printf("USING: \n");
//              printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
                matchMultiResolutionArc(rigg, start_node, next_iarc, next_earc);
                break;
            }
        }
    }

}

static void retargetSubgraph(bContext *C, RigGraph *rigg, RigArc *start_arc, RigNode *start_node)
{
    RigNode *inode = start_node;
    int i;

    /* no start arc on first node */
    if (start_arc)
    {       
        ReebNode *enode = start_node->link_mesh;
        ReebArc *earc = start_arc->link_mesh;
        
        retargetArctoArc(C, rigg, start_arc, start_node);
        
        enode = BIF_otherNodeFromIndex(earc, enode);
        inode = (RigNode*)BLI_otherNode((BArc*)start_arc, (BNode*)inode);
    
        /* match with lowest node with correct shape */
        matchMultiResolutionNode(rigg, inode, enode);
    }
    
    for(i = 0; i < inode->degree; i++)
    {
        RigArc *next_iarc = (RigArc*)inode->arcs[i];
        
        /* no back tracking */
        if (next_iarc != start_arc)
        {
            findCorrespondingArc(rigg, start_arc, inode, next_iarc, 1);
            if (next_iarc->link_mesh)
            {
                retargetSubgraph(C, rigg, next_iarc, inode);
            }
        }
    }
}

static void finishRetarget(RigGraph *rigg)
{
#ifdef USE_THREADS
    BLI_end_worker(rigg->worker);
#endif
}

static void adjustGraphs(bContext *C, RigGraph *rigg)
{
    bArmature *arm= rigg->ob->data;
    RigArc *arc;
    
    for (arc = rigg->arcs.first; arc; arc = arc->next)
    {
        if (arc->link_mesh)
        {
            retargetArctoArc(C, rigg, arc, arc->head);
        }
    }

    finishRetarget(rigg);

    /* Turn the list into an armature */
    arm->edbo = rigg->editbones;
    ED_armature_from_edit(rigg->ob);
    
    ED_undo_push(C, "Retarget Skeleton");
}

static void retargetGraphs(bContext *C, RigGraph *rigg)
{
    bArmature *arm= rigg->ob->data;
    ReebGraph *reebg = rigg->link_mesh;
    RigNode *inode;
    
    /* flag all ReebArcs as free */
    BIF_flagMultiArcs(reebg, ARC_FREE);
    
    /* return to first level */
    inode = rigg->head;
    
    matchMultiResolutionStartingNode(rigg, reebg, inode);

    retargetSubgraph(C, rigg, NULL, inode);
    
    //generateMissingArcs(rigg);
    
    finishRetarget(rigg);

    /* Turn the list into an armature */
    arm->edbo = rigg->editbones;
    ED_armature_from_edit(rigg->ob);
}

const char *RIG_nameBone(RigGraph *rg, int arc_index, int bone_index)
{
    RigArc *arc = BLI_findlink(&rg->arcs, arc_index);
    RigEdge *iedge;

    if (arc == NULL)
    {
        return "None";
    }
    
    if (bone_index == BLI_countlist(&arc->edges))
    {
        return "Last joint";
    }

    iedge = BLI_findlink(&arc->edges, bone_index);
    
    if (iedge == NULL)
    {
        return "Done";
    }
    
    if (iedge->bone == NULL)
    {
        return "Bone offset";
    }
    
    return iedge->bone->name;
}

int RIG_nbJoints(RigGraph *rg)
{
    RigArc *arc;
    int total = 0;
    
    total += BLI_countlist(&rg->nodes);
    
    for (arc = rg->arcs.first; arc; arc = arc->next)
    {
        total += BLI_countlist(&arc->edges) - 1; /* -1 because end nodes are already counted */
    }
    
    return total;
}

static void BIF_freeRetarget(void)
{
    if (GLOBAL_RIGG)
    {
        RIG_freeRigGraph((BGraph*)GLOBAL_RIGG);
        GLOBAL_RIGG = NULL;
    }
}

void BIF_retargetArmature(bContext *C)
{
    ReebGraph *reebg;
    double start_time, end_time;
    double gstart_time, gend_time;
    double reeb_time, rig_time=0.0, retarget_time=0.0, total_time;
    
    gstart_time = start_time = PIL_check_seconds_timer();
    
    reebg = BIF_ReebGraphMultiFromEditMesh(C);
    
    end_time = PIL_check_seconds_timer();
    reeb_time = end_time - start_time;
    
    printf("Reeb Graph created\n");

    CTX_DATA_BEGIN(C, Base*, base, selected_editable_bases) {
        Object *ob = base->object;

        if (ob->type==OB_ARMATURE)
        {
            RigGraph *rigg;
            bArmature *arm;

            arm = ob->data;
        
            /* Put the armature into editmode */
            
        
            start_time = PIL_check_seconds_timer();

            rigg = RIG_graphFromArmature(C, ob, arm);
            
            end_time = PIL_check_seconds_timer();
            rig_time = end_time - start_time;

            printf("Armature graph created\n");
    
            //RIG_printGraph(rigg);
            
            rigg->link_mesh = reebg;
            
            printf("retargetting %s\n", ob->id.name);
            
            start_time = PIL_check_seconds_timer();

            retargetGraphs(C, rigg);
            
            end_time = PIL_check_seconds_timer();
            retarget_time = end_time - start_time;

            BIF_freeRetarget();
            
            GLOBAL_RIGG = rigg;
            
            break; /* only one armature at a time */
        }
    }
    CTX_DATA_END;

    
    gend_time = PIL_check_seconds_timer();

    total_time = gend_time - gstart_time;

    printf("-----------\n");
    printf("runtime: \t%.3f\n", total_time);
    printf("reeb: \t\t%.3f (%.1f%%)\n", reeb_time, reeb_time / total_time * 100);
    printf("rig: \t\t%.3f (%.1f%%)\n", rig_time, rig_time / total_time * 100);
    printf("retarget: \t%.3f (%.1f%%)\n", retarget_time, retarget_time / total_time * 100);
    printf("-----------\n");
    
    ED_undo_push(C, "Retarget Skeleton");

    // XXX  
//  allqueue(REDRAWVIEW3D, 0);
}

void BIF_retargetArc(bContext *C, ReebArc *earc, RigGraph *template_rigg)
{
    Object *obedit = CTX_data_edit_object(C);
    Scene *scene = CTX_data_scene(C);
    bArmature *armedit = obedit->data;
    Object *ob;
    RigGraph *rigg;
    RigArc *iarc;
    char *side_string = scene->toolsettings->skgen_side_string;
    char *num_string = scene->toolsettings->skgen_num_string;
    int free_template = 0;
    
    if (template_rigg)
    {
        ob = template_rigg->ob;     
    }
    else
    {
        free_template = 1;
        ob = obedit;
        template_rigg = armatureSelectedToGraph(C, ob, ob->data);
    }
    
    if (template_rigg->arcs.first == NULL)
    {
//      XXX
//      error("No Template and no deforming bones selected");
        return;
    }
    
    rigg = cloneRigGraph(template_rigg, armedit->edbo, obedit, side_string, num_string);
    
    iarc = rigg->arcs.first;
    
    iarc->link_mesh = earc;
    iarc->head->link_mesh = earc->head;
    iarc->tail->link_mesh = earc->tail;
    
    retargetArctoArc(C, rigg, iarc, iarc->head);
    
    finishRetarget(rigg);
    
    /* free template if it comes from the edit armature */
    if (free_template)
    {
        RIG_freeRigGraph((BGraph*)template_rigg);
    }
    RIG_freeRigGraph((BGraph*)rigg);
    
    ED_armature_validate_active(armedit);

//  XXX
//  allqueue(REDRAWVIEW3D, 0);
}

void BIF_adjustRetarget(bContext *C)
{
    if (GLOBAL_RIGG)
    {
        adjustGraphs(C, GLOBAL_RIGG);
    }
}