/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#define SOFA_COMPONENT_ENGINE_INERTIAALIGN_CPP

#include "InertiaAlign.h"
#include <sofa/core/ObjectFactory.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
namespace sofa
{

namespace component
{

using namespace sofa::defaulttype;

int InertiaAlignClass = core::RegisterObject("An engine computing inertia matrix and the principal direction of a mesh.")
        .add< InertiaAlign >()
        ;

SOFA_DECL_CLASS(InertiaAlign)

InertiaAlign::InertiaAlign()
    : targetC( initData(&targetC,"targetCenter","input: the gravity center of the target mesh") )
    , sourceC( initData(&sourceC,"sourceCenter","input: the gravity center of the source mesh") )
    , targetInertiaMatrix( initData(&targetInertiaMatrix,"targetInertiaMatrix","input: the inertia matrix of the target mesh") )
    , sourceInertiaMatrix( initData(&sourceInertiaMatrix,"sourceInertiaMatrix","input: the inertia matrix of the source mesh") )
    , m_positiont( initData(&m_positiont,"targetPosition","input: positions of the target vertices") )
    , m_positions( initData(&m_positions,"sourcePosition","input: positions of the source vertices") )
{

}

InertiaAlign::~InertiaAlign()
{
}

void InertiaAlign::init()
{
    //Activate an output data
    m_positions.setPersistent(false);
    m_positiont.setPersistent(false);

    sourceInertiaMatrix.setPersistent(false);
    // Allow to edit an output data

    helper::WriteAccessor<Data<helper::vector<sofa::defaulttype::Vec<3,SReal> > > > waPositions = m_positions;

    Eigen::MatrixXd eigenSourceInertiaMatrix(3,3);
    Eigen::MatrixXd eigenTargetInertiaMatrix(3,3);

    SReal Sxx,Syy,Szz,Txx,Tyy,Tzz;
    for(unsigned int i=0; i<3; i++ )
    {
        for( unsigned int j=0; j<3; j++ )
        {
            eigenSourceInertiaMatrix(i,j) = sourceInertiaMatrix.getValue().col(j)[i];
            eigenTargetInertiaMatrix(i,j) = targetInertiaMatrix.getValue().col(j)[i];

        }
    }
    sout << "Source intertia matrix"<<sendl<<eigenSourceInertiaMatrix << sendl;
    sout << "Target intertia matrix"<<sendl<<eigenTargetInertiaMatrix << sendl;

    Eigen::EigenSolver<Eigen::Matrix3d> solverTarget(eigenTargetInertiaMatrix);
    Eigen::EigenSolver<Eigen::Matrix3d> solverSource(eigenSourceInertiaMatrix);

    /* Creation of the following transformation matrix:
     *
     *U.x V.x W.x tx
     *U.y V.y W.Y ty
     *U.Z V.Z W.z tz
     * 0   0   0   1
     *
     *With U,V,W The proper vectors of the Source inertia matrix (represent the principal axis of inertia)
     *And tx,ty,tz the translation beetween the source object and the world
     */

    Eigen::Matrix3d eigenvectorsTarget,tmp ;
    Eigen::Vector3d eigenvaluesTarget;
    Eigen::Matrix3cd complexEigenvectorsTarget = solverTarget.eigenvectors();
    Eigen::Vector3cd complexEigenvaluesTarget = solverTarget.eigenvalues();

    Eigen::Matrix3d eigenvectorsSource ;
    Eigen::Vector3d eigenvaluesSource ;
    Eigen::Matrix3cd complexEigenvectorsSource = solverSource.eigenvectors();
    Eigen::Vector3cd complexEigenvaluesSource = solverSource.eigenvalues();

    for(unsigned int i = 0; i<3; i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            eigenvectorsSource(i,j)= real(complexEigenvectorsSource(i,j));
            eigenvectorsTarget(i,j)= real(complexEigenvectorsTarget(i,j));
        }
        eigenvaluesTarget(i) = real(complexEigenvaluesTarget(i));
        eigenvaluesSource(i) = real(complexEigenvaluesSource(i));
    }

    SReal minSource = eigenvaluesSource(0);

    //Compute the length of the principal axes
    Sxx = eigenvaluesSource[0];
    Syy = eigenvaluesSource[1];
    Szz = eigenvaluesSource[2];
    Txx = eigenvaluesTarget[0];
    Tyy = eigenvaluesTarget[1];
    Tzz = eigenvaluesTarget[2];

    SReal Lxs = sqrt(abs(-Sxx+Syy+Szz)/2);
    SReal Lys = sqrt(abs( Sxx-Syy+Szz)/2);
    SReal Lzs = sqrt(abs( Sxx+Syy-Szz)/2);

    SReal Lxt = sqrt(abs(-Txx+Tyy+Tzz)/2);
    SReal Lyt = sqrt(abs( Txx-Tyy+Tzz)/2);
    SReal Lzt = sqrt(abs( Txx+Tyy-Tzz)/2);

