#pragma once

#include "Armature.h"

using namespace gti320;

// Constructor
//
Link::Link(Link *_parent, const Vector3f &_eulerAng, const Vector3f &_trans) :
        parent(_parent), eulerAng(_eulerAng), trans(_trans) {
    if (parent != nullptr) {
        parent->enfants.push_back(this);
    }
    M.setIdentity();
}

void Link::forward() {
    // Create a rotation matrix from the Euler angles
    //      of the current link.
    // Create a local 4D rigid transformation matrix from the
    //      3D rotation matrix and translation of the current link.
    Matrix<float, 4, 4> local;
    local.setIdentity();
    local.block(0, 0, 3, 3) = makeRotation(eulerAng(0), eulerAng(1), eulerAng(2));
    local(0, 3) = trans(0);
    local(1, 3) = trans(1);
    local(2, 3) = trans(2);

    // Update the global transformation for the link using the
    //      parent's rigid transformation matrix and the local transformation.
    //      Hint : the parent matrix should be post-multiplied.
    //      Hint : the root does not have a parent. You must consider this case.
    if (isRoot()) {
        M = local;
    } else {
        M = parent->M * local;
    }

    // Update the transformation of child links
    // by recursion.
    for (const auto &child: enfants) {
        child->forward();
    }
}


Armature::Armature() : links(), root(nullptr) {

}

Armature::~Armature() {
    for (Link *l: links) {
        delete l;
    }
}

void Armature::updateKinematics() {
    assert(root != nullptr);
    root->forward();
}

void Armature::pack(Vector<float, Dynamic> &theta) {
    // TODO Collect the Euler angles of each link and put them
    //      into the dense vector @a theta

}

void Armature::unpack(const Vector<float, Dynamic> &theta) {
    const int numLinks = links.size();
    assert(theta.size() == 3 * numLinks);

    // TODO Extract the Euler angles contained in the 
    //      dense vector @a theta and update the angles
    //      for each link in the armature.
    // 

}