#pragma once

/*
 * Nom: William Nolin
 * Code permanent : NOLW76060101
 * Email : william.nolin.1@ens.etsmtl.ca
 */

#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();
    }
}

Vector3f Link::globalPosition() {
    Vector3f pos;
    pos(0) = M(0, 3);
    pos(1) = M(1, 3);
    pos(2) = M(2, 3);
    return pos;
}

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) {
    // Collect the Euler angles of each link and put them
    //      into the dense vector @a theta
    theta.resize(links.size() * 3);

    for (int i = 0; i < links.size(); i++) {
        Link *link = links[i];
        int link_index = i * 3;

        theta(link_index) = link->eulerAng(0);
        theta(link_index + 1) = link->eulerAng(1);
        theta(link_index + 2) = link->eulerAng(2);
    }
}

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

    // Extract the Euler angles contained in the
    //      dense vector @a theta and update the angles
    //      for each link in the armature.
    for (int i = 0; i < links.size(); i++) {
        Link *link = links[i];
        int link_index = i * 3;

        link->eulerAng(0) = theta(link_index);
        link->eulerAng(1) = theta(link_index + 1);
        link->eulerAng(2) = theta(link_index + 2);
    }
}