    //Source inversion of axes (to be sure that u is on u', v on v' and w on w')
    //Check thats the proper values are in the same order for the two objects
    //If not switch the proper values.
    //Inversion of u and v
    if (eigenvaluesSource[0] < eigenvaluesSource[1])
    {
        eigenvaluesSource(0)= eigenvaluesSource(1);
        eigenvaluesSource(1)=minSource;
        minSource = eigenvaluesSource(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,1);
            eigenvectorsSource(i,1) =   eigenvectorsSource(i,0);
            eigenvectorsSource(i,0) = tmp(i);
        }

    }
    //Inversion of u and w
    if (eigenvaluesSource[0]<eigenvaluesSource[2])
    {
        eigenvaluesSource(0)= eigenvaluesSource(2);
        eigenvaluesSource(2)=minSource;
        minSource = eigenvaluesSource(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,2);
            eigenvectorsSource(i,2) =   eigenvectorsSource(i,0);
            eigenvectorsSource(i,0) = tmp(i);
        }
    }
    //Inversion of v and w
    minSource = eigenvaluesSource(1);

    if (eigenvaluesSource[1]<eigenvaluesSource[2])
    {
        eigenvaluesSource(1)= eigenvaluesSource(2);
        eigenvaluesSource(2)=minSource;
        minSource = eigenvaluesSource(1);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,2);
            eigenvectorsSource(i,2) =   eigenvectorsSource(i,1);
            eigenvectorsSource(i,1) = tmp(i);
        }
    }

    SReal minTarget = eigenvaluesTarget(0);

    //Target Inversion of axes
    //Inversion of u' and v'
    if (eigenvaluesTarget[0]<eigenvaluesTarget[1])
    {
        eigenvaluesTarget(0)= eigenvaluesTarget(1);
        eigenvaluesTarget(1)=minTarget;
        minTarget = eigenvaluesTarget(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,1);
            eigenvectorsTarget(i,1) =   eigenvectorsTarget(i,0);
            eigenvectorsTarget(i,0) = tmp(i);
        }
    }
    //Inversion of u' and w'
    if (eigenvaluesTarget[0]<eigenvaluesTarget[2])
    {
        eigenvaluesTarget(0)= eigenvaluesTarget(2);
        eigenvaluesTarget(2)=minTarget;
        minTarget = eigenvaluesTarget(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,2);
            eigenvectorsTarget(i,2) =   eigenvectorsTarget(i,0);
            eigenvectorsTarget(i,0) = tmp(i);
        }
    }
    //Inversion of v' and w'
    minTarget = eigenvaluesTarget(1);

    if (eigenvaluesTarget[1]<eigenvaluesTarget[2])
    {
        eigenvaluesTarget(1)= eigenvaluesTarget(2);
        eigenvaluesTarget(2)=minTarget;
        minTarget = eigenvaluesTarget(1);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,2);
            eigenvectorsTarget(i,2) =   eigenvectorsTarget(i,1);
            eigenvectorsTarget(i,1) = tmp(i);
        }
    }
    //Compute the length of the axes.
    //The scale along x is Lxs/Lxt
    //The ratio 1/5m have been supressed due to the ratio beetween the source/target

    Sxx = eigenvaluesSource[0];
    Syy = eigenvaluesSource[1];
    Szz = eigenvaluesSource[2];
    Txx = eigenvaluesTarget[0];
    Tyy = eigenvaluesTarget[1];
    Tzz = eigenvaluesTarget[2];

    Lxs = sqrt(abs(-Sxx+Syy+Szz)/2);
    Lys = sqrt(abs( Sxx-Syy+Szz)/2);
    Lzs = sqrt(abs( Sxx+Syy-Szz)/2);

    Lxt = sqrt(abs(-Txx+Tyy+Tzz)/2);
    Lyt = sqrt(abs( Txx-Tyy+Tzz)/2);
    Lzt = sqrt(abs( Txx+Tyy-Tzz)/2);

    sout << "EigenVectorsSource =" << sendl;
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
            sout << eigenvectorsSource(i,j) << " ";
        sout << sendl;

    }

    SReal scale_u = Lxs / Lxt;
    SReal scale_v = Lys / Lyt;
    SReal scale_w = Lzs / Lzt;

    sout << "Scale X = " << scale_u << "Scale Y = "<< scale_v << "Scale Z = " << scale_w << sendl;

    sout << "Lxs = " <<Lxs<< "Lys = " <<Lys<< "Lzs = " <<Lzs << sendl;
    sout << "Lxt = " <<Lxt<< "Lyt = " <<Lyt<< "Lzt = " <<Lzt << sendl;
    /*Creation of the two 4x4 transformation matrix:
     *
     *MTransformSource :
     *
     *U.x V.x W.x tx
     *U.y V.y W.Y ty
     *U.z V.z W.z tz
     * 0   0   0   1
     *
     *
     *Then inverse R = MtransformSource [0..2][0..2]
     *t = -R*t
     *
     * with U,V and W the eigenvectors of the source inertia matrix in the world coordinates.
     *And tx,ty,tz represents the translation beetween the center of the world and the inertia center of the object S
     *
     *
     *MTransformTarget :
     *
     *U'.x V'.x W'.x tx
     *U'.y V'.y W'.Y ty
     *U'.z V'.z W'.z tz
     *  0    0    0   1
     *

     *
     * with U',V' and W' the eigenvectors of the target inertia matrix in the world coordinates.
     *And tx,ty,tz represents the translation beetween the center of the world and the inertia center of the object T
     *
     *
     */

    Eigen::Matrix3d MRotationSource, MRotationTarget;
    defaulttype::Matrix4 MTransformSource,MTransformTarget,MTransformTargetTest,MTransform,Mscale;
    MRotationSource =  eigenvectorsSource;
    MRotationTarget =  eigenvectorsTarget;
    Eigen::Vector3d u,v,w;
    SReal sdirect;
    Mscale(0,0)=scale_u;
    Mscale(1,1)=scale_v;
    Mscale(2,2)=scale_w;
    Mscale(3,3)=1;
    for(unsigned int i=0;i<3;i++)
    {
        u(i) = MRotationSource(i,0);
        v(i) = MRotationSource(i,1);
        w(i) = MRotationSource(i,2);

    }
    sdirect = u.cross(v).dot(w);

    for(unsigned int i=0;i<3;i++)
    {
        if(sdirect < 0)
        {
            MRotationSource(i,2) = -MRotationSource(i,2);
        }
    }

    sout <<"Source Directe? "<< sdirect << sendl;

    for(unsigned int i=0;i<3;i++)
    {
        u(i) = MRotationTarget(i,0);
        v(i) = MRotationTarget(i,1);
        w(i) = MRotationTarget(i,2);

    }

    //Checks if the axes are direct or not
    sdirect = u.cross(v).dot(w);

    for(unsigned int i=0;i<3;i++)
    {
        if(sdirect == -1)
        {
            MRotationTarget(i,2) = -MRotationTarget(i,2);
        }
    }

    sout <<"Target Directe? "<< sdirect << sendl;

    //Normalised last line
    MTransformTarget(3,3) = 1;

    for(unsigned int i=0;i<3;i++)
    {
        //TODO : vérifier ces histoires de Lxs/Lxt

        MTransformTarget(i,0) = MRotationTarget(i,0)*Lxt;
        MTransformTarget(i,1) = MRotationTarget(i,1)*Lyt;
        MTransformTarget(i,2) = MRotationTarget(i,2)*Lzt;
        MTransformTarget(3,i) = 0;

    }
    Eigen::Vector3d TargetTransform;
    for(unsigned int i=0;i<3;i++)
    {
        TargetTransform(i)= -(*targetC.beginEdit())[0];
    }
    MTransformTarget(0,3) = -(*targetC.beginEdit())[0];
    MTransformTarget(1,3) = -(*targetC.beginEdit())[1];
    MTransformTarget(2,3) = -(*targetC.beginEdit())[2];

    //Construction of S
    MTransformSource(3,3) = 1;
    Vector4 TranslationSource;

    MTransformSource(0,3) = -(*sourceC.beginEdit())[0];
    MTransformSource(1,3) = -(*sourceC.beginEdit())[1];
    MTransformSource(2,3) = -(*sourceC.beginEdit())[2];

    for(unsigned int i=0;i<3;i++)
    {
        //TODO : vérifier ces histoires de Lxs/Lxt

        MTransformSource(i,0) = MRotationSource(i,0) *Lxs;
        MTransformSource(i,1) = MRotationSource(i,1) *Lys;
        MTransformSource(i,2) = MRotationSource(i,2) *Lzs;
        MTransformSource(3,i) = 0;
        TranslationSource(i)  = MTransformSource(i,3);

    }

    sout << "MTransformSource before inversion ="<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout <<MTransformSource(i,j)<< " ";
        }
        sout << sendl;
    }

    MTransformSource = inverseTransform(MTransformSource);
    sout << "MTransformSource after inversion ="<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout <<MTransformSource(i,j)<< " ";
        }
        sout << sendl;
    }
    defaulttype::Matrix4 MTranslation;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            MTranslation(i,j)=0;
            if(i==j)
                MTranslation(i,i) = 1;
        }
    }

    int indice_min =0;
    SReal distance, distance_min;

    distance = std::numeric_limits<SReal>::max();
    distance_min = std::numeric_limits<SReal>::max();

    for (unsigned int i=0;i<4;i++)
        for (unsigned int j=0;j<4;j++)
            MTransformTargetTest(i,j) = MTransformTarget(i,j);

    for(unsigned int i=0;i<4;i++)//Test of the 4 possible permutations
    {
        switch (i)
        {
        case 0 :
            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) = MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) = MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) = MTransformTarget(j,2) ;
            }
            break;
        case 1 :

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) = MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) =-MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) =-MTransformTarget(j,2) ;
            }
            break;
        case 2 :

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) =-MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) = MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) =-MTransformTarget(j,2) ;
            }
            break;
        case 3:

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) =-MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) =-MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) = MTransformTarget(j,2) ;

            }
            break;

        }
        MTransform = MTransformTargetTest * MTransformSource ;

        positionDistSource = (*m_positions.beginEdit());
        for (size_t j = 0; j < waPositions.size(); j++)
        {
            defaulttype::Vector4 pointS,pointT;
            pointS(0) = (*m_positions.beginEdit())[j][0];
            pointS(1) = (*m_positions.beginEdit())[j][1];
            pointS(2) = (*m_positions.beginEdit())[j][2];
            pointS(3) = 1;
            pointT = MTransform * pointS  ;
            (*m_positions.beginEdit())[j][0] =pointT(0);
            (*m_positions.beginEdit())[j][1] =pointT(1);
            (*m_positions.beginEdit())[j][2] =pointT(2);
        }

        distance =computeDistances(*m_positions.beginEdit(),*m_positiont.beginEdit());
        if (distance < distance_min)
        {
            indice_min = i;
            distance_min = distance;
        }
        (*m_positions.beginEdit()) = positionDistSource;
    }
    sout << "Indice choisi : " << indice_min << " avec une distance de " << distance_min << sendl;
    //Compute the best transformation
    switch(indice_min)
    {
    case 0 ://Nothing is inverted
        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) = MTransformTarget(j,0) ;
            MTransformTarget(j,1) = MTransformTarget(j,1) ;
            MTransformTarget(j,2) = MTransformTarget(j,2) ;
        }
        break;
    case 1 ://v and w are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) = MTransformTarget(j,0) ;
            MTransformTarget(j,1) =-MTransformTarget(j,1) ;
            MTransformTarget(j,2) =-MTransformTarget(j,2) ;
        }
        break;
    case 2 ://u and w are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) =-MTransformTarget(j,0) ;
            MTransformTarget(j,1) = MTransformTarget(j,1) ;
            MTransformTarget(j,2) =-MTransformTarget(j,2) ;
        }
        break;
    case 3:// u and v are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) =-MTransformTarget(j,0) ;
            MTransformTarget(j,1) =-MTransformTarget(j,1) ;
            MTransformTarget(j,2) = MTransformTarget(j,2) ;
        }
        break;
    }

    for(unsigned int k=0;k<4;k++)
    {
       for(unsigned int j=0;j<4;j++)
       {
         sout << MTransformTarget(k,j) << " ";
       }
       sout << sendl;
    }
    MTransformSource = MTransformTarget * MTransformSource;

    for(unsigned int k=0;k<4;k++)
    {
       for(unsigned int j=0;j<4;j++)
       {
         sout << MTransformSource(k,j) << " ";
       }
       sout << sendl;
    }

    Eigen::Matrix4d MDeterminantTest;
    for(unsigned int k=0;k<3;k++)
    {
       for(unsigned int j=0;j<3;j++)
       {
           MDeterminantTest(k,j)=MTransformSource(k,j);
       }
    }
    if(MDeterminantTest.determinant()<0)
        sout << "The MTransformSourceMatrix is not a Transformation matrix" << sendl;
    for (size_t i = 0; i < waPositions.size(); i++)
    {
        defaulttype::Vector4 pointS,pointT;
        pointS(0) = (*m_positions.beginEdit())[i][0];
        pointS(1) = (*m_positions.beginEdit())[i][1];
        pointS(2) = (*m_positions.beginEdit())[i][2];
        pointS(3) = 1;
        pointT = MTransformSource * pointS  ;
        (*m_positions.beginEdit())[i][0] =pointT(0);
        (*m_positions.beginEdit())[i][1] =pointT(1);
        (*m_positions.beginEdit())[i][2] =pointT(2);
    }
    //After this step, this data cannot be modified
    m_positions.endEdit();

    m_positiont.endEdit();

}

//Hausdorff distance
//good approx but slow

/**
 * Compute the distance from a point to a point cloud
 */

SReal InertiaAlign::distance(sofa::defaulttype::Vec<3,SReal> p, helper::vector<sofa::defaulttype::Vec<3,SReal> > S)
{
    SReal min = std::numeric_limits<SReal>::max();

    for (unsigned int i = 0 ; i < S.size(); i++)
    {
        SReal d = (p-S[i]).norm();
        if (d<min) min = d;
    }

    return min;
}

/**
 * Compute distances between both point clouds (symmetrical and non-symmetrical distances)
 */
SReal InertiaAlign::computeDistances( helper::vector<sofa::defaulttype::Vec<3,SReal> > S, helper::vector<sofa::defaulttype::Vec<3,SReal> > T)
{
    SReal maxST = 0.0;
    for (unsigned int i = 0 ; i < S.size(); i++)
    {
        SReal d = InertiaAlign::distance(S[i], T);
        if (d>maxST) maxST = d;
    }

    SReal maxTS = 0.0;
    for (unsigned int i = 0 ; i < T.size(); i++)
    {
        SReal d = InertiaAlign::distance(T[i], S);
        if (d>maxTS) maxTS = d;
    }

    if (maxTS > maxST)
        return maxST/S.size();
    else
        return maxTS/S.size();

}

Matrix4 InertiaAlign::inverseTransform(Matrix4 transformToInvert)
{
    Matrix3 rotationToInvert;
    Matrix3 rotationInverted;
    Vector3 Translation;
    Matrix4 transformInverted;

    sout << "Before inversion"<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout << transformToInvert(i,j)<< " ";
        }
        sout << sendl;
    }
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            rotationToInvert(i,j) = transformToInvert(i,j);
        }
        Translation(i)=transformToInvert(i,3);
    }

    bool bS = invertMatrix(rotationInverted,rotationToInvert);

    sout << "Translation = " << Translation;
    if(!bS)
    {
        sout <<"Error : Source transformation matrix is not invertible"<<sendl;
    }
    else //Compute R-1 * t
    {
        Translation = (-1*rotationInverted * Translation);
    }
    sout << "Translation = " << Translation;
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            transformInverted(i,j) = rotationInverted(i,j);
        }
        transformInverted(i,3)= Translation(i);
    }
    transformInverted(3,3)=1;

    sout << "After inversion"<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout << transformInverted(i,j)<< " ";
        }
        sout << sendl;
    }
    return transformInverted;

}

SReal InertiaAlign::abs(SReal a)
{
    return sqrt(a*a);
}

} // namespace component

} // namespace sofa


#include "sbs/physics/xpbd/green_constraint.h"

#include <Eigen/LU>
#include <Eigen/SVD>
#include <sbs/physics/simulation.h>

