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

template<typename _Scalar, int _Rows, int _Storage>
float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) {
    _Scalar product = 0;
    for (int i = 0; i < b.size(); i++) {
        product += a(i, 0) * b(i);
    }
    return product;
}

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.
    auto svd = SVD(m_J);
    svd.decompose();

//    svd.getSigma() * svd.getV().transpose() * d_theta - svd.getU().transpose() * dx;

    int rank = 0;
    for (int i = 0; i < svd.getSigma().size(); i++) {
        if (svd.getSigma()(i) == 0) {
            break;
        }

        rank++;
    }
    auto d_theta = Vector<float, Dynamic>(rank);
    auto z = Vector<float, Dynamic>(rank);
    for (int i = 0; i < rank; i++) {
        auto ui = svd.getU().block(i, 0, 3, 1);
        auto si = svd.getSigma()(i);
        z(i) = dotP(ui, dx) / si;
    }

    d_theta = svd.getV() * z;

    // 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.
    float alpha = 1;
    Vector<float, Dynamic> initial_theta;
    m_armature->pack(initial_theta);
    do {
        m_armature->unpack(initial_theta + alpha * d_theta);
        m_armature->updateKinematics();

        alpha /= 2;
    } while (getError(dx) < error);
}