#pragma once

#include <cfloat>
#include "IKSolver.h"
#include "Armature.h"
#include "SVD.h"

using namespace gti320;

namespace {
}

IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_armature), m_targetPos(_targetPos), m_J() {
}


float IKSolver::getError(Vector3f &dx) const {
    // TODO Compute the error between the current end effector 
    //      position and the target position
    dx.setZero();

    const int numLinks = m_armature->links.size();
    Link *endEffector = m_armature->links[numLinks - 1];

    Vector3f f_theta = endEffector->globalPosition();
    Vector3f ddx = m_targetPos - f_theta;
    dx(0) = ddx(0);
    dx(1) = ddx(1);
    dx(2) = ddx(2);

    return ddx.norm();
}

void jacobian(Jacobianf &m_J, Link *link, Vector3f &ri, int i) {
    // axes x, y et z
    for (int j = 0; j < 3; j++) {
        // crossP
        m_J(0, i) = link->M(j, 1) * ri(2) - link->M(j, 2) * ri(1);
        m_J(1, i) = link->M(j, 0) * ri(2) - link->M(j, 2) * ri(0);
        m_J(2, i) = link->M(j, 0) * ri(1) - link->M(j, 1) * ri(0);
    }
}

void IKSolver::solve() {
    const int numLinks = m_armature->links.size();
    const int dim = 3 * (numLinks);
    m_J.resize(3, dim);

    // We assume that the last link is the "end effector"
    //
    Link *endEffector = m_armature->links[numLinks - 1];

    // Build the Jacobian matrix m_J.
    //      Each column corresponds to a separate link
    for (int i = 0; i < numLinks; i++) {
        Link *link = m_armature->links[i];
        Vector3f shift = endEffector->globalPosition() - link->globalPosition();

        jacobian(m_J, link, shift, i);
    }

    // TODO Compute the error between the current end effector 
    //      position and the target position by calling getError()
    Vector3f dx;
    float error = getError(dx);

    // TODO Compute the change in the joint angles by solving:
    //    df/dtheta * delta_theta = delta_x
    //  where df/dtheta is the Jacobian matrix.
    //    
    //

    // TODO Perform gradient descent method with line search
    //      to move the end effector toward the target position.
    //
    //   Hint: use the Armature::unpack() and Armature::pack() functions
    //   to set and get the joint angles of the armature.
    // 
    //   Hint: whenever you change the joint angles, you must also call
    //   armature->updateKinematics() to compute the global position.
    //    

}