namespace sbs {
namespace physics {
namespace xpbd {

green_constraint_t::green_constraint_t(
    scalar_type const alpha,
    scalar_type const beta,
    simulation_t const& simulation,
    index_type bi,
    index_type v1,
    index_type v2,
    index_type v3,
    index_type v4,
    scalar_type young_modulus,
    scalar_type poisson_ratio)
    : constraint_t(alpha, beta),
      bi_(bi),
      v1_(v1),
      v2_(v2),
      v3_(v3),
      v4_(v4),
      DmInv_(),
      V0_(),
      mu_(),
      lambda_()
{
    auto const& p1 = simulation.particles()[bi_][v1_];
    auto const& p2 = simulation.particles()[bi_][v2_];
    auto const& p3 = simulation.particles()[bi_][v3_];
    auto const& p4 = simulation.particles()[bi_][v4_];

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1.x0() - p4.x0()).transpose();
    Dm.col(1) = (p2.x0() - p4.x0()).transpose();
    Dm.col(2) = (p3.x0() - p4.x0()).transpose();

    DmInv_  = Dm.inverse();
    V0_     = (1. / 6.) * Dm.determinant();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

void green_constraint_t::project_positions(simulation_t& simulation, scalar_type dt)
{
    auto& p1 = simulation.particles()[bi_][v1_];
    auto& p2 = simulation.particles()[bi_][v2_];
    auto& p3 = simulation.particles()[bi_][v3_];
    auto& p4 = simulation.particles()[bi_][v4_];

    scalar_type const w1 = p1.invmass();
    scalar_type const w2 = p2.invmass();
    scalar_type const w3 = p3.invmass();
    scalar_type const w4 = p4.invmass();

    auto const Vsigned        = signed_volume(p1.xi(), p2.xi(), p3.xi(), p4.xi());
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);

    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1.xi() - p4.xi());
    Ds.col(1) = (p2.xi() - p4.xi());
    Ds.col(2) = (p3.xi() - p4.xi());

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    Eigen::Matrix3d const Piola = U * Piolahat * V.transpose();

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const dt2         = dt * dt;
    scalar_type const alpha_tilde = alpha() / dt2;
    scalar_type const beta_tilde  = beta() * dt2;
    scalar_type const gamma       = alpha_tilde * beta_tilde / dt;

    // clang-format off
    scalar_type const gradC_dot_displacement =
        f1.dot(p1.xi() - p1.xn()) +
        f2.dot(p2.xi() - p2.xn()) +
        f3.dot(p3.xi() - p3.xn()) +
        f4.dot(p4.xi() - p4.xn());
    // clang-format on

    scalar_type const delta_lagrange_num =
        -(C + alpha_tilde * lagrange_) + gamma * gradC_dot_displacement;
    scalar_type const delta_lagrange_den = (1. + gamma) * (weighted_sum_of_gradients) + alpha_tilde;
    scalar_type const delta_lagrange     = delta_lagrange_num / delta_lagrange_den;

    lagrange_ += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p1.xi() += w1 * -f1 * delta_lagrange;
    p2.xi() += w2 * -f2 * delta_lagrange;
    p3.xi() += w3 * -f3 * delta_lagrange;
    p4.xi() += w4 * -f4 * delta_lagrange;
}

scalar_type green_constraint_t::signed_volume(
    Eigen::Vector3d const& p1,
    Eigen::Vector3d const& p2,
    Eigen::Vector3d const& p3,
    Eigen::Vector3d const& p4) const
{
    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4);
    Dm.col(1) = (p2 - p4);
    Dm.col(2) = (p3 - p4);

    scalar_type const V = (1. / 6.) * Dm.determinant();
    return V;
}

} // namespace xpbd
} // namespace physics
} // namespace sbs


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef META_MATH_SQRT_INCLUDE
#define META_MATH_SQRT_INCLUDE

#include <boost/numeric/meta_math/abs.hpp>

namespace meta_math {

template <long int root, long int x>
struct sqrt_check
{
    static bool const value = root * root <= x && (root+1) * (root+1) > x;
};

namespace impl {

    template <long int guess, long int x, bool Converged>
    struct sqrt_impl
    {
	typedef long int type;
	typedef sqrt_impl   self;
	static long int const quotient = x / guess,
	                      new_value = (quotient + guess) / 2;
	static bool const converging = abs<guess - quotient>::value < 2;
	static long int const value = sqrt_impl<new_value, x, converging>::value;
    };

    // If the condition becomes true the guessed root will be the returned value
    template <long int guess, long int x>
    struct sqrt_impl<guess, x, true>
    {
	static long int const value = guess;
    };

}

template <long int x>
struct sqrt
{
  typedef long int type;
  static long int const value = impl::sqrt_impl<1, x, false>::value;
};

} // namespace meta_math

#endif // META_MATH_SQRT_INCLUDE


#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <time.h>
#include <math.h>
#include <dirent.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iomanip>
#include <limits>
#include <string>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/math/distributions/normal.hpp>

#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/flann/flann.hpp"

#include "FastEMD/emd_hat.hpp"
#include "FastEMD/emd_hat_signatures_interface.hpp"
#include "RubnerEMD/emd.hpp"
#include "FastEMD/tictoc.hpp"

using namespace cv;
using namespace std;

namespace fs = boost::filesystem;

// TODO: config
/* CONFIGURATIONS */
bool use_fast_emd = 1;
float gd_dist = 350;
/* END CONFIGURATIONS */

void help();
void print_progress(time_t start, int total, int& completed, int per_cout);
void print_keypoints(const vector<KeyPoint>& kp);

float calc_emd(int first, int second);
double calc_fast_emd(int first, int second);
float calc_scalar_manitude(const Mat& desc, int index);
void calc_contrast(const vector<KeyPoint>& kp);
void calc_dist_mat(const Mat& desc_1, const Mat& desc_2, bool gd = false);
float euclid_dist(const float *v1, const float *v2, int dim1, int dim2);
float emd_dist(feature_t *f1, feature_t *f2);
double fast_emd_dist(feature_tt *f1, feature_tt *f2);

Mat dist_mat;
vector<Mat> img_descriptors;
vector<vector<KeyPoint> > img_keypoints;
vector<string> img_paths;

int main(int argc, char *argv[]) {
	if (argc < 4) {
		help();
		return -1;
	}

	const char *index_path = argv[1];
	const char *queries_path = argv[2];
	const char *out_path = argv[3];
	if (argc > 5)
		gd_dist = atof(argv[5]);

	cout << "Reading queries from " << queries_path << endl;
	vector<string> query_paths;
	string query_path;
	int query_count;
	ifstream queries_file(queries_path);
	queries_file >> query_count;
	for (int i = 0; i < query_count; ++i) {
		queries_file >> query_path;
		query_paths.push_back(query_path);
	}

	cout << "Reading index from " << index_path << endl;
	FileStorage fs(index_path, FileStorage::READ);
	FileNode imgs = fs["index"];
	FileNodeIterator it;
	int img_count = 0;
	vector<pair<int, int> > query_ids;
	for (it = imgs.begin(); it != imgs.end(); ++it, ++img_count) {
		string img_path;
		vector<KeyPoint> img_keypoint;
		Mat img_descriptor;

		(*it)["path"] >> img_path;
		read((*it)["keypoints"], img_keypoint);
		(*it)["descriptors"] >> img_descriptor;

		img_paths.push_back(img_path);
		img_keypoints.push_back(img_keypoint);
		img_descriptors.push_back(img_descriptor);

		for (int i = 0; i < query_count; ++i) {
			if (img_path == query_paths[i])
				query_ids.push_back(pair<int, int>(img_count, i));
		}
	}

	query_count = query_ids.size();

	cout << "Calcing emds..." << endl;

	int total_process = query_count * img_count;
	int completed_process = 0;
	time_t start = time(0);

	string img_path_1, img_path_2;
	int query_id;

	ofstream ofile(out_path);

	tictoc timer;
	timer.tic();
	// calculate emd distances of each pair of images
	for (int i = 0; i < query_count; ++i) {
		query_id = query_ids[i].first;
		img_path_1 = query_paths[query_ids[i].second];

		for (int j = 0; j < img_count; ++j) {
			if (query_id == j)
				continue;

			img_path_2 = img_paths[j];

			double emd_dist;
			if (use_fast_emd)
				emd_dist = calc_fast_emd(query_id, j);
			else
				emd_dist = calc_emd(query_id, j);

			// cout << setprecision(3) << fixed;
			// timer.clear();
			// timer.tic();
			// emd_dist = calc_fast_emd(i, j);
			// timer.toc();
			// cout << "fast: " << timer.totalTimeSec() << endl;
			// cout << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			// timer.clear();
			// timer.tic();
			// emd_dist = calc_emd(i, j);
			// timer.toc();
			// cout << "rubner: " << timer.totalTimeSec() << endl;
			// cout << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			ofile << setprecision(3) << fixed;
			ofile << img_path_1 << " " << img_path_2 << " " << emd_dist << endl;

			// print_progress(start, total_process, completed_process, 20);
		}
	}
	timer.toc();
	cout << "Time in seconds: " << timer.totalTimeSec() << endl;

	ofile.close();

	return 0;
}

void help() {
	cout << "Usage: ./feature-selection <index_path> <queries_path> <out_path> <fast_thresh=350>" << endl;
}

void print_progress(time_t start, int total, int& completed, int per_count) {
	++completed;
	if (completed % per_count != 0)
		return;
	int remaining_process = total - completed;
	time_t current = time(0);
	float elapsed_time = difftime(current, start);
	float remaining_time = elapsed_time * remaining_process / completed;
	cout << fixed << setprecision(3);
	cout << "Remaining time: ";
	cout << remaining_time / 60 << "min" << endl;
}

void print_keypoints(const vector<KeyPoint>& kp) {
	KeyPoint k;
	for (int i = 0; i < (int) kp.size(); ++i) {
		k = kp[i];
		cout << "(" << k.pt.x << "," << k.pt.y << "): " << k.response << ", " << k.size << endl;
	}
}

float calc_emd(int first, int second) {
	vector<KeyPoint> *keypoints_1, *keypoints_2;
	Mat *desc_1, *desc_2;

	keypoints_1 = &img_keypoints[first];
	desc_1 = &img_descriptors[first];

	keypoints_2 = &img_keypoints[second];
	desc_2 = &img_descriptors[second];

	calc_dist_mat(*desc_1, *desc_2);

	int feature_count_1 = keypoints_1->size();
	int feature_count_2 = keypoints_2->size();

	signature_t signature_1;
	signature_t signature_2;

	signature_1.n = feature_count_1;
	signature_2.n = feature_count_2;

	signature_1.Features = new feature_t[feature_count_1];
	signature_2.Features = new feature_t[feature_count_2];

	signature_1.Weights = new float[feature_count_1];
	signature_2.Weights = new float[feature_count_2];

	for (int i = 0; i < feature_count_1; ++i) {
		signature_1.Weights[i] = calc_scalar_manitude(*desc_1, i);
		signature_1.Features[i] = i;
	}

	for (int i = 0; i < feature_count_2; ++i) {
		signature_2.Weights[i] = calc_scalar_manitude(*desc_2, i);
		signature_2.Features[i] = i;
	}

	float dist = emd(&signature_1, &signature_2, emd_dist, 0, 0);

	delete[] signature_1.Features;
	delete[] signature_1.Weights;
	delete[] signature_2.Features;
	delete[] signature_2.Weights;

	// cout << "EMD: " << dist << endl;

	return dist;
}

double calc_fast_emd(int first, int second) {
	vector<KeyPoint> *keypoints_1, *keypoints_2;
	Mat *desc_1, *desc_2;

	keypoints_1 = &img_keypoints[first];
	desc_1 = &img_descriptors[first];

	keypoints_2 = &img_keypoints[second];
	desc_2 = &img_descriptors[second];

	calc_dist_mat(*desc_1, *desc_2, true);

	int feature_count_1 = keypoints_1->size();
	int feature_count_2 = keypoints_2->size();

	signature_tt<double> signature_1;
	signature_tt<double> signature_2;

	signature_1.n = feature_count_1;
	signature_2.n = feature_count_2;

	signature_1.Features = new feature_tt[feature_count_1];
	signature_2.Features = new feature_tt[feature_count_2];

	signature_1.Weights = new double[feature_count_1];
	signature_2.Weights = new double[feature_count_2];

	for (int i = 0; i < feature_count_1; ++i) {
		signature_1.Weights[i] = calc_scalar_manitude(*desc_1, i);
		signature_1.Features[i] = i;
	}

	for (int i = 0; i < feature_count_2; ++i) {
		signature_2.Weights[i] = calc_scalar_manitude(*desc_2, i);
		signature_2.Features[i] = i;
	}

	double dist = emd_hat_signature_interface<double>(&signature_1, &signature_2, fast_emd_dist, -1);

	delete[] signature_1.Features;
	delete[] signature_1.Weights;
	delete[] signature_2.Features;
	delete[] signature_2.Weights;

	// cout << "EMD: " << dist << endl;

	return dist / 1000;
}

float calc_scalar_manitude(const Mat& desc, int index) {
	const float* d = desc.ptr<float>(index);
	int cols = desc.cols;
	float magnitude = 0;
	for (int i = 0; i < cols; ++i)
		magnitude += d[i] * d[i];
	return sqrt(magnitude);
}

void calc_dist_mat(const Mat& desc_1, const Mat& desc_2, bool gd) {
	float dist;
	int rows = desc_1.rows;
	int cols = desc_2.rows;
	int dim1 = desc_1.cols;
	int dim2 = desc_2.cols;
	dist_mat = Mat::zeros(rows, cols, CV_32F);
	for (int i = 0; i < rows; ++i) {
		float *row = dist_mat.ptr<float>(i);
		const float *v1 = desc_1.ptr<float>(i);
		for (int j = 0; j < cols; ++j) {
			const float *v2 = desc_2.ptr<float>(j);
			dist = euclid_dist(v1, v2, dim1, dim2);
			if (gd)
				row[j] = gd_dist < dist ? gd_dist : dist;
			else
				row[j] = dist;
		}
	}
}

float euclid_dist(const float *v1, const float *v2, int dim1, int dim2) {
	int max_dim = max(dim1, dim2);
	float d1, d2;
	float dist = 0;
	for (int i = 0; i < max_dim; ++i) {
		if (i < dim1)
			d1 = v1[i];
		else
			d1 = 0;

		if (i < dim2)
			d2 = v2[i];
		else
			d2 = 0;

		dist += pow(d1 - d2, 2);
	}
	return sqrt(dist);
}

float emd_dist(feature_t *f1, feature_t *f2) {
	float dist = dist_mat.at<float>(*f1, *f2);
	return dist;
}

double fast_emd_dist(feature_tt *f1, feature_tt *f2) {
	double dist = dist_mat.at<float>(*f1, *f2);
	return dist;
}


#include <iostream>
#include <armadillo>
#include <vector>
#include <string>
#include "fringe_topofit.hpp"

int main(int argc, char **argv)
{

    std::vector<std::string> files(4);
    files[0] = "test1.dat";
    files[1] = "test2.dat";
    files[2] = "test3.dat";

    std::complex<float> cJ(0.0, 1.0);

    for(int ii=0; ii<3; ii++)
    {
        arma::fvec alldata;
        alldata.load(files[ii], arma::raw_binary);

        int nifg = (alldata.size() - 4)/2;
        arma::fvec bperp(alldata.memptr() + 4, nifg, false);
        arma::fvec ph(alldata.memptr()+nifg+4, nifg, false);

        arma::cx_fvec cph(nifg);
        arma::cx_fvec resid(nifg);

        for(int kk=0; kk<nifg; kk++)
            cph[kk] = std::exp(cJ * ph[kk]);

        float wvl = alldata[0];
        float rng = alldata[1];
        float inc = alldata[2];
        float delz = alldata[3];

        float Kmod, Cph;

        topofit worker(20.0, 0.1, wvl, nifg);

        float coh = worker.fit(cph.memptr(), bperp.memptr(), rng, inc, resid.memptr(), Kmod, Cph);
        std::cout << "File: " << files[ii] << " K: " << Kmod
                << " C: " << Cph << " coh: " << coh
                << " K_true: " << delz/(wvl*rng*sin(inc*M_PI/180.0)/4.0/M_PI)<< "\n";
    }
}


#include "Tensor.h"
#include "AnisGeodesic.h"
#include <iostream>

#include <Eigen/Dense>
using namespace Eigen;

using namespace GeoProperty;

void Tensor::makeTensor(const Vec3& d1,const Vec3& d2,Tensor &t)
{
	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned i = 0; i < 2; i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = t.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;

	t.mag1 = abs(egval[ind1]); t.mag2 = abs(egval[ind2]);
	t.dir1 = egvec(ind1, 0) * d1 + egvec(ind1, 1) * d2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * d1 + egvec(ind2, 1) * d2; t.dir2.normalize();
}

void Tensor::averageTensor(Tensor &t1,Tensor &t2,Tensor &t)
{
	double rat1= 0.5;
	double rat2= 0.5;

	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	Vec2 v1_,v2_;
	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);

	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned i = 0; i < 2; i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = rat1*t1.mat[i][j] + rat2*t2.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) > abs(egval[1])) //////////////////////////////
		swap(ind1, ind2);

	double nl = (abs(t1.mag1) + abs(t1.mag2) + abs(t2.mag1) + abs(t2.mag2));
	if (nl > 1e-10)
		nl /= (2.0*(abs(egval[ind1]) + abs(egval[ind2])));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(std::vector<Tensor> &ts,Tensor &t, double rat)
{
	std::vector<double> rats(ts.size(),rat/double(ts.size()-1)); rats[0]=1-rat;
	Tensor& t1=ts.front();
	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;
	if(ts.size()==1) {t=t1; return;}

	Vec2 v1_,v2_;
	for(unsigned i=1;i<ts.size();i++)
	{
		Tensor& t2=ts[i];
		v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
		v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);
		t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
		t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
		t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;
	}

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for(unsigned i=0;i<2;i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			for (unsigned k = 0; k < ts.size(); k++)
			{
				hesMat(i, j) += rats[k] * ts[k].mat[i][j];
			}
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) < abs(egval[1]))
		swap(ind1, ind2);

	double nl = 0;
	for (unsigned i = 0; i<ts.size(); i++)
		nl += rats[i] * abs(ts[i].mag1) + abs(ts[i].mag2);
	if (nl>1e-10)
		nl /= ts.size()*(abs(egval[ind1]) + abs(egval[ind2]));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(Tensor &t1,Tensor &t2,Tensor &t, double rat)
{
	double rat1= 1-rat;

	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	Vec2 v1_,v2_;
	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);

	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for(unsigned i=0;i<2;i++)
	{
		for (unsigned j = 0; j < 2; j++)
		{
			hesMat(i, j) = rat1*t1.mat[i][j] + rat*t2.mat[i][j];
		}
	}

	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if ((abs(egval[0]) < abs(egval[1])) && (t1.mag1 >= t1.mag2) || (abs(egval[0]) > abs(egval[1])) && (t1.mag1<t1.mag2))
		swap(ind1, ind2);

	double nl = (abs(t1.mag1) + abs(t1.mag2) + abs(t2.mag1) + abs(t2.mag2));
	if (nl>1e-10)
		nl /= (2.0*(abs(egval[ind1]) + abs(egval[ind2])));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::averageTensor(std::vector<Tensor> &ts,Tensor &t)
{
	if (ts.empty()) return;

	Tensor& t1=ts.front();
	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
	t1.mat[0][1]=t1.mat[1][0]=0;

	if(ts.size()==1)
	{
		t=t1; return;
	}

	for(unsigned i=1;i<ts.size();i++)
	{
		Tensor& t2=ts[i];

		Vec2 v1(t2.dir1.dot(t1.dir1), t2.dir1.dot(t1.dir2));
		Vec2 v2(t2.dir2.dot(t1.dir1), t2.dir2.dot(t1.dir2));

		t2.mat[0][0] = t2.mag1*v1.x*v1.x + t2.mag2*v2.x*v2.x;
		t2.mat[0][1] = t2.mat[1][0] = t2.mag1*v1.x*v1.y + t2.mag2*v2.x*v2.y;
		t2.mat[1][1] = t2.mag1*v1.y*v1.y + t2.mag2*v2.y*v2.y;
	}

	Matrix2d hesMat = Eigen::Matrix2d::Constant(2, 2, 0.0);
	for (unsigned k = 0; k < ts.size(); k++)
	{
		hesMat(0, 0) += ts[k].mat[0][0];
		hesMat(0, 1) += ts[k].mat[0][1];
		hesMat(1, 0) += ts[k].mat[1][0];
		hesMat(1, 1) += ts[k].mat[1][1];
	}
	SelfAdjointEigenSolver<Matrix2d> vd(hesMat);
	Vector2d egval = vd.eigenvalues();
	Matrix2d egvec = vd.eigenvectors();

	unsigned ind1 = 0, ind2 = 1;
	if (abs(egval[0]) < abs(egval[1]))
		swap(ind1, ind2);

	double nl = 0;
	for (unsigned i = 0; i<ts.size(); i++)
		nl += abs(ts[i].mag1) + abs(ts[i].mag2);
	if (nl>1e-10)
		nl /= ts.size()*(abs(egval[ind1]) + abs(egval[ind2]));
	else
		nl = 1;

	t.mag1 = egval[ind1] * nl; t.mag2 = egval[ind2] * nl; //how to normalized it is very important;
	t.dir1 = egvec(ind1, 0) * t1.dir1 + egvec(ind1, 1) * t1.dir2; t.dir1.normalize();
	t.dir2 = egvec(ind2, 0) * t1.dir1 + egvec(ind2, 1) * t1.dir2; t.dir2.normalize();
}

void Tensor::computeTriangleCurvature(MyMesh* orgMesh,std::vector<Tensor>& faceTensor)
{
	/*
	int fNum = orgMesh->getFaces().size();
	faceTensor.clear(); faceTensor.resize(fNum);
	const auto& fts = orgMesh->getFIter();
#pragma omp parallel for
	for(int i=0;i<fNum;i++){
		auto f_it = fts[i];
		MyMesh::VertexIter v[3]={f_it->vertex_iter(0),f_it->vertex_iter(1),f_it->vertex_iter(2)};

		// local coord system for single triangle;
		Vec3 axix = v[1]->coordinate()- v[0]->coordinate(); axix.normalize();
		Vec3 axiy = v[2]->coordinate()- v[0]->coordinate();
		Vec3 norm = axix.cross(axiy); norm.normalize();
		axiy = norm.cross(axix); axiy.normalize();

		std::vector<Tensor> ts(3);
		for(unsigned i=0;i<3;i++){
			ts[i].dir1 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(0));
			ts[i].dir2 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(1));
			ts[i].mag1 = v[0]->magnitude(0);
			ts[i].mag2 = v[0]->magnitude(1);
		}

		Tensor t;
		averageTensor(ts,t);
		faceTensor[i] =t;
/*
	//second method;  which seems wrong, but whose result is close to method 1;
		double anis[2];
		Jacobi jcb(2);
		for(unsigned i=0;i<2;i++){
			std::vector<std::vector<double> > mat(2,std::vector<double>(2,0));
			for(unsigned j=0;j<3;j++){
				Vec3 projVec = v_its[j]->direction(i); projVec.normalize();
				projVec -= projVec.dot(norm) * norm; projVec.normalize();

				double x = v_its[j]->direction(i).dot(u);
				double y = v_its[j]->direction(i).dot(v);

				mat[0][0] += v_its[j]->magnitude(i)*x*x;
				mat[0][1] += v_its[j]->magnitude(i)*x*y;
				mat[1][1] += v_its[j]->magnitude(i)*y*y;
			}
			mat[1][0] = mat[0][1];

			jcb.setMatrix(mat);
			jcb.run();

			std::vector<double> egvalue = jcb.getEigenvalues();

			std::vector<std::vector<double> > egvector = jcb.getEigenvectors();
			unsigned ind=0;
			if(egvalue[0]<egvalue[1]) ind=1 ;
			Vec3 td = u*egvector[ind][0] + v*egvector[ind][1]; td.normalize();

			anis[i]= *std::max_element(egvalue.begin(),egvalue.end());
		}
		faceAnis.push_back(abs(anis[1]-anis[0]));
*/
//	}

	faceTensor.clear();
	Tensor t;
	for(auto f_it = orgMesh->getFaces().begin(); f_it!=orgMesh->getFaces().end(); f_it++)
	{
		MyMesh::VertexIter v[3]={f_it->vertex_iter(0),f_it->vertex_iter(1),f_it->vertex_iter(2)};

		// local coord system for single triangle;
		Vec3 axix = v[1]->coordinate()- v[0]->coordinate(); axix.normalize();
		Vec3 axiy = v[2]->coordinate()- v[0]->coordinate();
		Vec3 norm = axix.cross(axiy); norm.normalize();
		axiy = norm.cross(axix); axiy.normalize();

		std::vector<Tensor> ts(3);
		for(unsigned i=0;i<3;i++)
		{
			ts[i].dir1 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(0));
			ts[i].dir2 = basicNormalTransport(v[i]->normal(),norm,v[i]->direction(1));
			ts[i].mag1 = v[i]->magnitude(0);
			ts[i].mag2 = v[i]->magnitude(1);
		}

		averageTensor(ts,t);
		faceTensor.push_back(t);
	}
}

Tensor GeoProperty::lineSearchHessian(Vec3& v, Tensor& tensor)
{
	double x = v.dot(tensor.dir1);
	double y = v.dot(tensor.dir2);

	/*
	double denom = pow(x,2)*tensor.mag1+pow(y,2)*tensor.mag2;
	double denomSqrt = sqrt(denom);
	double upscale = 1e5;

	tensor.mat[0][0]=tensor.mag1;
	tensor.mat[1][1]=tensor.mag2;
	tensor.mat[0][1]=tensor.mat[1][0]=0;
	Tensor res;
	for (int i = 0; i < 2; i++){
	for (int j = 0; j < 2; j++){
	double t1 = 0.0 , t2 = 0.0;
	for (int k = 0; k < 2; k++){
	t1 += tensor.mat[i][k] * vecProj[k];
	t2 += tensor.mat[j][k] * vecProj[k];
	}
	res.mat[i][j] = (0.5 * tensor.mat[i][j] * upscale) / (denomSqrt * upscale) -
	(0.5 * t1 * upscale * t2) / (denom * upscale * denomSqrt);
	}
	}
	*/

	//make it as a metric tensor

	double a = tensor.mag1;
	double d = tensor.mag2;

	double dis = a*x*x + d*y*y;
	double dis23 = sqrt(dis) * dis;

	Tensor res;
	if (dis23>1e-8)
	{
		res.mat[0][0] = a / dis - a*a*x*x / dis23;
		res.mat[0][1] = -a*d*x*y / dis23;
		res.mat[1][0] = res.mat[0][1];
		res.mat[1][1] = d / dis - d*d*y*y / dis23;
		Tensor::makeTensor(tensor.dir1, tensor.dir2, res);
	}
	return res;
}

// double Tensor::compareTensors(Tensor& t1, Tensor& t2, std::vector<Vec2> vs)
// {
// 	//compute the difference of two tensors.... it is hard to define the difference..
// /*
// 	U1 = {{a1, b1}, {c1, d1}} ;
// 	U2 = {{a2, b2}, {c2, d2}};
// 	V = {Cos[#], Sin[#]} &;
// 	Sum[Power[Sqrt[V[x].U1.V[x]] - Sqrt[V[x].U2.V[x]], 2], {x, 0, Pi,
// 		Pi/360}]
// */
// 	t1.mat[0][0]=t1.mag1; t1.mat[1][1]=t1.mag2;
// 	t1.mat[0][1]=t1.mat[1][0]=0;
//
// 	Vec2 v1_,v2_;
// 	v1_.x=t2.dir1.dot(t1.dir1); v1_.y=t2.dir1.dot(t1.dir2);
// 	v2_.x=t2.dir2.dot(t1.dir1); v2_.y=t2.dir2.dot(t1.dir2);
// 	t2.mat[0][0]= t2.mag1*v1_.x*v1_.x+t2.mag2*v2_.x*v2_.x;
// 	t2.mat[0][1]= t2.mat[1][0]= t2.mag1*v1_.x*v1_.y+t2.mag2*v2_.x*v2_.y;
// 	t2.mat[1][1]= t2.mag1*v1_.y*v1_.y+t2.mag2*v2_.y*v2_.y;
//
// 	//Sum[Power[Sqrt[V[x].U1.V[x]] - Sqrt[V[x].U2.V[x]], 2]
// 	double ds=0;
// 	for(unsigned i=0;i<vs.size();i++)
// 	{
// 		ds+= pow(
// 			sqrt(abs((vs[i].x*t1.mat[0][0] + vs[i].y*t1.mat[1][0])*vs[i].x + (vs[i].x*t1.mat[0][1] + vs[i].y*t1.mat[1][1])*vs[i].y)) -
// 			sqrt(abs((vs[i].x*t2.mat[0][0] + vs[i].y*t2.mat[1][0])*vs[i].x + (vs[i].x*t2.mat[0][1] + vs[i].y*t2.mat[1][1])*vs[i].y))
// 			,2);
// 	}
// /*
// 	for(unsigned i=0;i<vs.size();i++){
// 		ds+= abs(
// 			sqrt(abs((vs[i].x*t1.mat[0][0] + vs[i].y*t1.mat[1][0])*vs[i].x + (vs[i].x*t1.mat[0][1] + vs[i].y*t1.mat[1][1])*vs[i].y)) -
// 			sqrt(abs((vs[i].x*t2.mat[0][0] + vs[i].y*t2.mat[1][0])*vs[i].x + (vs[i].x*t2.mat[0][1] + vs[i].y*t2.mat[1][1])*vs[i].y))
// 			);
// 	}
// */
// 	return ds;
// }


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <cassert>
#include <time.h>
#include <queue>

//#include <boost/heap/fibonacci_heap.hpp>

#include "CRS.h"

using namespace std;

CRS data;
bool* relaxed;
unsigned* totalDist;

class ComparePair {
public:
  bool operator()(pair<unsigned,unsigned> n1, pair<unsigned,unsigned> n2) {
    if(n1.second > n2.second)
      return true;
    else
      return false;
  }
};
priority_queue<pair<unsigned, unsigned>, vector<pair<unsigned, unsigned> >, ComparePair> work;

double abs(double val) {
  if(val < 0)
    return -1.0 * val;
  else
    return val;
}

unsigned min(unsigned a, unsigned b) {
  return (a < b) ? a : b;
}

int main(int argc, char** argv) {

  if((argc != 3) && (argc != 5)) {
    cout << "ERROR: incorrect input parameters!\n";
    cout << argv[0] << " <input file name> <source vertex>\n-- OR --\n";
    cout << argv[0] << " <input file name> <source vertex> -out <output file name>" << endl;
    exit(1);
  }

  ifstream in(argv[1]);
  unsigned source = atoi(argv[2]);
  bool genOutput = false;
  ofstream out(argv[4]);
  if(argc == 5) {
    genOutput = true;
  }

  if(!in.is_open()) {
    cout << "ERROR: Can't open file " << argv[1] << endl;
    exit(1);
  }
  cout << "Running on " << argv[1] << " with source vertex " << source << endl;
  time_t t1, t2, t3;
  t1 = time(NULL);

  // Get vertices and edges
  unsigned numVertices, numEdges;
  in >> numVertices >> ws >> numEdges >> ws;

  totalDist = new unsigned[numVertices];
  relaxed = new bool[numVertices];
  for(int i = 0; i < numVertices; i++) {
    relaxed[i] = false;
    totalDist[i] = -1;
  }
  cout << "EdgeDist initialized to " << totalDist[0] << endl;
  totalDist[source] = 0;
  work.push(make_pair(source, 0));

  int src, dest, weight;
  data.row->resize(numVertices);
  while(in.good()) {
    in >> src >> ws >> dest >> ws >> weight >> ws;
    //cout << "---" << src << " " << dest << " " << weight << endl;
    data.add(src, dest, weight);
  }
  //data.print();
  t2 = time(NULL);
  cout << "Number of rows: " << data.row->size() << endl;

  cout << "Running Dijkstra's Algorithm\n";
  while(!work.empty()) {
    pair<unsigned, unsigned> top = work.top();
    work.pop();

    //cout << "Top: " << top.first << ", " << top.second << endl;

    int vertex = top.first;
    if(relaxed[vertex])
      continue;

    // Check for disconnected graphs!
    if(totalDist[vertex] == -1) {
      cout << "Disconnected graph detected, break!\n";
      break;
    }

    relaxed[vertex] = true;

    deque<pair<unsigned, unsigned> >* row = data.getRow(vertex);
    if(row != NULL) {
      for(deque<pair<unsigned,unsigned> >::iterator it = row->begin(); it!=row->end(); it++) {
	if(!relaxed[it->first]) {
	  // update distance
	  int dist = totalDist[vertex]+it->second;
	  if(dist < totalDist[it->first]) {
	    totalDist[it->first] = dist;
	    //cout << "  Checking " << it->first << ", minDist = " << minDist << endl;
	    work.push(make_pair(it->first, dist));
	  }
	}
      }
    }
  }

  t3 = time(NULL);

  cout << "Setup time: " << (t2 - t1) << " seconds\n";
  cout << "Dijkstra SSSP time: " << (t3 - t2) << " seconds\n";

  if(genOutput) {
    for(int i = 0; i < numVertices; i++) {
      out <<  i << "," << totalDist[i] << endl;
    }
  }
}


/**
 * Copyright (c) 2011, 2012
 * Claudio Kopper <claudio.kopper@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id: I3CLSimVectorTransformMatrix.cxx 108199 2013-07-12 21:33:08Z nwhitehorn $
 *
 * @file I3CLSimVectorTransformMatrix.cxx
 * @version $Revision: 108199 $
 * @date $Date: 2013-07-12 15:33:08 -0600 (Fri, 12 Jul 2013) $
 * @author Claudio Kopper
 */

#include <icetray/serialization.h>
#include <clsim/function/I3CLSimVectorTransformMatrix.h>

#include <typeinfo>
#include <cmath>
#include <math.h>
#include <sstream>
#include <stdexcept>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include "clsim/I3CLSimHelperToFloatString.h"
using namespace I3CLSimHelper;

I3CLSimVectorTransformMatrix::
I3CLSimVectorTransformMatrix(
    const I3Matrix &matrix,
    bool renormalize)
:
matrix_(matrix),
renormalize_(renormalize)
{
    if ((matrix_.size1() != 3) || (matrix_.size2() != 3))
        throw std::range_error("matrix must be 3x3!");
}

I3CLSimVectorTransformMatrix::I3CLSimVectorTransformMatrix() {;}

I3CLSimVectorTransformMatrix::~I3CLSimVectorTransformMatrix()
{;}

bool I3CLSimVectorTransformMatrix::HasNativeImplementation() const
{
    return true;
}

std::vector<double> I3CLSimVectorTransformMatrix::ApplyTransform(const std::vector<double> &vec) const
{
    if (vec.size() != 3)
        throw std::range_error("vector must contain excatly 3 elements!");

    boost::numeric::ublas::vector<double> uvec_in(3);
    std::copy(vec.begin(), vec.end(), uvec_in.begin());
    boost::numeric::ublas::vector<double> uvec_out(3);

    boost::numeric::ublas::noalias(uvec_out) = boost::numeric::ublas::prod(matrix_, uvec_in);

    std::vector<double> out_vec(3, NAN);

    double norm=1.;

    if (renormalize_) {
        norm=0.;
        for (std::size_t i=0;i<3;++i)
        {
            norm += (uvec_out(i)*uvec_out(i));
        }
        norm = std::sqrt(norm);

        for (std::size_t i=0;i<3;++i)
        {
            out_vec[i] = uvec_out(i)/norm;
        }
    } else {
        for (std::size_t i=0;i<3;++i)
        {
            out_vec[i] = uvec_out(i);
        }
    }

    return out_vec;
}

std::string I3CLSimVectorTransformMatrix::GetOpenCLFunction(const std::string &functionName) const
{
    // the OpenCL interface takes a pointer to a float4, but ignores the fourth component

    std::string funcDef =
    std::string("inline void ") + functionName + std::string("(float4 *vec)");

    std::string funcBody = std::string() +
    "{\n";

    funcBody = funcBody +
    "    *vec = (float4)\n"
    "    (\n"
    "        (" + ToFloatString(matrix_(0,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(0,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(0,2)) + "*(*vec).z),\n"
    "        (" + ToFloatString(matrix_(1,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(1,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(1,2)) + "*(*vec).z),\n"
    "        (" + ToFloatString(matrix_(2,0)) + "*(*vec).x)+(" + ToFloatString(matrix_(2,1)) + "*(*vec).y)+(" + ToFloatString(matrix_(2,2)) + "*(*vec).z),\n"
    "        (*vec).w\n"
    "    );\n";

    if (renormalize_) {
        funcBody = funcBody +
        "#ifdef USE_NATIVE_MATH\n"
        "    (*vec).xyz = fast_normalize((*vec).xyz);\n"
        "#else\n"
        "    const float norm = rsqrt((*vec).x*(*vec).x + (*vec).y*(*vec).y + (*vec).z*(*vec).z);\n"
        "    (*vec).xyz = (*vec).xyz*norm;\n"
        "#endif\n";
    }

    funcBody = funcBody +
    "}\n";

    return funcDef + ";\n\n" + funcDef + "\n" + funcBody;
}

bool I3CLSimVectorTransformMatrix::CompareTo(const I3CLSimVectorTransform &other) const
{
    try
    {
        const I3CLSimVectorTransformMatrix &other_ = dynamic_cast<const I3CLSimVectorTransformMatrix &>(other);

        if ((other_.matrix_.size1() != matrix_.size1()))
            return false;
        if ((other_.matrix_.size2() != matrix_.size2()))
            return false;
        for (std::size_t i=0;i<matrix_.size1();++i)
            for (std::size_t j=0;j<matrix_.size2();++j)
            {
                if ((other_.matrix_(i,j) != matrix_(i,j)))
                    return false;
            }

        if ((other_.renormalize_ != renormalize_))
            return false;

        return true;
    }
    catch (const std::bad_cast& e)
    {
        // not of the same type, treat it as non-equal
        return false;
    }

}

template <class Archive>
void I3CLSimVectorTransformMatrix::serialize(Archive &ar, unsigned version)
{
    if (version>i3clsimvectortransformmatrix_version_)
        log_fatal("Attempting to read version %u from file but running version %u of I3CLSimVectorTransformMatrix class.",version,i3clsimvectortransformmatrix_version_);

    ar & make_nvp("I3CLSimVectorTransform", base_object<I3CLSimVectorTransform>(*this));
    ar & make_nvp("matrix", matrix_);
    ar & make_nvp("renormalize", renormalize_);
}

I3_SERIALIZABLE(I3CLSimVectorTransformMatrix);


#pragma once
#include <Eigen/Eigen>

class XXZ
{
private:
	int n_;
	double J_;
	double Delta_;

public:
	XXZ(int n, double J, double Delta)
		: n_(n), J_(J), Delta_(Delta)
	{
	}

	template<class State>
	typename State::T operator()(const State& smp) const
	{
		typename State::T s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			int zz = smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += J_*Delta_*zz; //zz
			s += J_*(1-zz)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		return s;
	}

	std::vector<std::array<int,2> > offDiagonals(const Eigen::VectorXi& s) const
	{
		std::vector< std::array<int, 2> > res;
		for(int i = 0; i < n_; i++)
		{
			if( s(i) != s((i+1)%n_) )
				res.push_back(std::array<int,2>{ i,(i+1)%n_ });
		}
		return res;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int zz = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col] += J_*Delta_*zz;
			m[col ^ x] += J_*(1 - zz);
		}
		return m;
	}
};


/***************************************************************************
   Copyright 2015 Ufora Inc.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****************************************************************************/
#pragma once

#include <boost/random.hpp>
#include "../lassert.hpp"

namespace Ufora {
namespace math {
namespace Random {

template<class scalar_type>
class Normal {
public:
		Normal(uint32_t inSeed = 1u) :
				mSampler(mGen, mNormal)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		T operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::normal_distribution<scalar_type> >::result_type result_type;

		result_type min() const
			{
			return mSampler.min();
			}

		result_type max() const
			{
			return mSampler.max();
			}
private:
		boost::normal_distribution<scalar_type> mNormal;

		boost::mt19937 mGen;

	    boost::variate_generator<boost::mt19937&, boost::normal_distribution<scalar_type> > mSampler;
};

template<class scalar_type,
	bool = std::is_integral<scalar_type>::value,
	bool = std::is_floating_point<scalar_type>::value>
class Uniform;

template<class scalar_type>
class Uniform<scalar_type, false, true> {
public:
		Uniform(uint32_t inSeed = 1u) :
				mSampler(mGen, mUniform)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		scalar_type operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::uniform_real<scalar_type> >::result_type result_type;

		result_type min()
			{
			return mSampler.min();
			}

		result_type max()
			{
			return mSampler.max();
			}
private:
		boost::uniform_real<scalar_type> mUniform;

		boost::mt19937 mGen;

		boost::variate_generator<boost::mt19937&, boost::uniform_real<scalar_type> > mSampler;
};

template<class scalar_type>
class Uniform<scalar_type, true, false> {
public:
		Uniform(uint32_t inSeed = 1u) :
				mSampler(mGen, mUniform)
			{
			mGen.seed(boost::uint32_t(inSeed));
			}

		scalar_type	operator()(void)
			{
			return mSampler();
			}

		template<class T>
		scalar_type operator()(T value)
			{
			return mSampler(value);
			}

		typedef typename boost::variate_generator<boost::mt19937&, boost::uniform_int<scalar_type> >::result_type result_type;

		result_type min()
			{
			return mSampler.min();
			}

		result_type max()
			{
			return mSampler.max();
			}
private:
		boost::uniform_int<scalar_type> mUniform;

		boost::mt19937 mGen;

	    boost::variate_generator<boost::mt19937&, boost::uniform_int<scalar_type> > mSampler;
};

template<class T, class scalar_type>
decltype(*T().begin()) pickRandomlyFromSet(
								const T& values,
								Uniform<scalar_type>& random
								)
	{
	int which = values.size();

	while (which >= values.size())
		which = random() * values.size();

	auto it = values.begin();

	while (which)
		{
		it++;
		which--;
		}

	return *it;
	}

template<class T, class scalar_type, class weight_function_type>
decltype(*T().begin()) pickRandomlyFromSetWithWeight(
								const T& values,
								Uniform<scalar_type>& random,
								weight_function_type weightFunction
								)
	{
	double totalWeight = 0;

	for (auto val: values)
		totalWeight += weightFunction(val);

	lassert(totalWeight > 0.0);

	double w = totalWeight * random();

	for (auto val: values)
		if (weightFunction(val) >= w)
			return val;
		else
			w -= weightFunction(val);

	lassert(false);
	}

}; //namespace Random
}; //namespace math
}; //namespace Ufora


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_face_normals.h"
#include <Eigen/Geometry>

#define SQRT_ONE_OVER_THREE 0.57735026918962573
template <typename DerivedV, typename DerivedF, typename DerivedZ, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  using namespace Eigen;
  Matrix<typename DerivedN::Scalar,3,1> Z(0,0,0);
  return per_face_normals(V,F,Z,N);
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals_stable(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  using namespace Eigen;
  typedef Matrix<typename DerivedV::Scalar,1,3> RowVectorV3;
  typedef typename DerivedV::Scalar Scalar;

  const size_t m = F.rows();

  N.resize(F.rows(),3);
  // Grad all points
  for(size_t f = 0;f<m;f++)
  {
    const RowVectorV3 p0 = V.row(F(f,0));
    const RowVectorV3 p1 = V.row(F(f,1));
    const RowVectorV3 p2 = V.row(F(f,2));
    const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0);
    const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1);
    const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2);

    // careful sum
    for(int d = 0;d<3;d++)
    {
      // This is a little _silly_ in terms of complexity, but its recursive
      // implementation is clean looking...
      const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 =
        [&sum3](Scalar a, Scalar b, Scalar c)->Scalar
      {
        if(fabs(c)>fabs(a))
        {
          return sum3(c,b,a);
        }
        // c < a
        if(fabs(c)>fabs(b))
        {
          return sum3(a,c,b);
        }
        // c < a, c < b
        if(fabs(b)>fabs(a))
        {
          return sum3(b,a,c);
        }
        return (a+b)+c;
      };

      N(f,d) = sum3(n0(d),n1(d),n2(d));
    }
    // sum better not be sure, or else NaN
    N.row(f) /= N.row(f).norm();
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// Nonsense template
namespace igl{template<> void per_face_normals<Eigen::Matrix<double, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&){} }
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 1, 3, 1, 1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 1, -1, -1>, Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1>, Eigen::Matrix<float, -1, -1, 1, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<class Eigen::Matrix<double,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<double,-1,-1,0,-1,-1> >(class Eigen::MatrixBase<class Eigen::Matrix<double,-1,3,0,-1,3> > const &,class Eigen::MatrixBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,-1,0,-1,-1> > &);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 3, 0, 2, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 2, 3, 0, 2, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
#endif


/* plantcalc -- Power plant modelling
 * (c) 2012 Michał Górny
 * Released under the terms of the 2-clause BSD license
 */

#ifdef HAVE_CONFIG_H
#	include "config.h"
#endif

#include "linearequationsolver.hxx"
#include "../equations/linearequation.hxx"
#include "../exceptions/contradictionerror.hxx"
#include "../variable.hxx"

#include <algorithm>
#include <cassert>
#include <cmath>
#include <map>
#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/LU>

namespace
{
	class col_index
	{
		int _i;

	public:
		col_index()
			: _i(-1)
		{
		}

		void operator=(int x)
		{
			_i = x;
		}

		operator int() const
		{
			return _i;
		}
	};
}

LinearEquationSolver::LinearEquationSolver(EquationSystem& eqs, double epsilon)
	: _eqs(eqs), _epsilon(epsilon)
{
}

bool LinearEquationSolver::iterate()
{
	EquationSystem& eqs = _eqs;
	typedef std::vector<LinearEquation*> linear_eq_list;
	linear_eq_list lineqs;

	// collect linear equations
	for (EquationSystem::iterator it = eqs.begin(); it != eqs.end(); ++it)
	{
		LinearEquation* e = dynamic_cast<LinearEquation*>(*it);

		if (e)
			lineqs.push_back(e);
	}

	const int num_eqs = lineqs.size();
	if (num_eqs == 0)
		return false;

	int curr_var_index = 0;

	typedef std::map<Variable*, col_index> variable_map;
	variable_map varmap;

	// gather variable counts
	linear_eq_list::iterator it;
	for (it = lineqs.begin(); it != lineqs.end();)
	{
		LinearEquation& e = **it;
		LinearEquation::list_type::iterator jt;

		bool accepted = true;

		for (jt = e._vars.begin(); jt != e._vars.end(); ++jt)
		{
			LinearEquation::list_elem_type& cv = *jt;

			Variable* unk_var = 0;

			if (!cv.variable1->is_set())
				unk_var = cv.variable1;

			if (cv.variable2 && !cv.variable2->is_set())
			{
				// if there's no usable coefficient, drop that eq
				if (unk_var)
				{
					it = lineqs.erase(it);
					accepted = false;
					break;
				}
				unk_var = cv.variable2;
			}

			if (unk_var)
			{
				col_index& col = varmap[unk_var];

				if (col == -1)
					col = curr_var_index++;
			}
		}

		if (accepted)
			++it;
	}

	const int num_vars = varmap.size();
	Eigen::MatrixXd coeff_matrix
		= Eigen::MatrixXd::Zero(num_eqs + 1, num_vars);
	Eigen::VectorXd value_vector(num_eqs + 1);

	int row;
	for (it = lineqs.begin(), row = 0; it != lineqs.end(); ++it)
	{
		LinearEquation& e = **it;
		LinearEquation::list_type::iterator jt;

		double val = 0;

		for (jt = e._vars.begin(); jt != e._vars.end(); ++jt)
		{
			LinearEquation::list_elem_type& cv = *jt;

			Variable* unk_var = 0;
			double coeff = cv.coefficient;

			if (!cv.variable1->is_set())
				unk_var = cv.variable1;
			else
				coeff *= *cv.variable1;

			if (cv.variable2)
			{
				if (!cv.variable2->is_set())
				{
					assert(!unk_var);
					unk_var = cv.variable2;
				}
				else
					coeff *= *cv.variable2;
			}

			if (!unk_var)
				val -= coeff;
			else
			{
				col_index& col = varmap[unk_var];
				assert(col < num_vars);

				coeff_matrix(row, col) = coeff;
			}
		}

		value_vector(row) = val;
		++row;
	}

	Eigen::FullPivLU<Eigen::MatrixXd> lu = coeff_matrix.fullPivLu();

	Eigen::VectorXd sol_vector = lu.solve(value_vector);
	Eigen::VectorXd null_vector = lu.kernel().rowwise().sum();

	double error = (coeff_matrix * sol_vector - value_vector).norm();

	if (std::abs(error) > _epsilon)
		throw ContradictionError();

	std::vector<bool> keep_equations(num_eqs, false);

	variable_map::iterator vt;
	for (vt = varmap.begin(); vt != varmap.end(); ++vt)
	{
		Variable& v = *(*vt).first;
		col_index& col = (*vt).second;

		if (null_vector(col) > 0.5)
		{
			// find all equations with the variable and remove them
			// so they will remain in the original system of equations

			for (int i = 0; i < num_eqs; ++i)
			{
				if (std::abs(coeff_matrix(i, col)) > _epsilon)
					keep_equations[i] = true;
			}

			continue;
		}

		v.set_value(sol_vector(col));
	}

	// remove the solved equations
	// they should be in the same order in both vectors
	EquationSystem::iterator et = eqs.begin();
	for (linear_eq_list::size_type i = 0; i < lineqs.size(); ++i)
	{
		if (keep_equations[i])
			continue;

		Equation* eq = lineqs[i];

		assert(et != eqs.end());
		while (*et != eq)
		{
			++et;
			assert(et != eqs.end());
		}
		et = eqs.erase(et);
	}

	return false;
}


#ifndef DYN_GCA_INCLUDED
#define DYN_GCA_INCLUDED

#include <boost/multi_array.hpp>

using namespace std;

/**
 * @file   dyn_gca.hpp
 * @brief  ODE system for group SIS with group selection mechanism for transmission rates
 *
 * @author  LHD
 * @since   2021-02-06
 */

struct Sparam {
    const double beta;
    const double gamma;
    const double rho;
    const double b;
    const double c;
    const double mu;
    const int dim1;
    const int dim2;
}; // parameter structure

//********** function dydt definition **************************************************************
int dydt(double t, const double y[], double f[], void * param) {
// ODE system for interacting contagions

    // Cast parameters
    Sparam& p = *static_cast<Sparam* >(param);

    // Create multi_array reference to y and f
    typedef boost::multi_array_ref<const double,2> CSTmatref_type;
    typedef boost::multi_array_ref<double,2> matref_type;
    typedef CSTmatref_type::index indexref;
    CSTmatref_type yref(y,boost::extents[p.dim1][p.dim2]);
    matref_type fref(f,boost::extents[p.dim1][p.dim2]);

    // Calculate mean-field coupling and observed fitness landscape
    vector<double> Zvec(p.dim1, 0.0); vector<double> popvec(p.dim1, 0.0); double R=0.0;
    for(int l=0; l<p.dim1; ++l) {
        for(int i=0; i<p.dim2; ++i) {
            Zvec[l] += exp(p.b*i-p.c*l)*yref[l][i]; //should this really be proportional to C_i,l?
            popvec[l] += yref[l][i];
            R += p.rho*i*yref[l][i];
        }
        if(popvec[l]>0.0) Zvec[l] /= popvec[l];
    }
    double Z = accumulate(Zvec.begin(), Zvec.end(), 0.0);
    int n=p.dim2-1;
    int L=p.dim1-1;

    // Compute derivatives
    for(int l=0; l<p.dim1; ++l) {
        for(int i=0; i<p.dim2; ++i) {
            fref[l][i] = -1.0*p.gamma*i*yref[l][i] - p.beta*l*(i+R)*(n-i)*yref[l][i];
            if(i>0) fref[l][i] += p.beta*l*((i-1)+R)*(n-i+1)*yref[l][i-1];
            if(i<n) fref[l][i] += p.gamma*(i+1)*yref[l][i+1];
            if(l>0) fref[l][i] += p.rho*yref[l-1][i]*(Zvec[l]/Zvec[l-1]+p.mu) - p.rho*yref[l][i]*(Zvec[l-1]/Zvec[l]+p.mu);
            if(l<L) fref[l][i] += p.rho*yref[l+1][i]*(Zvec[l]/Zvec[l+1]+p.mu) - p.rho*yref[l][i]*(Zvec[l+1]/Zvec[l]+p.mu);
        }
    }

    return GSL_SUCCESS;

} //********** end function dydt definition ********************************************************

#endif // DYN_GCA_HPP_INCLUDED


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial:  Use of the iterative solvers in ViennaCL with Boost.uBLAS
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// Necessary to obtain a suitable performance in ublas
#ifndef NDEBUG
 #define NDEBUG
#endif

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

using namespace boost::numeric;

int main()
{
  typedef float       ScalarType;

  //
  // Set up some ublas objects
  //
  ublas::vector<ScalarType> rhs;
  ublas::vector<ScalarType> rhs2;
  ublas::vector<ScalarType> ref_result;
  ublas::vector<ScalarType> result;
  ublas::compressed_matrix<ScalarType> ublas_matrix;

  //
  // Read system from file
  //
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return 0;
  }
  //std::cout << "done reading matrix" << std::endl;

  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }
  //std::cout << "done reading rhs" << std::endl;

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ref_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }
  //std::cout << "done reading result" << std::endl;

  //
  // set up ILUT preconditioners for ViennaCL and ublas objects. Other preconditioners can also be used (see manual)
  //
  viennacl::linalg::ilut_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  viennacl::linalg::ilu0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          ublas_block_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());

  //
  // Conjugate gradient solver:
  //
  std::cout << "----- CG Test -----" << std::endl;

  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag());
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilut);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_ilu0);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::cg_tag(1e-6, 20), ublas_block_ilu0);
  std::cout << "Residual norm: " << norm_2(prod(ublas_matrix, result) - rhs) << std::endl;

  //
  // Stabilized BiConjugate gradient solver:
  //
  std::cout << "----- BiCGStab Test -----" << std::endl;

  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag());          //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilut); //with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::bicgstab_tag(1e-6, 20), ublas_ilu0); //with preconditioner

  //
  // GMRES solver:
  //
  std::cout << "----- GMRES Test -----" << std::endl;

  //
  // for ublas objects:
  //
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag());   //without preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilut);//with preconditioner
  result = viennacl::linalg::solve(ublas_matrix, rhs, viennacl::linalg::gmres_tag(1e-6, 20), ublas_ilu0);//with preconditioner

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return 0;
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_HYPOT_INCLUDED
#define BOOST_MATH_HYPOT_INCLUDED

#include <cmath>
#include <boost/limits.hpp>
#include <algorithm> // swap

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; }
#endif

namespace boost{ namespace math{

template <class T>
T hypot(T x, T y)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   //
   // normalize x and y, so that both are positive and x >= y:
   //
   x = (std::fabs)(x);
   y = (std::fabs)(y);

   // special case, see C99 Annex F:
   if(std::numeric_limits<T>::has_infinity
      && ((x == std::numeric_limits<T>::infinity())
      || (y == std::numeric_limits<T>::infinity())))
      return std::numeric_limits<T>::infinity();

   if(y > x)
      (std::swap)(x, y);
   //
   // figure out overflow and underflow limits:
   //
   T safe_upper = (std::sqrt)((std::numeric_limits<T>::max)()) / 2;
   T safe_lower = (std::sqrt)((std::numeric_limits<T>::min)());
   static const T one = 1;
   //
   // Now handle special cases:
   //
   if(x >= safe_upper)
   {
      if(y <= one)
      {
         // y is neligible:
         return x;
      }
      return (std::sqrt)(x) * (std::sqrt)(y) * (std::sqrt)(x/y + y/x);
   }
   else if(y <= safe_lower)
   {
      if((x >= one) || (y == 0))
      {
         // y is negligible:
         return x;
      }
      return (std::sqrt)(x) * (std::sqrt)(y) * (std::sqrt)(x/y + y/x);
   }
   //
   // If we get here then x^2+y^2 will not overflow or underflow:
   //
   return (std::sqrt)(x*x + y*y);
}

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the y < 0 case.
//  Updated 2015 to use Carlson's latest methods.
//

#ifndef BOOST_MATH_ELLINT_RC_HPP
#define BOOST_MATH_ELLINT_RC_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/constants/constants.hpp>
#include <iostream>

// Carlson's degenerate elliptic integral
// R_C(x, y) = R_F(x, y, y) = 0.5 * \int_{0}^{\infty} (t+x)^{-1/2} (t+y)^{-1} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc_imp(T x, T y, const Policy& pol)
{
    BOOST_MATH_STD_USING

    static const char* function = "boost::math::ellint_rc<%1%>(%1%,%1%)";

    if(x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be non-negative but got %1%", x, pol);
    }
    if(y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must not be zero but got %1%", y, pol);
    }

    // for y < 0, the integral is singular, return Cauchy principal value
    T prefix, result;
    if(y < 0)
    {
        prefix = sqrt(x / (x - y));
        x = x - y;
        y = -y;
    }
    else
       prefix = 1;

    if(x == 0)
    {
       result = constants::half_pi<T>() / sqrt(y);
    }
    else if(x == y)
    {
       result = 1 / sqrt(x);
    }
    else if(y > x)
    {
       result = atan(sqrt((y - x) / x)) / sqrt(y - x);
    }
    else
    {
       if(y / x > 0.5)
       {
          T arg = sqrt((x - y) / x);
          result = (boost::math::log1p(arg) - boost::math::log1p(-arg)) / (2 * sqrt(x - y));
       }
       else
       {
          result = log((sqrt(x) + sqrt(x - y)) / sqrt(y)) / sqrt(x - y);
       }
    }
    return prefix * result;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rc_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y), pol), "boost::math::ellint_rc<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y)
{
   return ellint_rc(x, y, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RC_HPP


// This file is part of Man, a robotic perception, locomotion, and
// team strategy application created by the Northern Bites RoboCup
// team of Bowdoin College in Brunswick, Maine, for the Aldebaran
// Nao robot.
//
// Man is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// Man 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 Lesser Public License for more details.
//
// You should have received a copy of the GNU General Public License
// and the GNU Lesser Public License along with Man.  If not, see
// <http://www.gnu.org/licenses/>.

#include <math.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>              // for lu_factorize
#include <boost/numeric/ublas/io.hpp>              // for cout

#include "nb/CoordFrame.h"

using namespace NBMath;
using namespace CoordFrame4D;

// -------------------- Helper matrix methods --------------------

//TODO: Move all these rot4D, etc into a Coord4D namespace.
// also, make all these use bounded matrices (see NBMatrixMath)
const NBMath::ufmatrix4
CoordFrame4D::rotation4D(const Axis axis,
                         const double angle) {
    NBMath::ufmatrix4 rot = boost::numeric::ublas::identity_matrix <double>(4);

    if (angle == 0.0) { //OPTIMIZAION POINT
        return rot;
    }

    //TODO: Make this one call:
    float sinAngle;
    float cosAngle;
    sincosf(angle, &sinAngle, &cosAngle);

    switch(axis) {
    case X_AXIS:
        rot(Y_AXIS, Y_AXIS) =  cosAngle;
        rot(Y_AXIS, Z_AXIS) = -sinAngle;
        rot(Z_AXIS, Y_AXIS) =  sinAngle;
        rot(Z_AXIS, Z_AXIS) =  cosAngle;
        break;
    case Y_AXIS:
        rot(X_AXIS, X_AXIS) =  cosAngle;
        rot(X_AXIS, Z_AXIS) =  sinAngle;
        rot(Z_AXIS, X_AXIS) = -sinAngle;
        rot(Z_AXIS, Z_AXIS) =  cosAngle;
        break;
    case Z_AXIS:
        rot(X_AXIS, X_AXIS) =  cosAngle;
        rot(X_AXIS, Y_AXIS) = -sinAngle;
        rot(Y_AXIS, X_AXIS) =  sinAngle;
        rot(Y_AXIS, Y_AXIS) =  cosAngle;
        break;
    default:
        break;
    }
    return rot;
}

const NBMath::ufmatrix4 CoordFrame4D::translation4D(const double dx,
                                                    const double dy,
                                                    const double dz) {
    NBMath::ufmatrix4 trans = boost::numeric::ublas::identity_matrix <double>(4);
    trans(X_AXIS, W_AXIS) = dx;
    trans(Y_AXIS, W_AXIS) = dy;
    trans(Z_AXIS, W_AXIS) = dz;
    return trans;
}

const NBMath::ufvector4 CoordFrame4D::vector4D(const double x, const double y,
                                               const double z,
                                               const double w) {
    NBMath::ufvector4 p = boost::numeric::ublas::zero_vector <double> (4);
    p(0) = x;
    p(1) = y;
    p(2) = z;
    p(3) = w;
    return p;
}

/**
 * Returns precalculated Trans[x,y,z].Rotz[wz].Roty[wy].Rotx[wx]
 */
const NBMath::ufmatrix4 CoordFrame4D::get6DTransform(const double x,
                                                     const double y,
                                                     const double z,
                                                     const double wx,
                                                     const double wy,
                                                     const double wz){
    float cwx,cwy,cwz,
        swx,swy,swz;
    sincosf(wx,&swx,&cwx);
    sincosf(wy,&swy,&cwy);
    sincosf(wz,&swz,&cwz);
    NBMath::ufmatrix4 r = boost::numeric::ublas::identity_matrix<double>(4);

    //Row 1
    r(0,0) =cwy*cwz;
    r(0,1) =cwz*swx*swy-cwx*swz;
    r(0,2) =cwx*cwz*swy+swx*swz;
    r(0,3) =x;
    //Row2
    r(1,0) =cwy*swz;
    r(1,1) =cwx*cwz+swx*swy*swz;
    r(1,2) =-cwz*swx+cwx*swy*swz;
    r(1,3) =y;
    //Row3
    r(2,0) =-swy;
    r(2,1) =cwy*swx;
    r(2,2) =cwx*cwy;
    r(2,3) =z;

    return r;
}

const NBMath::ufmatrix4
CoordFrame4D::invertHomogenous(const NBMath::ufmatrix4 source){
    const NBMath::ufmatrix3 Rt = trans(subrange(source,0,3,0,3));
    const NBMath::ufvector3 Rtd = -prod(Rt,
                                        CoordFrame3D::vector3D(source(0,3),
                                                               source(1,3),
                                                               source(2,3)));
    NBMath::ufmatrix4 result =
        boost::numeric::ublas::identity_matrix<double>(4);
    subrange(result,0,3,0,3) = Rt;
    result(0,3) = Rtd(0);
    result(1,3) = Rtd(1);
    result(2,3) = Rtd(2);
    return result;
}