/******************************************************************************
*       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                             *
******************************************************************************/

#include <SofaConstraint/LMConstraintSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/core/behavior/LinearSolver.h>
#include <SofaBaseLinearSolver/FullMatrix.h>
#include <SofaBaseLinearSolver/FullVector.h>
#include <sofa/simulation/AnimateBeginEvent.h>

#include <sofa/defaulttype/Quat.h>

#include <sofa/core/ObjectFactory.h>
#include <sofa/helper/AdvancedTimer.h>

#include <Eigen/LU>
#include <Eigen/QR>
using std::cerr;
using std::endl;

namespace sofa
{

namespace component
{

namespace constraintset
{

using linearsolver::FullVector;
using linearsolver::FullMatrix;

LMConstraintSolver::LMConstraintSolver()
    : constraintAcc( initData( &constraintAcc, false, "constraintAcc", "Constraint the acceleration"))
    , constraintVel( initData( &constraintVel, true, "constraintVel", "Constraint the velocity"))
    , constraintPos( initData( &constraintPos, true, "constraintPos", "Constraint the position"))
    , numIterations( initData( &numIterations, (unsigned int)25, "numIterations", "Number of iterations for Gauss-Seidel when solving the Constraints"))
    , maxError( initData( &maxError, 0.0000001, "maxError", "threshold for the residue of the Gauss-Seidel algorithm"))
    , graphGSError( initData(&graphGSError,"graphGSError","Graph of residuals at each iteration") )
    , traceKineticEnergy( initData( &traceKineticEnergy, false, "traceKineticEnergy", "Trace the evolution of the Kinetic Energy throughout the solution of the system"))
    , graphKineticEnergy( initData(&graphKineticEnergy,"graphKineticEnergy","Graph of the kinetic energy of the system") )
    , LMConstraintVisitor( sofa::core::ExecParams::defaultInstance() )
{
    graphGSError.setGroup("Statistics");
    graphGSError.setWidget("graph");

    graphKineticEnergy.setGroup("Statistics");
    graphKineticEnergy.setWidget("graph");

    traceKineticEnergy.setGroup("Statistics");
    this->f_listening.setValue(true);

	numIterations.setRequired(true);
	maxError.setRequired(true);
}

void LMConstraintSolver::init()
{
	sofa::core::behavior::ConstraintSolver::init();

    helper::vector< core::behavior::BaseConstraintCorrection* > listConstraintCorrection;
    ((simulation::Node*) getContext())->get<core::behavior::BaseConstraintCorrection>(&listConstraintCorrection, core::objectmodel::BaseContext::SearchDown);
    for (unsigned int i=0; i<listConstraintCorrection.size(); ++i)
    {
        core::behavior::BaseMechanicalState* constrainedDof;
        listConstraintCorrection[i]->getContext()->get(constrainedDof);
        if (constrainedDof)
        {
            constraintCorrections.insert(std::make_pair(constrainedDof, listConstraintCorrection[i]));
            listConstraintCorrection[i]->removeConstraintSolver(this);
            listConstraintCorrection[i]->addConstraintSolver(this);
        }
    }

    graphKineticEnergy.setDisplayed(traceKineticEnergy.getValue());
}
void LMConstraintSolver::removeConstraintCorrection(core::behavior::BaseConstraintCorrection * /*s*/)
{
    //TODO: remove the pair containing s from constraintCorrections
}

void LMConstraintSolver::convertSparseToDense(const SparseMatrixEigen& sparseM, MatrixEigen& denseM) const
{
    denseM=MatrixEigen::Zero(sparseM.rows(), sparseM.cols());

    for (int k=0; k<sparseM.outerSize(); ++k)
        for (SparseMatrixEigen::InnerIterator it(sparseM,k); it; ++it)
        {
            denseM(it.row(),it.col()) = it.value();
        }
}

bool LMConstraintSolver::needPriorStatePropagation(core::ConstraintParams::ConstOrder order) const
{
    using core::behavior::BaseLMConstraint;
    bool needPriorPropagation=false;
    {
        helper::vector< BaseLMConstraint* > c;
        this->getContext()->get<BaseLMConstraint>(&c, core::objectmodel::BaseContext::SearchDown);
        for (unsigned int i=0; i<c.size(); ++i)
        {
            if (!c[i]->isCorrectionComputedWithSimulatedDOF(order))
            {
                needPriorPropagation=true;
                if (f_printLog.getValue()) cerr << "Propagating the State because of "<< c[i]->getName() << endl;
                break;
            }
        }
    }
    return needPriorPropagation;
}

bool LMConstraintSolver::prepareStates(const core::ConstraintParams *cparams, MultiVecId id, MultiVecId /*res2*/)
{
    //Get the matrices through mappings
    //************************************************************
    // Update the State of the Mapped dofs                      //
    //************************************************************

//    if (f_printLog.getValue()) cerr << "LMConstraintSolver::prepareStates"<<endl;

    core::MechanicalParams mparams;
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::TRACE_ARGUMENT arg;
#endif
    using core::behavior::BaseLMConstraint ;
    LMConstraintVisitor.clear();

    VecId vid = id.getDefaultId();

    orderState=cparams->constOrder();
    LMConstraintVisitor.setMultiVecId(id);
    if      (orderState==core::ConstraintParams::ACC)
    {
        if (!constraintAcc.getValue()) return false;
        if (needPriorStatePropagation(orderState))
        {
            simulation::MechanicalPropagateDxVisitor propagateState(&mparams, core::VecDerivId(vid), false, false);
            propagateState.execute(this->getContext());
        }
        // calling writeConstraintEquations
        LMConstraintVisitor.setOrder(orderState);
        LMConstraintVisitor.setTags(getTags()).execute(this->getContext());

        if (f_printLog.getValue()) cerr << "LMConstraintSolver::prepareStates for accelerations"<<endl;

        simulation::MechanicalProjectJacobianMatrixVisitor(&mparams).execute(this->getContext());
#ifdef SOFA_DUMP_VISITOR_INFO
        arg.push_back(std::make_pair("Order", "Acceleration"));
#endif
    }
    else if (orderState==core::ConstraintParams::VEL)
    {
        if (!constraintVel.getValue()) return false;
        if (f_printLog.getValue()) cerr << "LMConstraintSolver::prepareStates for velocities"<<endl;
        if (needPriorStatePropagation(orderState))
        {
            simulation::MechanicalPropagateVelocityVisitor propagateState(&mparams, 0.0, core::VecDerivId(vid));
            propagateState.execute(this->getContext());
        }
        else
        {
            //TODO: change ProjectVelocityVisitor to pass the VecId
            simulation::MechanicalProjectVelocityVisitor projectVel(&mparams,this->getContext()->getTime(), core::VecDerivId(vid) );
            projectVel.execute(this->getContext());
        }

        // calling writeConstraintEquations
        LMConstraintVisitor.setOrder(orderState);
        LMConstraintVisitor.setTags(getTags()).execute(this->getContext());

        simulation::MechanicalProjectJacobianMatrixVisitor(&mparams).execute(this->getContext());

#ifdef SOFA_DUMP_VISITOR_INFO
        arg.push_back(std::make_pair("Order", "Velocity"));
#endif

    }
    else
    {
        if (!constraintPos.getValue()) return false;
        if (f_printLog.getValue()) cerr << "LMConstraintSolver::prepareStates for positions"<<endl;

        if (needPriorStatePropagation(orderState))
        {
            simulation::MechanicalPropagatePositionVisitor propagateState(&mparams, 0.0, core::VecCoordId(vid));
            propagateState.execute(this->getContext());
        }
        else
        {
            simulation::MechanicalProjectPositionVisitor projectPos(&mparams,this->getContext()->getTime(), core::VecCoordId(vid));
            projectPos.execute(this->getContext());
        }
        // calling writeConstraintEquations
        LMConstraintVisitor.setOrder(orderState);
        LMConstraintVisitor.setTags(getTags()).execute(this->getContext());

#ifdef SOFA_DUMP_VISITOR_INFO
        arg.push_back(std::make_pair("Order", "Position"));
#endif
    }

    //************************************************************
    // Find the number of constraints                           //
    //************************************************************
    numConstraint=0;
    const helper::vector< BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();
    for (unsigned int mat=0; mat<LMConstraints.size(); ++mat)
    {
        numConstraint += LMConstraints[mat]->getNumConstraint(orderState);
        if (f_printLog.getValue()) cerr << "LMConstraintSolver::prepareStates, constraint " << LMConstraints[mat]->getName()
                    << " between "<< LMConstraints[mat]->getSimulatedMechModel1()->getName()
                    << " and " << LMConstraints[mat]->getSimulatedMechModel2()->getName()
                    <<", add "<<  LMConstraints[mat]->getNumConstraint(orderState)
                    << " constraint for order "<< orderState <<endl;
    }
    if (numConstraint == 0)
    {
        return false; //Nothing to solve
    }

    if      (orderState==core::ConstraintParams::POS)
    {
        sofa::helper::AdvancedTimer::valSet("numConstraintsPosition", numConstraint);
        if (f_printLog.getValue()) cerr << "Applying the constraint on the position"<<endl;
    }
    else if (orderState==core::ConstraintParams::VEL)
    {
        sofa::helper::AdvancedTimer::valSet("numConstraintsVelocity", numConstraint);
        if (f_printLog.getValue()) cerr << "Applying the constraint on the velocity"<<endl;
    }
    else if (orderState==core::ConstraintParams::ACC)
    {
        sofa::helper::AdvancedTimer::valSet("numConstraintsAcceleration", numConstraint);
        if (f_printLog.getValue()) cerr << "Applying the constraint on the acceleration"<<endl;
    }
    else cerr << "Order Not recognized " << orderState << endl;

    setDofs.clear();
    dofUsed.clear();
    invMass_Ltrans.clear();
    return true;
}

bool LMConstraintSolver::buildSystem(const core::ConstraintParams *cParams, MultiVecId id, MultiVecId /*res2*/)
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("SystemCreation");
#endif
//    cerr<<"-----------LMConstraintSolver::buildSystem " <<  endl;

    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem Prepare");
    const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();
    //Informations to build the matrices
    //Dofs to be constrained
    for (unsigned int mat=0; mat<LMConstraints.size(); ++mat)
    {
        sofa::core::behavior::BaseLMConstraint *constraint=LMConstraints[mat];
        if (constraint->getNumConstraint(cParams->constOrder()))
        {
            setDofs.insert(constraint->getSimulatedMechModel1());
            setDofs.insert(constraint->getSimulatedMechModel2());
//            cerr<<"LMConstraintSolver::buildSystem, inserting " << constraint->getSimulatedMechModel1()->getName() << endl;
//            cerr<<"LMConstraintSolver::buildSystem, inserting " << constraint->getSimulatedMechModel2()->getName() << endl;
        }
    }
    for (SetDof::iterator it=setDofs.begin(); it!=setDofs.end();)
    {
        SetDof::iterator currentIt=it;
        ++it;

        if (!(*currentIt)->getContext()->getMass())
        {
//            cerr << "LMConstraintSolver::buildSystem, erase " << (*currentIt)->getName() << endl;
            setDofs.erase(currentIt);
        }
#ifdef SOFA_DUMP_VISITOR_INFO
        else if (sofa::simulation::Visitor::IsExportStateVectorEnabled())
        {
            sofa::simulation::Visitor::printNode("Input_"+(*currentIt)->getName());
            sofa::simulation::Visitor::printVector(*currentIt, id.getDefaultId());
            sofa::simulation::Visitor::printCloseNode("Input_"+(*currentIt)->getName());
        }
#endif
    }
    sofa::helper::AdvancedTimer::stepEnd  ("SolveConstraints "  + id.getName() + " BuildSystem Prepare");

    //Store the indices used in order to speed up the correction propagation
    //************************************************************
    // Build the Right Hand Term
    //************************************************************
    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem C");
    c = VectorEigen::Zero((int)numConstraint);
    buildRightHandTerm(LMConstraints, c, id, orderState);
    sofa::helper::AdvancedTimer::stepEnd("SolveConstraints "  + id.getName() + " BuildSystem C");

    //************************************************************
    // Build M^-1
    //************************************************************
    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem M^-1");
    buildInverseMassMatrices(setDofs, invMassMatrix);
    sofa::helper::AdvancedTimer::stepEnd("SolveConstraints "  + id.getName() + " BuildSystem M^-1");

    //************************************************************
    //Building L
    //************************************************************
    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem L");
    LMatrices.clear();
    //Init matrices to the good size
    for (SetDof::iterator it=setDofs.begin(); it!=setDofs.end(); ++it)
    {
        const core::behavior::BaseMechanicalState* dofs=*it;
        SparseMatrixEigen L(numConstraint,dofs->getSize()*dofs->getDerivDimension());
        L.reserve(numConstraint*(1+dofs->getSize()));//TODO: give a better estimation of non-zero coefficients
        LMatrices.insert(std::make_pair(dofs, L));
    }
    buildLMatrices(orderState, LMConstraints, LMatrices, dofUsed);

    //Remove empty L Matrices
    for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end();)
    {
        SparseMatrixEigen& matrix= it->second;
        matrix.finalize();
        DofToMatrix::iterator itCurrent=it;
        ++it;
        if (!matrix.nonZeros()) //Empty Matrix: act as an obstacle
        {
            invMassMatrix.erase(itCurrent->first);
            setDofs.erase(const_cast<sofa::core::behavior::BaseMechanicalState*>(itCurrent->first));
            LMatrices.erase(itCurrent);
        }

    }
    sofa::helper::AdvancedTimer::stepEnd("SolveConstraints "  + id.getName() + " BuildSystem L");

    //************************************************************
    // Building W=L0.M0^-1.L0^T + L1.M1^-1.L1^T + ... and M^-1.L^T
    //************************************************************
    //Store the matrix M^-1.L^T for each dof in order to apply the modification of the state
    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem W");
    SparseMatrixEigen WEi((int)numConstraint,(int)numConstraint);
    buildLeftMatrix(invMassMatrix, LMatrices,WEi, invMass_Ltrans);
    sofa::helper::AdvancedTimer::stepEnd("SolveConstraints "  + id.getName() + " BuildSystem W");

    sofa::helper::AdvancedTimer::stepBegin("SolveConstraints "  + id.getName() + " BuildSystem conversionW");
    //Convert the Sparse Matrix AEi into a Dense Matrix-> faster access to elements, and possilibity to use a direct LU solution
    convertSparseToDense(WEi,W);

    sofa::helper::AdvancedTimer::stepEnd("SolveConstraints "  + id.getName() + " BuildSystem conversionW");
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("SystemCreation");
#endif
    return true;
}

bool LMConstraintSolver::solveSystem(const core::ConstraintParams* cParams, MultiVecId id, MultiVecId /*res2*/)
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("SystemConstraintSolution");
#endif

    //************************************************************
    // Solving the System using Eigen2
    //************************************************************
    if (f_printLog.getValue())
    {
        cerr << "W= L0.M0^-1.L0^T + L1.M1^-1.L1^T + ...: "<<endl;
        cerr <<"\n" << W << endl;
        cerr << "for a constraint: " << ""<<endl;
        cerr << "\n" << c << endl;
    }

    const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();

    //"Cold" start
    Lambda=VectorEigen::Zero(numConstraint);
    bool solutionFound=solveConstraintSystemUsingGaussSeidel(id, cParams->constOrder(), LMConstraints,
            W, c, Lambda);
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("SystemConstraintSolution");
#endif
    return solutionFound;
}

bool LMConstraintSolver::applyCorrection(const core::ConstraintParams* cparams, MultiVecId id, MultiVecId /*res2*/)
{
    //************************************************************
    // Constraint Correction
    //************************************************************
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("SystemCorrection");
#endif
    //************************************************************
    // Updating the state vectors
    // get the displacement. deltaState = M^-1.L^T.lambda : lambda being the solution of the system
    for (SetDof::iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        bool updateVelocities=!constraintVel.getValue();
        VecId v = id.getId(dofs);
        constraintStateCorrection(v, cparams->constOrder(), updateVelocities,invMass_Ltrans[dofs] , Lambda, dofUsed[dofs], dofs);

#ifdef SOFA_DUMP_VISITOR_INFO
        if (sofa::simulation::Visitor::IsExportStateVectorEnabled())
        {
            sofa::simulation::Visitor::printNode("Output_"+dofs->getName());
            sofa::simulation::Visitor::printVector(dofs, v);
            sofa::simulation::Visitor::printCloseNode("Output_"+dofs->getName());
        }
#endif
    }

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("SystemCorrection");
#endif
    return true;
}

//----------------------------------------------------------------------------------------------//
// Specific method to build the matrices

void LMConstraintSolver::buildLeftMatrix(const DofToMatrix& invMassMatrix, DofToMatrix& LMatrix, SparseMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const
{
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second;
        const SparseMatrixEigen &L=LMatrix[dofs];

        if (f_printLog.getValue())  cerr << "Matrix L for " << dofs->getName() << "\n" << L << endl;
        if (f_printLog.getValue())  cerr << "Matrix M-1 for " << dofs->getName() << "\n" << invMass << endl;

        const SparseMatrixEigen &invM_LTrans=invMass*L.transpose();
        invMass_Ltrans.insert( std::make_pair(dofs,invM_LTrans) );
        LeftMatrix += L*invM_LTrans;
    }
}

void LMConstraintSolver::buildLMatrices( ConstOrder Order,
        const helper::vector< core::behavior::BaseLMConstraint* > &LMConstraints,
        DofToMatrix &LMatrices,
        DofToMask &dofUsed) const
{
    unsigned constraintOffset=0;
    //We Take one by one the constraint, and write their equations in the corresponding matrix L
    for (unsigned int mat=0; mat<LMConstraints.size(); ++mat)
    {
        core::behavior::BaseLMConstraint *constraint=LMConstraints[mat];

        const core::behavior::BaseMechanicalState *dof1=constraint->getSimulatedMechModel1();
        const core::behavior::BaseMechanicalState *dof2=constraint->getSimulatedMechModel2();

        //Get the entries in the Vector of constraints corresponding to the constraint equations
        std::list< unsigned int > equationsUsed;
        constraint->getEquationsUsed(Order, equationsUsed);

        if (equationsUsed.empty()) continue;

        DofToMatrix::iterator itL1=LMatrices.find(dof1);
        if (itL1 != LMatrices.end())
        {
            buildLMatrix(itL1->first, equationsUsed, constraintOffset, itL1->second, dofUsed[dof1]);
        }

        DofToMatrix::iterator itL2=LMatrices.find(dof2);
        if (dof1 != dof2 && itL2 != LMatrices.end())
        {
            buildLMatrix(itL2->first, equationsUsed,constraintOffset, itL2->second,dofUsed[dof2]);
        }
        constraintOffset += equationsUsed.size();
    }
}

void LMConstraintSolver::buildInverseMassMatrices( const SetDof &setDofs, DofToMatrix& invMassMatrices)
{
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const unsigned int dimensionDofs=dofs->getDerivDimension();

        //Verify if the Big M^-1 matrix has not already been computed and stored in memory
        DofToMatrix::iterator mFound = invMassMatrices.find(dofs);
        bool needToConstructMassMatrix = true;
        if (mFound != invMassMatrices.end())
        {
            //WARNING HACK! we should find a way to know when the Mass changed: in some case the number of Dof can be the same, but the mass changed
            if ((int) (dofs->getSize()*dimensionDofs) != mFound->second.rows())
                needToConstructMassMatrix=true;
            else
                needToConstructMassMatrix=false;
        }

        //************************************************************
        //Building M^-1
        //Called only ONCE!
        core::behavior::BaseMass *mass=dofs->getContext()->getMass();

        if (needToConstructMassMatrix)
        {
            SparseMatrixEigen invMass(dofs->getSize()*dimensionDofs, dofs->getSize()*dimensionDofs);
            invMass.reserve(dofs->getSize()*dimensionDofs*dimensionDofs);

            DofToConstraintCorrection::iterator constraintCorrectionFound=constraintCorrections.find(dofs);
            if (constraintCorrectionFound != constraintCorrections.end())
            {
                buildInverseMassMatrix(dofs, constraintCorrectionFound->second, invMass);
            }
            else
            {
                buildInverseMassMatrix(dofs, mass, invMass);
            }
            invMass.finalize();
            //Store the matrix in memory
            if (mFound != invMassMatrices.end())
                invMassMatrices[dofs]=invMass;
            else
                invMassMatrices.insert( std::make_pair(dofs,invMass) );
        }
    }
}

void LMConstraintSolver::buildInverseMassMatrix( const sofa::core::behavior::BaseMechanicalState* /*mstate*/, const core::behavior::BaseConstraintCorrection* constraintCorrection, SparseMatrixEigen& invMass) const
{
    //Get the Full Matrix from the constraint correction
    FullMatrix<SReal> computationInvM;
    constraintCorrection->getComplianceMatrix(&computationInvM);
    //Then convert it into a Sparse Matrix: as it is done only at the init, or when topological changes occur, this should not be a burden for the framerate
    for (Index i=0; i<computationInvM.rowSize(); ++i)
    {
        invMass.startVec(i);
        for (Index j=0; j<computationInvM.colSize(); ++j)
        {
            SReal value=computationInvM.element(i,j);
            if (value != 0)
            {
                invMass.insertBack(i,j)=value;
            }
        }
    }
}

void LMConstraintSolver::buildInverseMassMatrix( const sofa::core::behavior::BaseMechanicalState* mstate, const core::behavior::BaseMass* mass, SparseMatrixEigen& invMass) const
{
    const unsigned int dimensionDofs=mstate->getDerivDimension();
    //Build M, using blocks of [ dimensionDofs x dimensionDofs ] given for each particle
    //computationM is a block corresponding the mass matrix of a particle
    FullMatrix<SReal> computationM(dimensionDofs, dimensionDofs);
    MatrixEigen invMEigen((int)dimensionDofs,(int)dimensionDofs);

    for (size_t i=0; i<mstate->getSize(); ++i)
    {
        mass->getElementMass(i,&computationM);

        //Translate the FullMatrix into a Eigen Matrix to invert it
        MatrixEigen mapMEigen=Eigen::Map<MatrixEigen>(computationM[0],(int)computationM.rowSize(),(int)computationM.colSize());
        invMEigen=mapMEigen.inverse();

        //Store into the sparse matrix the block corresponding to the inverse of the mass matrix of a particle
        for (unsigned int r=0; r<dimensionDofs; ++r)
        {
            invMass.startVec(i*dimensionDofs+r);
            for (unsigned int c=0; c<dimensionDofs; ++c)
            {
                if (invMEigen(r,c) != 0)
                {
                    invMass.insertBack(i*dimensionDofs+r,i*dimensionDofs+c)=invMEigen(r,c);
                }
            }
        }
    }
}

void LMConstraintSolver::buildLMatrix( const sofa::core::behavior::BaseMechanicalState *dof,
        const std::list<unsigned int> &idxEquations, unsigned int constraintOffset,
        SparseMatrixEigen& L, sofa::helper::set< unsigned int > &dofUsed) const
{
    const unsigned int dimensionDofs=dof->getDerivDimension();
    typedef core::behavior::BaseMechanicalState::ConstraintBlock ConstraintBlock;
    //Get blocks of values from the Mechanical States
    std::list< ConstraintBlock > blocks=dof->constraintBlocks( idxEquations );

    //Fill the matrices
    const unsigned int numEquations=idxEquations.size();

    for (unsigned int eq=0; eq<numEquations; ++eq)
    {
        const int idxRow=constraintOffset+eq;

        for (std::list< ConstraintBlock >::const_iterator itBlock=blocks.begin(); itBlock!=blocks.end(); ++itBlock)
        {
            const ConstraintBlock &b=(*itBlock);
            const defaulttype::BaseMatrix &m=b.getMatrix();
            const unsigned int column=b.getColumn()*dimensionDofs;

            for (Index j=0; j<m.colSize(); ++j)
            {
                SReal value=m.element(eq,j);
                if (value!=0)
                {
                    L.insert(idxRow, column+j) = m.element(eq,j);
                    dofUsed.insert((column+j)/dimensionDofs);
                }
            }
        }
    }
    for (std::list< ConstraintBlock >::iterator itBlock=blocks.begin(); itBlock!=blocks.end(); ++itBlock)
    {
        delete itBlock->getMatrix();
    }
}

void LMConstraintSolver::buildRightHandTerm( const helper::vector< core::behavior::BaseLMConstraint* > &LMConstraints, VectorEigen &c,
        MultiVecId /*id*/, ConstOrder Order) const
{

    FullVector<SReal> c_fullvector(c.data(), (Index)c.rows());
    for (unsigned int mat=0; mat<LMConstraints.size(); ++mat)  LMConstraints[mat]->getConstraintViolation(&c_fullvector, Order);
}

bool LMConstraintSolver::solveConstraintSystemUsingGaussSeidel( MultiVecId id, ConstOrder Order,
        const helper::vector< core::behavior::BaseLMConstraint* > &LMConstraints,
        const MatrixEigen &W, const VectorEigen &c, VectorEigen &Lambda)
{
    if (f_printLog.getValue()) cerr << "Using Gauss-Seidel solution"<<endl;
    std::string orderName;
    switch (Order)
    {
    case core::ConstraintParams::ACC :
        orderName="Acceleration";
        break;
    case core::ConstraintParams::VEL :
        orderName="Velocity";
        break;
    case core::ConstraintParams::POS :
    case core::ConstraintParams::POS_AND_VEL :
        orderName="Position";
        break;
    }

    const SReal invNormC=1.0/c.norm();

    helper::vector<double> &vError=(*graphGSError.beginEdit())["Error "+ orderName];

    vError.push_back(c.sum());
    graphGSError.endEdit();

    VectorEigen LambdaPrevious=Lambda;

    //Store the invalid block of the W matrix
    std::set< int > emptyBlock;
    helper::vector< std::pair<MatrixEigen,VectorEigen> > constraintToBlock;
    helper::vector< MatrixEigen> constraintInvWBlock;
    helper::vector< MatrixEigen> constraintWBlock;

    //Preparation Step:
    {
        //Iterate on all the Constraint components
        unsigned int idxConstraint=0;
        for (unsigned int componentConstraint=0; componentConstraint<LMConstraints.size(); ++componentConstraint)
        {
            const sofa::core::behavior::BaseLMConstraint *constraint=LMConstraints[componentConstraint];
            //Get the vector containing all the constraint stored in one component
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(Order);

            unsigned int numConstraintToProcess=0;
            for (unsigned int constraintEntry=0; constraintEntry<constraintOrder.size(); ++constraintEntry, idxConstraint += numConstraintToProcess)
            {
                numConstraintToProcess=constraintOrder[constraintEntry]->getNumConstraint();

                const MatrixEigen &wConstraint=W.block(idxConstraint,idxConstraint,numConstraintToProcess, numConstraintToProcess);
                if (wConstraint.diagonal().isZero(1e-15)) emptyBlock.insert(idxConstraint);
                constraintWBlock.push_back(wConstraint);
                constraintInvWBlock.push_back(wConstraint.inverse());

                const VectorEigen &cb=c.block(idxConstraint         , 0,
                        numConstraintToProcess, 1);
                const MatrixEigen &wb=W.block(idxConstraint         , 0,
                        numConstraintToProcess, W.cols());
                constraintToBlock.push_back(std::make_pair(wb,cb));
            }
        }
    }
    //-- Initialization of X, solution of the system
    bool continueIteration=true;
    bool correctionDone=true;
    unsigned int iteration=0;

    const unsigned int numIterationsGS=numIterations.getValue();
    const unsigned int numLMConstraintComponents=LMConstraints.size();

    for (; iteration < numIterationsGS && continueIteration && correctionDone; ++iteration)
    {
        unsigned int idxConstraint=0;
        VectorEigen LambdaPreviousIteration;
        continueIteration=false;
        correctionDone=false;
        unsigned int idxBlocks=0;
        //Iterate on all the Constraint components

        for (unsigned int componentConstraint=0; componentConstraint<numLMConstraintComponents; ++componentConstraint)
        {
            sofa::core::behavior::BaseLMConstraint *constraint=LMConstraints[componentConstraint];
            //Get the vector containing all the constraint stored in one component
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(Order);

            unsigned int numConstraintToProcess=0;
            for (unsigned int constraintEntry=0; constraintEntry<constraintOrder.size(); ++constraintEntry, idxConstraint += numConstraintToProcess, ++idxBlocks)
            {
                //-------------------------------------
                //Initialize the variables, and store X^(k-1) in previousIteration
                numConstraintToProcess=constraintOrder[constraintEntry]->getNumConstraint();

                //Invalid constraints: due to projective constraints, or constraint expressed for Obstacle objects: we just ignore them
                if (emptyBlock.find(idxConstraint) != emptyBlock.end()) continue;

                LambdaPreviousIteration = Lambda.block(idxConstraint,0,numConstraintToProcess,1);
                //Set to Zero sigma for the current set of constraints
                Lambda.block(idxConstraint,0,numConstraintToProcess,1).setZero();

                const MatrixEigen &Wblock   =constraintWBlock[idxBlocks];
                const MatrixEigen &invWblock=constraintInvWBlock[idxBlocks];

                const std::pair<MatrixEigen,VectorEigen> &blocks=constraintToBlock[idxBlocks];
                const VectorEigen &cb=blocks.second;
                const MatrixEigen &wb=blocks.first;

                //Compute Sigma
                VectorEigen sigma = cb; sigma.noalias() = sigma - (wb * Lambda);

                VectorEigen newLambda; newLambda.noalias() = invWblock*sigma;
                constraint->LagrangeMultiplierEvaluation(Wblock.data(),sigma.data(), newLambda.data(),
                        constraintOrder[constraintEntry]);

                //****************************************************************
                if (!constraintOrder[constraintEntry]->isActive())
                {
                    Lambda.block(idxConstraint,0,numConstraintToProcess,1).setZero();
                    if (f_printLog.getValue())
                        cerr <<"["<< iteration << "/" << numIterations.getValue() <<"] ###Deactivated###" << (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum()  << " for system n°" << idxConstraint << "/" << W.cols() << " of " << numConstraintToProcess << " equations "
                                << " between " << constraint->getSimulatedMechModel1()->getName() << " and " << constraint->getSimulatedMechModel2()->getName() << ""<<endl;
                    correctionDone |=    ( (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() != 0);
                }
                else
                {
                    Lambda.block(idxConstraint,0,numConstraintToProcess,1)=newLambda;
                    if (f_printLog.getValue())
                        cerr <<"["<< iteration << "/" << numIterations.getValue() <<"] " << (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum()  << " for system n°" << idxConstraint << "/" << W.cols() << " of " << numConstraintToProcess << " equations "
                                << " between " << constraint->getSimulatedMechModel1()->getName() << " and " << constraint->getSimulatedMechModel2()->getName() << ""<<endl;
                    correctionDone |=  ( (LambdaPreviousIteration - Lambda.block(idxConstraint,0,numConstraintToProcess,1)).sum() != 0);
                }
            }
        }

        if (Order == core::ConstraintParams::VEL && traceKineticEnergy.getValue())
        {
            if (iteration == 0)
            {
                graphKineticEnergy.beginEdit()->clear();
                graphKineticEnergy.endEdit();
            }
            VectorEigen LambdaSave=Lambda;
            Lambda.noalias() = Lambda - LambdaPrevious;
            computeKineticEnergy(id);
            Lambda = LambdaSave;
            LambdaPrevious=Lambda;
        }

        const SReal residual=(c-W*Lambda).norm()*invNormC;
        vError.push_back( residual );
        graphGSError.endEdit();
        continueIteration = (residual > maxError.getValue());

        if (this->f_printLog.getValue()) cerr << "ITERATION " << iteration << " ENDED:  Residual for iteration " << iteration << " = " << residual <<endl;
    }

    if (iteration == numIterations.getValue() && f_printLog.getValue())
        cerr << "no convergence in Gauss-Seidel for " << orderName;

    if (f_printLog.getValue())
        cerr << "Gauss-Seidel done in " << iteration << " iterations "<<endl;

    if (Order == core::ConstraintParams::VEL && traceKineticEnergy.getValue()) return false;
    return true;
}

void LMConstraintSolver::constraintStateCorrection(VecId id,  core::ConstraintParams::ConstOrder order,
        bool isPositionChangesUpdateVelocity,
        const SparseMatrixEigen  &invM_Ltrans,
        const VectorEigen  &c,
        const sofa::helper::set< unsigned int > &dofUsed,
        sofa::core::behavior::BaseMechanicalState* dofs) const
{

    //Correct Dof
    //    operation: M0^-1.L0^T.lambda -> A

    VectorEigen A;
    /* This seems very redundant, but the SparseTimeDenseProduct seems buggy when the dense rhs
    and sparse lhs uses different storage order. This is always the case with a sparse row major
    and a dense vector (col major otherwise compile time assert )*/
    //Eigen::SparseMatrix<SReal> invM_Ltrans_colMajor = Eigen::SparseMatrix<SReal>(invM_Ltrans);

    A.noalias() = invM_Ltrans*c;
    if (f_printLog.getValue())
    {
        cerr << "M^-1.L^T " << "\n" << invM_Ltrans << endl;
        cerr << "Lambda " << dofs->getName() << "\n" << c << endl;
        cerr << "Correction " << dofs->getName() << "\n" << A << endl;
    }
    const unsigned int dimensionDofs=dofs->getDerivDimension();

    //In case of position correction, we need to update the velocities
    if (order==core::ConstraintParams::POS)
    {
        //Detect Rigid Bodies
        if (dofs->getCoordDimension() == 7 && dofs->getDerivDimension() == 6)
        {
            VectorEigen Acorrection=VectorEigen::Zero(dofs->getSize()*(3+4));
            //We have to transform the Euler Rotations into a quaternion
            unsigned int offset=0;

            for (int l=0; l<A.rows(); l+=6)
            {
                offset=l/6;
                Acorrection(l+0+offset)=A(l+0);
                Acorrection(l+1+offset)=A(l+1);
                Acorrection(l+2+offset)=A(l+2);

                defaulttype::Quaternion q=defaulttype::Quaternion::createQuaterFromEuler(defaulttype::Vector3(A(l+3),A(l+4),A(l+5)));

                Acorrection(l+3+offset)=q[0];
                Acorrection(l+4+offset)=q[1];
                Acorrection(l+5+offset)=q[2];
                Acorrection(l+6+offset)=q[3];
            }
            offset=0;
            FullVector<SReal> v(Acorrection.data(), (Index)Acorrection.rows());

            if (f_printLog.getValue())  cerr << "Lambda Corrected for Rigid " << "\n" << Acorrection << endl;
            dofs->addFromBaseVectorSameSize(id,&v,offset );

        }
        else
        {
            std::set< unsigned int >::const_iterator it;
            for (it=dofUsed.begin(); it!=dofUsed.end(); ++it)
            {
                unsigned int offset=(*it);
                FullVector<SReal> v(&(A.data()[offset*dimensionDofs]),dimensionDofs);
                dofs->addFromBaseVectorDifferentSize(id,&v,offset );
            }
        }

        if (isPositionChangesUpdateVelocity)
        {
            const double h=1.0/getContext()->getDt();

            std::set< unsigned int >::const_iterator it;
            for (it=dofUsed.begin(); it!=dofUsed.end(); ++it)
            {
                unsigned int offset=(*it);
                FullVector<SReal> v(&(A.data()[offset*dimensionDofs]),dimensionDofs);
                for (unsigned int i=0; i<dimensionDofs; ++i) v[i]*=h;
                dofs->addFromBaseVectorDifferentSize(id,&v,offset );
            }
        }

    }
    else
    {
        std::set< unsigned int >::const_iterator it;
        for (it=dofUsed.begin(); it!=dofUsed.end(); ++it)
        {
            unsigned int offset=(*it);
            FullVector<SReal> v(&(A.data()[offset*dimensionDofs]),dimensionDofs);
            dofs->addFromBaseVectorDifferentSize(id,&v,offset );
        }

    }
}

void LMConstraintSolver::computeKineticEnergy(MultiVecId id)
{
    helper::vector<double> &vError=(*graphKineticEnergy.beginEdit())["KineticEnergy"];

    core::ConstraintParams cparam;
    cparam.setOrder(core::ConstraintParams::VEL);

    applyCorrection(&cparam, id);

    double kineticEnergy=0;
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const core::behavior::BaseMass *mass=dofs->getContext()->getMass();
        if (mass) kineticEnergy += mass->getKineticEnergy();
    }
    vError.push_back(kineticEnergy);
    graphKineticEnergy.endEdit();
}

void LMConstraintSolver::handleEvent(core::objectmodel::Event *e)
{
    if (sofa::simulation::AnimateBeginEvent::checkEventType(e))
    {
        graphGSError.beginEdit()->clear();
        graphGSError.endEdit();
    }
}

int LMConstraintSolverClass = core::RegisterObject("A Constraint Solver working specifically with LMConstraint based components")
        .add< LMConstraintSolver >();

SOFA_DECL_CLASS(LMConstraintSolver);

} // namespace constraintset

} // namespace component

} // namespace sofa


// Copyright (c) 2020 The Gapcoin Core developers
// Copyright (c) 2016-2018 Duality Blockchain Solutions Developers
// Copyright (c) 2014-2018 The Dash Core Developers
// Copyright (c) 2009-2018 The Bitcoin Developers
// Copyright (c) 2009-2018 Satoshi Nakamoto
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <qt/miningpage.h>
#include <qt/forms/ui_miningpage.h>
#include <qt/clientmodel.h>
#include <qt/guiutil.h>
#include <qt/walletmodel.h>

#include <bignum.h>
#include <miner.h>
#include <net.h>
#include <pow.h>
#include <rpc/server.h>
#include <univalue.h>
#include <util.h>
#include <utiltime.h>
#include <validation.h>

#include <boost/thread.hpp>
#include <stdio.h>

#include <QtDebug>

extern UniValue GetNetworkHashPS(int lookup, int height);

MiningPage::MiningPage(const PlatformStyle *platformStyle, QWidget *parent) :
    QWidget(parent),
    ui(new Ui::MiningPage),
    hasMiningprivkey(false)
{
    ui->setupUi(this);
    this->powUtils = new PoWUtils;
    int nMaxUseThreads = GUIUtil::MaxThreads();

    /*
    std::string PrivAddress = gArgs.GetArg("-miningprivkey", "");
    if (!PrivAddress.empty())
    {
        CBitcoinSecret Secret;
        Secret.SetString(PrivAddress);
        if (Secret.IsValid())
        {
            CBitcoinAddress Address;
            Address.Set(Secret.GetKey().GetPubKey().GetID());
            ui->labelAddress->setText(QString("All mined coins will go to %1").arg(Address.ToString().c_str()));
            hasMiningprivkey = true;
        }
    }
    */

    ui->sliderCores->setMinimum(0);
    ui->sliderCores->setMaximum(nMaxUseThreads);
    ui->sliderCores->setValue(nMaxUseThreads);
    ui->labelNCores->setText(QString("%1").arg(nMaxUseThreads));
    ui->sliderGraphSampleTime->setMaximum(0);
    ui->sliderGraphSampleTime->setMaximum(6);

    ui->sliderCores->setToolTip(tr("Utilisez le slider pour sélectionner la quantité de threads du Processeur à utiliser."));
    ui->labelNetHashRate->setToolTip(tr("Puissance de hashage totale du réseau Gapcoin."));
    ui->labelMinerHashRate->setToolTip(tr("Puissance de hashage de votre Processeur en cours de minage."));
    ui->labelNextBlock->setToolTip(tr("Estimation de la quantité moyenne de Gap que vous miner en 24 heures."));

    isMining = gArgs.GetBoolArg("-gen", false)? 1 : 0;

    QValidator *headershiftValuevalidator = new QIntValidator(14, 512, this);
    ui->headershiftValue->setValidator(headershiftValuevalidator);
    QValidator *sievesizeValuevalidator = new QIntValidator(1000, 1533554432, this);
    ui->sievesizeValue->setValidator(sievesizeValuevalidator);
    QValidator *sieveprimesValuevalidator = new QIntValidator(1000, 40900000, this);
    ui->sieveprimesValue->setValidator(sieveprimesValuevalidator);

    connect(ui->sliderCores, SIGNAL(valueChanged(int)), this, SLOT(changeNumberOfCores(int)));
    connect(ui->sliderGraphSampleTime, SIGNAL(valueChanged(int)), this, SLOT(changeSampleTime(int)));
    connect(ui->pushSwitchMining, SIGNAL(clicked()), this, SLOT(switchMining()));
    connect(ui->pushButtonClearData, SIGNAL(clicked()), this, SLOT(clearHashRateData()));
    connect(ui->checkBoxShowGraph, SIGNAL(stateChanged(int)), this, SLOT(showHashRate(int)));

    ui->minerHashRateWidget->graphType = HashRateGraphWidget::GraphType::MINER_HASHRATE;
    ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::FIVE_MINUTES);

    showHashMeterControls(false);

    updateUI(isMining);
    startTimer(8000);
}

MiningPage::~MiningPage()
{
    delete ui;
}

void MiningPage::setModel(WalletModel *model)
{
    this->model = model;
}

void MiningPage::setClientModel(ClientModel *model)
{
    this->clientModel = model;
}

void MiningPage::updateUI(bool fGenerate)
{

    qint64 NetworkHashrate = GUIUtil::GetNetworkHashPS(120, -1);
    // int64_t hashrate = (int64_t)dHashesPerSec;
    qint64 Hashrate = GUIUtil::GetHashRate();

    ui->labelNetHashRate->setText(GUIUtil::FormatHashRate(NetworkHashrate));
    ui->labelMinerHashRate->setText(GUIUtil::FormatHashRate(Hashrate));

    QString NextBlockTime;
    if (Hashrate == 0)
        NextBlockTime = QChar(L'∞');
    else
    {
        double gaps_per_day = this->powUtils->gaps_per_day(dHashesPerSec, chainActive.Tip()->nDifficulty);
        NextBlockTime = QString::number(gaps_per_day);
    }
    ui->labelNextBlock->setText(NextBlockTime);

    if (GUIUtil::GetHashRate() == 0) {
        ui->pushSwitchMining->setToolTip(tr("Cliquez sur 'Débuter le minage' pour commencer à miner"));
        ui->pushSwitchMining->setText(tr("Débuter le minage"));
        ui->pushSwitchMining->setEnabled(true);
     }
     else {
        ui->pushSwitchMining->setToolTip(tr("Cliquez sur 'Arrêter le minage' pour arrêter de miner"));
        ui->pushSwitchMining->setText(tr("Arrêter le minage"));
        ui->pushSwitchMining->setEnabled(true);
    }
    ui->pushSwitchMining->setEnabled(true);

    QString status = QString("Minage de Gapcoin à l'arrêt");
    if (fGenerate)
        status = QString("Minage avec %1/%2 threads, shift: %3, sieve size: %4, number of primes in sieve: %5 - hashrate: %6 (%7 tests par sec.)")
                .arg((int)ui->sliderCores->value())
                .arg(GUIUtil::MaxThreads())
                .arg(nMiningShift).arg(nMiningSieveSize)
                .arg(nMiningPrimes)
                .arg(GUIUtil::FormatHashRate(Hashrate)).arg(dTestsPerSec);
    ui->miningStatistics->setText(status);
}

void MiningPage::restartMining(bool fGenerate, int nThreads)
{
    isMining = fGenerate;
    if (nThreads <= maxGenProc)
        nUseThreads = nThreads;

    // unlock wallet before mining

  #ifndef __linux__
    if (fGenerate && !hasMiningprivkey && !unlockContext.get())
    {
        this->unlockContext.reset(new WalletModel::UnlockContext(model->requestUnlock()));
        if (!unlockContext->isValid())
        {
            unlockContext.reset(nullptr);
            return;
        }
    }
  #endif

    nMiningShift = ui->headershiftValue->text().toInt();
    nMiningSieveSize = ui->sievesizeValue->text().toInt();
    nMiningPrimes = ui->sieveprimesValue->text().toInt();

    GenerateGapcoins(true, nThreads, Params());

    // lock wallet after mining
    if (!fGenerate && !hasMiningprivkey)
        unlockContext.reset(nullptr);

    updateUI(fGenerate);
}

void MiningPage::StartMiner()
{
    int nThreads = (int)ui->sliderCores->value();
    nMiningShift = ui->headershiftValue->text().toInt();
    nMiningSieveSize = ui->sievesizeValue->text().toInt();
    nMiningPrimes = ui->sieveprimesValue->text().toInt();

    GenerateGapcoins(true, nThreads, Params());
    isMining = true;
    updateUI(isMining);
}

void MiningPage::StopMiner()
{
    isMining = false;
    int nThreads = (int)ui->sliderCores->value();
    GenerateGapcoins(false, nThreads, Params());
    updateUI(isMining);
}

void MiningPage::changeNumberOfCores(int i)
{
    // restartMining(isMining, i);

    ui->labelNCores->setText(QString("%1").arg(i));
    if (i == 0) {
        StopMiner();
    }
    else if (i > 0 && GUIUtil::GetHashRate() > 0) {
        StartMiner();
    }
}

void MiningPage::switchMining()
{
    // restartMining(!isMining, ui->sliderCores->value());

    int64_t hashRate = GUIUtil::GetHashRate();
    int nThreads = (int)ui->sliderCores->value();

    if (hashRate > 0) {
        ui->pushSwitchMining->setText(tr("Stopping."));
        StopMiner();
    }
    else if (nThreads == 0 && hashRate == 0){
        ui->sliderCores->setValue(1);
        ui->pushSwitchMining->setText(tr("Starting."));
        StartMiner();
    }
    else {
        ui->pushSwitchMining->setText(tr("Starting."));
        StartMiner();
    }
}

void MiningPage::timerEvent(QTimerEvent *)
{
    updateUI(isMining);
}

void MiningPage::updateSievePrimes(int i)
{

    sievesizeValue = pow(2, i);
    qDebug() << "New header shift: " << QString("%1").arg(i) << ", new sieve size value: " << QString("%1").arg(sievesizeValue);
    ui->sievesizeValue->setText(QString("%1").arg(sievesizeValue));
}

void MiningPage::showHashRate(int i)
{
    if (i == 0) {
        ui->minerHashRateWidget->StopHashMeter();
        showHashMeterControls(false);
    }
    else {
        ui->minerHashRateWidget->StartHashMeter();
        showHashMeterControls(true);
    }
}

void MiningPage::showHashMeterControls(bool show)
{
    if (show == false) {
        ui->sliderGraphSampleTime->setVisible(false);
        ui->labelGraphSampleSize->setVisible(false);
        ui->pushButtonClearData->setVisible(false);
    }
    else {
        ui->sliderGraphSampleTime->setVisible(true);
        ui->labelGraphSampleSize->setVisible(true);
        ui->pushButtonClearData->setVisible(true);
    }
}

void MiningPage::changeSampleTime(int i)
{
    if (i == 0) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::FIVE_MINUTES);
        ui->labelGraphSampleSize->setText(QString("5 minutes"));
    }
    else if (i == 1) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::TEN_MINUTES);
        ui->labelGraphSampleSize->setText(QString("10 minutes"));
    }
    else if (i == 2) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::THIRTY_MINUTES);
        ui->labelGraphSampleSize->setText(QString("30 minutes"));
    }
    else if (i == 3) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::ONE_HOUR);
        ui->labelGraphSampleSize->setText(QString("1 hour"));
    }
    else if (i == 4) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::EIGHT_HOURS);
        ui->labelGraphSampleSize->setText(QString("8 hours"));
    }
    else if (i == 5) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::TWELVE_HOURS);
        ui->labelGraphSampleSize->setText(QString("12 hours"));
    }
    else if (i == 6) {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::ONE_DAY);
        ui->labelGraphSampleSize->setText(QString("1 day"));
    }
    else {
        ui->minerHashRateWidget->UpdateSampleTime(HashRateGraphWidget::SampleTime::ONE_DAY);
        ui->labelGraphSampleSize->setText(QString("1 day"));
    }
}

void MiningPage::clearHashRateData()
{
    ui->minerHashRateWidget->clear();
}


//==============================================================================
//         Copyright 2009 - 2014 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_DETAILS_TOPOLOGY_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_DETAILS_TOPOLOGY_HPP_INCLUDED

#include <boost/simd/swar/functions/details/permuted.hpp>
#include <boost/simd/sdk/simd/preprocessor/repeat.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/dispatch/meta/any.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/int.hpp>

namespace boost { namespace simd { namespace details
{
  //============================================================================
  // Adapt a permutation of [C ... 2*C[ to be into [0 ... C[
  //============================================================================
  template<typename Permutation, std::size_t Cardinal>
  struct remap
  {
    template<typename I, typename C>
    struct  apply
          : boost::mpl::int_
            < mpl::apply<Permutation,I,C>::type::value
            - int(Cardinal)*( mpl::apply<Permutation,I,C>::type::value > -1 )
            >
    {};
  };

  //============================================================================
  // Adapt a permutation of [0 ... 2*C[ to be into the [0 ... C[
  //============================================================================
  template<typename Permutation, std::size_t Cardinal>
  struct clamp
  {
    template<typename I, typename C>
    struct  apply
          : boost::mpl::int_
            < (mpl::apply<Permutation,I,C>::type::value >= C::value)
            ? (mpl::apply<Permutation,I,C>::type::value - C::value)
            : mpl::apply<Permutation,I,C>::type::value
            >
    {};
  };
  //============================================================================
  // Adapt a permutation to be remove all -1
  //============================================================================
  template<typename Permutation>
  struct fix_zero
  {
    template<typename I, typename C>
    struct  apply
          : boost::mpl::int_
            < (mpl::apply<Permutation,I,C>::type::value >= 0)
            ? mpl::apply<Permutation,I,C>::type::value
            : I::value
            >
    {};
  };

  //============================================================================
  // Check if Permutation indexing I is forcing a zero
  //============================================================================
  template< typename Permutation, typename Index, typename Cardinal>
  struct  is_zeroing_at
        : mpl::bool_< mpl::apply<Permutation,Index,Cardinal>::type::value == -1 >
  {};

  //============================================================================
  // Does the permutation does any zeroing ?
  //============================================================================
  template<typename Permutation, std::size_t Cardinal>
  struct  perform_zeroing
        : dispatch::meta::any_seq < is_zeroing_at < Permutation
                                                  , boost::mpl::_1
                                                  , mpl::int_<Cardinal>
                                                  >
                                  , mpl::range_c<int,0,Cardinal>
                                  >
  {};

  //============================================================================
  // Zeroing mask generator
  //============================================================================
  template<typename T, typename Permutation, std::size_t Index, std::size_t Cardinal>
  struct  zero_mask
        : mpl::int_< mpl::apply < Permutation
                                , mpl::int_<Index/sizeof(T)>
                                , mpl::int_<Cardinal>
                                >::type::value == -1
                    ? 0 : ~0
                    >
  {};

  //============================================================================
  // Predefined topology
  //============================================================================
  typedef boost::mpl::int_<-1> first_arg;
  typedef boost::mpl::int_< 0> mixed_arg;
  typedef boost::mpl::int_< 1> second_arg;

  //============================================================================
  // Computes the topology of a permutation:
  // -1 means all indexing is from a0
  //  0 means all indexing are random
  // +1 means all indexing is from a1
  //============================================================================
  template<typename Permutation, std::size_t Cardinal>
  struct topology
  {
    typedef mixed_arg type;
  };

  #define M0(z,n,t) static const int i##n = permuted<Permutation,n,t>::value;
  #define M1(z,n,t) && !(i##n>=t)
  #define M2(z,n,t) && (i##n>=t||i##n==-1)

  #define M3(z,n,t)                                                            \
  template<typename Permutation> struct topology<Permutation,n>                \
  {                                                                            \
    BOOST_PP_REPEAT(n,M0,n);                                                   \
                                                                               \
    typedef boost::mpl::                                                       \
            int_< true BOOST_PP_REPEAT(n,M1,n)                                 \
                ? -1 : (true BOOST_PP_REPEAT(n,M2,n) ? 1 : 0)                  \
                > type;                                                        \
  };                                                                           \
  /**/

  BOOST_SIMD_PP_REPEAT_POWER_OF_2_FROM(2,M3,~)

  #undef M0
  #undef M1
  #undef M2
  #undef M3
} } }

#endif


//          Copyright Rein Halbersma 2010-2020.
// Distributed under 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)

#include <dctl/egdb/index.hpp>          // subdatabase
#include <dctl/core.hpp>                // state, international, board
#include <dctl/util/type_traits.hpp>    // position_t
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_CASE, BOOST_CHECK_EQUAL, BOOST_AUTO_TEST_SUITE_END
#include <vector>                       // vector

using namespace dctl::egdb;
using namespace dctl::core;

BOOST_AUTO_TEST_SUITE(Index)

BOOST_AUTO_TEST_CASE(NumeralIsInverseOfPosition)
{
        using state_type = basic_state<international, basic_board<micro>>;

        using subdivision_type = subdivision<position_t<state_type>>;
        auto const subdivisions = std::vector<subdivision_type>
        {
                { 4, 0, 0, 0 },
                { 4, 0, 4, 0 },
                { 0, 4, 0, 0 },
                { 0, 4, 0, 4 },
                { 2, 2, 0, 0 },
                { 2, 2, 2, 0 },
                { 2, 2, 0, 2 },
                { 2, 2, 2, 2 },
                { 4, 0, 2, 0 },
                { 0, 4, 0, 2 },
                { 2, 2, 1, 1 }
        };

        for (auto const& subdb : subdivisions) {
                for (auto i = 0LL; i < subdb.size(); ++i) {
                        if (auto const p = subdb.position(i); p) {
                                BOOST_CHECK_EQUAL(subdb.index(p.value()), i);
                        }
                }
        }
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>
#include <ccmc/Kameleon.h>
#include <ccmc/Tracer.h>
#include <ccmc/Fieldline.h>
#include <ccmc/StringConstants.h>
#include <ccmc/Interpolator.h>
//#include <ccmc/old-tracer.h>
#include <ccmc/Point3f.h>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
//#include <boost/program_options.hpp>
//#include <google/profiler.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <ctime>
#define LENGTH 500;

using namespace std;
using namespace ccmc;

//namespace po = boost::program_options;

int main (int argc, char * argv[])
{
// Output file names-----------------
	ofstream File("data_out.txt");
	ofstream File2("flineout.txt");
	ofstream File3("integrals.txt");
	ofstream File4("minima.txt");
	ofstream File5("maxima.txt");
	ofstream File6("polymin.txt");
	ofstream File7("polymax.txt");
    ofstream FileE("flineExEyEz.txt");
//------------------------------------
	std::string filename;
	std::string variable;
	std::string positions;
    std::vector<Point3f> seedsVector;
	float c0;
	float c1;
	float c2;
	int iterations = 10;

	if (argc != 6)
	{
		cout << "integrator <filename> <variable> <positions>" << endl;
//		exit(1);
	}

	filename = argv[1];
	variable = argv[2];
	positions = argv[3];
	/*po::options_description desc("Test program");
	desc.add_options()("help", "produce help message")("input-file", po::value<std::string>(&filename), "input file")
					  ("variable", po::value<std::string>(&variable), "Variable to trace")
					  ("positions",po::value<std::string>(&positions), "Positions file");
	po::positional_options_description p;
	p.add("input-file", -1);

	po::variables_map vm;
	po::store(
			po::command_line_parser(argc, argv).options(desc).positional(p).run(),
			vm);
	po::notify(vm);

	if (vm.count("help"))
	{
		cout << desc << endl;
	}

	if (vm.count("input-file"))
	{
		cout << "Input file: " << filename << std::endl;
	} else
	{
		std::cout << "Input file was not set." << std::endl;
		std::cout << desc << std::endl;
		exit(1);
	}
	std::cout << "after po options" << std::endl;

*/

	cout << "hello from integrator!" << std::endl;

	Kameleon kameleon;
	kameleon.open(filename);
	cout <<"file open\n";
	kameleon.loadVectorVariable(variable);
	cout <<"integrator: variables loaded\n";
	Tracer tracer(&kameleon);
	tracer.setMaxIterations(20000);
	//tracer.setInnerBoundary(2.5f);
	tracer.setDn(.2f);
	cout<<"integrator: tracer on\n";

	clock_t start, finish;
	start = clock();

	std::ifstream seedsFileStream;
	seedsFileStream.open(positions.c_str());
	if (!seedsFileStream.is_open())
	{
			std::cerr << "Error opening seeds position file " << positions << "." << std::endl;
			exit(1);
	}else
	{
		std::string line;
		while(seedsFileStream.good())
		{
			getline(seedsFileStream, line);
			//std::cout << line << std::endl;
			boost::char_separator<char> sep(" ");
			boost::tokenizer<boost::char_separator<char> > tokens(line, sep);
			int count = 0;
			Point3f position;
			foreach(std::string t, tokens)
			{
				if (count == 0)
				{
					position.component1 = boost::lexical_cast<float>(t);
					count++;
				} else if (count == 1)
				{
					position.component2 = boost::lexical_cast<float>(t);
					count++;
				} else
				{
					position.component3 = boost::lexical_cast<float>(t);
				}
			}
			if (count > 0)
					seedsVector.push_back(position);
		}
	}
	File2 << "f3index px py pz f3data(px,py,pz)  f3int(ds*data)\n";
	FileE << "flinenum flinedex px py pz Ex Ey Ez\n";
	int polymin = 0;
	int polymax = 0;

    for (int i = 0; i < seedsVector.size(); i++)
    {
	cout <<"seed number "<< i << "\n";
    Point3f curr = seedsVector[i];
	c0 = curr.component1;
	c1 = curr.component2;
	c2 = curr.component3;
	// File2<<"seed: "<< c0 <<" "<< c1 <<" "<< c2 << "\n";
 	cout<<"seed: "<< c0 <<" "<< c1 <<" "<< c2 << "\n";
	Fieldline f1; //create new field line
	cout<<"Before bidirectional Trace\n";
	f1 = tracer.bidirectionalTrace(variable,c0,c1,c2);
	cout <<"After bidirectional Trace\n";
	finish = clock();
	float elapsed_time = ((double) finish - (double) start) / CLOCKS_PER_SEC;

	int vnum = 1;
	std::string variables[] = {ccmc::strings::variables::b_};
	Fieldline f2;  //create new field line
    Fieldline fEx; //create field line for storing Efield
	Fieldline fEy;
	Fieldline fEz;
	float binv = 0.0; //create variable to hold partial integral ds/B
	Point3f p;  // create point to store sampled positions
    Point3f E; // create point to store Efield
    Point3f B; // create point to store Bfield
	ccmc::Interpolator * interpolator = kameleon.createNewInterpolator();
	for (int f = 0; f < f1.size(); f++)  //march along field line
	{
		p = f1.getPosition(f); // store current position in p
		for (int g = 0; g < vnum; g++) // for each variables
		{
			binv = interpolator->interpolate(variables[g],p.component1,p.component2,p.component3);
			binv = 1.0/binv; // compute 1/B
			f2.insertPointData(p,binv); // add data at position into f2
			E.component1 =interpolator->interpolate(ccmc::strings::variables::ex_,
				p.component1,p.component2,p.component3);
                        E.component2 =interpolator->interpolate(ccmc::strings::variables::ey_,
                                p.component1,p.component2,p.component3);
                        E.component3 =interpolator->interpolate(ccmc::strings::variables::ez_,
                                p.component1,p.component2,p.component3);
			fEx.insertPointData(p,E.component1);
                        fEy.insertPointData(p,E.component2);
                        fEz.insertPointData(p,E.component3);
		}
	}
	delete interpolator;
	f2.minmax();

	float dn = .2f;
	int step_max = 20000;
	float r_end = 2.5f;

	finish = clock();
	float elapsed_time2 = ((double) finish - (double) start) / CLOCKS_PER_SEC;

	vector<float> elengths = f2.getDs(); //creates elements and elementsMagnitude
	vector<float> length = f2.measure(); //uses elementsMagnitude

	std::cout << "total points: "<< f2.size()<<"\n" ;
	std::cout << "points in f1: " << f1.size() << "\n";
	std::cout << "New elapsed time: " << elapsed_time << " with " << f1.getPositions().size() << " positions." << "\n";
	std::cout << "f2index px py pz data int(ds*data)"<< "\n";
	f2.getElements(); //returns elements
	f2.getDs();
	f2.measure();
	f2.integrate(); //uses elementsMagnitude and values
	// f2.minmax(); //moved above, just after b inverse calculation, else ds/B gives more max than min!
	int extrema;
	cout << "f2 has "<<f2.mincount<<" minima and "<< f2.maxcount << " maxima\n";
	polymin = polymin + f2.mincount;
	polymax = polymax + f2.maxcount;
	File6 << polymin - f2.mincount<<"\n";
	File7 << polymax - f2.maxcount<<"\n";

	cout <<" minima: "<< f2.minima.size() <<"\n";
	 for (int m = 0; m < f2.minima.size(); m++)
	 {
	 extrema = f2.minima[m];
	 if ((extrema > 0)&&(extrema < f2.size()-1))
	 {
         File4 << f2.getPositions()[extrema].component1 << " ";
         File4 << f2.getPositions()[extrema].component2 << " ";
         File4 << f2.getPositions()[extrema].component3;
	 File4 << " "<<f2.getData()[extrema-1]<<" "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema+1];
	 File4 << " "<<2*(f2.getData()[extrema] - f2.getData()[extrema-1])/(f2.getData()[extrema]+f2.getData()[extrema-1]);
	 File4 << " "<<2*(f2.getData()[extrema] - f2.getData()[extrema+1])/(f2.getData()[extrema]+f2.getData()[extrema+1]);
	 }
	 else
	 {
         File4 << f2.getPositions()[extrema].component1 << " ";
         File4 << f2.getPositions()[extrema].component2 << " ";
         File4 << f2.getPositions()[extrema].component3;
	 File4 << " "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema];
	 File4 << " 0.0 0.0";
	 }
	 File4 <<" "<<f2.getData()[extrema]<<" "<< m-f2.GlobMinIndex <<"\n";
	 }

	 cout <<" maxima: "<< f2.maxima.size() <<"\n";
         for (int m = 0; m < f2.maxima.size(); m++)
         {
	 extrema = f2.maxima[m];
         if ((extrema > 0)&&(extrema < f2.size()-1))
         {
         File5 << f2.getPositions()[extrema].component1 << " ";
         File5 << f2.getPositions()[extrema].component2 << " ";
         File5 << f2.getPositions()[extrema].component3;
         File5 << " "<< f2.getData()[extrema-1]<<" "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema+1];
	 File5 << " "<<2*(f2.getData()[extrema] - f2.getData()[extrema-1])/(f2.getData()[extrema]+f2.getData()[extrema-1]);
         File5 << " "<<2*(f2.getData()[extrema] - f2.getData()[extrema+1])/(f2.getData()[extrema]+f2.getData()[extrema+1]);
	 }
         else
         {
         File5 << f2.getPositions()[extrema].component1 << " ";
         File5 << f2.getPositions()[extrema].component2 << " ";
         File5 << f2.getPositions()[extrema].component3;
         File5 << " "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema]<<" "<<f2.getData()[extrema];
         File5 << " 0.0 0.0";
         }
	 File5 << " "<<f2.getData()[extrema]<<" "<< m - f2.GlobMaxIndex <<"\n";
         }

		for (int l = 0; l < f2.size(); l++)
		{
			if ((l>0)&&(l<f2.size()))
			{
		//	File << l << " " << f2.getPositions()[l].component1 << " ";
		//	File << f2.getPositions()[l].component2 << " " << f2.getPositions()[l].component3;
		//	File << " "  << f2.getData()[l];
		//	File << " " << f2.getElements()[l];
		//	File << " " << f2.getDs()[l];
		//	File << " " << f2.measure()[l];
		//	File << " " << f2.integrate()[l];
			}
		}
	Fieldline f3 = f2.interpolate(3,100); // Interpolate f2 to fixed ds
	fEx.getDs(); // creates differential elements
	fEx.measure(); // integrates over elements
	fEy.getDs();
	fEy.measure();
	fEz.getDs();
	fEz.measure();
	Fieldline fExint = fEx.interpolate(3,100); // Interpolate fEx to fixed ds
    Fieldline fEyint = fEy.interpolate(3,100); // Interpolate fEy to fixed ds
    Fieldline fEzint = fEz.interpolate(3,100); // Interpolate fEz to fixed ds
	std::cout << "Interpolated Field line size: "<<f3.size() << "\n";
	if (f3.size() != 100)
	 {cout<<"Findme:interpolated field line not right!";}
	//File2 << "f3index px py pz f3data(px,py,pz)  f3int(ds*data)"<< "\n";
	int j = 0; // locates index within f2
	float t=0.0; // locates t between points in f2
	int k = 0;
	cout<<"Testing f3"<<"\n";
	cout<<"f3.getPositions()[k].component1= "<<f3.getPositions()[k].component1<<"\n";
	std::cout <<"Before print loop, f3.size()= "<<f3.size()<<"\n";
	for(k = 0; k < f3.size(); k++)
	{
		File2 << k << " " << f3.getPositions()[k].component1;
		File2 <<      " " << f3.getPositions()[k].component2;
		File2 <<      " " << f3.getPositions()[k].component3;
		File2 <<      " " << f3.getData()[k];
		j = f3.getNearest()[k]; //nearest lower vertex in f2
		t = f3.getTlocal()[k]; //tlocal between nearest and nearest+1 in f2
		File2 <<      " "<< f2.getIntegral(j)*(1.0-t)+t*f2.getIntegral(j) << "\n";
	}
	std::cout<<"\n"<<"Wrote field line to file"<<"\n";
	std::cout<< "Writing out integral \n";
	File3 << c0<<" "<<c1<<" "<<c2<< " "<< f2.integrate()[f2.size()-1]<<" "<<f2.mincount<< " "<< f2.maxcount<<"\n";

	int sze = fEx.size();
	std::cout<<"Writing electric field Ex to file\n";
        for(k = 0; k < sze; k++)
        {
		FileE << i << " "; // fline number
                FileE << float(k)/sze << " " << fEx.getPositions()[k].component1;
                FileE <<      " " << fEx.getPositions()[k].component2;
                FileE <<      " " << fEx.getPositions()[k].component3;
                FileE <<      " " << fEx.getData()[k]; //components of E
                FileE <<      " " << fEy.getData()[k];
                FileE <<      " " << fEz.getData()[k];
	        FileE << "\n";
        }

	}
	kameleon.close();
	return 0;
}


#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <tf/tf.h>

#include "QuadState.h"
#include "ImageProcessor.h"

#include "MessageHandler.h"

namespace localization {

    MessageHandler::MessageHandler(const CameraInfo &cameraInfo, QuadState &state, ImageProcessor *processor)
            : it_(nh_), state_(state), processor_(processor), cameraInfo_(cameraInfo) {
        imageSub_ = it_.subscribe(cameraInfo.topic, 1, &MessageHandler::cameraCallback, this);
        imagePub_ = it_.advertise(cameraInfo.name + "/intersection_detection", 1);
    }

    MessageHandler::~MessageHandler() {
    }

    void MessageHandler::lookForMessages() {
        ros::Rate rate(30);
        while (ros::ok()) {
            ros::spinOnce();
            rate.sleep();
        }
    }

    void MessageHandler::cameraCallback(const sensor_msgs::ImageConstPtr &msg) {
        if (state_.update(msg->header.stamp) && !isProcessing_) {
            cv::Mat input = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8)->image;

            cv::Mat result;
            isProcessing_ = true;
            processor_->processImage(input, result);
            isProcessing_ = false;

            sensor_msgs::ImagePtr image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", result).toImageMsg();
            imagePub_.publish(image);
        }
    }
}


#ifndef _STATE_HH_
#define _STATE_HH_

/** \file
    The representation of states. */

#include <boost/shared_ptr.hpp>
#include <ext/functional>
#include <algorithm>
#include <cassert>
#include <functional>
#include <iostream>
#include <map>
#include <utility>
#include <vector>

/** Type representing an individual state.  The agent code uses this
    type as its internal representation of a state.  Therefore, it
    first transforms any observation from RL Glue into a
    std::vector<double>.  Smart pointers allow us to keep in memory
    only those instances still used by the agent. */
typedef std::vector<double> StateVector;
typedef boost::shared_ptr<const StateVector> StateVectorRef;

/** The probability of a certain state.  The state pointer must be
    valid (not NULL) and the probability must be nonnegative.
    Typically used as the value type in StateDistribution. */
typedef std::pair<StateVectorRef, double> StateProbability;

/** A common data structure representing a probability distribution
    over states.  Unless otherwise specified, the probabilities sum to
    1.  The contents of this data structure must be sorted (using the
    default comparator), and no two elements can have the same first
    part (state reference).  Note that we use a sorted vector instead
    of a map to minimize the memory footprint and optimize for fast
    iteration. */
typedef std::vector<StateProbability> StateDistribution;

/** A data structure that specifies how to interpret state vectors by
    weighting the state variables.  Each key is an index into a state
    vector, and the corresponding datum is the (positive) factor by
    which to multiply that state variable's value, to obtain the
    normalized state.  Straight Euclidean distance in this transformed
    space emulates scaled Euclidean distance in the original space.
    State vector indices that do not appear in the map are assumed to
    be irrelevant: the data associated with these keys are assumed to
    be 0, in effect implementing state abstraction. */
typedef std::map<size_t, double> StateVariables;

/** Output operator for printing states.  It prints the value of each
    state variable in order, separated by spaces, including a trailing
    space.  This operator aids in debugging.
    \param out The output stream on which to print the state
    \param s The state to print
    \return The output stream, after having the state printed to it */
std::ostream &operator<<(std::ostream &out, const StateVectorRef &s);

/** Function object used to hash states, for use with
    __gnu_cxx::hash_map. */
struct StateHasher: std::unary_function<StateVectorRef, size_t>
{
  size_t operator()(const StateVectorRef &x) const {
    return reinterpret_cast<size_t>(x.get());
  }
};

/** Scaled Euclidean distance between states.  This implementation may
    optionally ignore certain dimensions of the state space.*/
class DistanceFunction:
  public std::binary_function<StateVectorRef, StateVectorRef, double>
{
public:
  /** \param scale Determines how to scale the inputs before computing
                   distances. */
  DistanceFunction(const StateVariables &scale);

  /** Computes scaled Euclidean distance between two states.
      \param x State with size() larger than the greatest index in the
               scale mapping provided to this Distance Function.
      \param y State with size() larger than the greatest index in the
               scale mapping provided to this Distance Function.
      \return The scaled Euclidean distance in the specified
              dimensions */
  double operator()(const StateVectorRef &x, const StateVectorRef &y) const;

private:
  const StateVariables m_scale;
};

/** Comparator for states that only uses certain dimensions of the
    state space. */
class AbstractStateComparator:
  public std::binary_function<StateVectorRef, StateVectorRef, bool>
{
public:
  /** \param dimensions The keys of this mapping are the indices of
                        the state vectors to consider.  The data
                        associated with each key are ignored. */
  AbstractStateComparator(const StateVariables &dimensions);

  /** \param x A state such that the largest element of
               dimensions is a valid index
      \param y A state such that the largest element of
               dimensions is a valid index
      \return True iff the relevant elements of x are
              lexicographically smaller than the relevant elements of
              y */
  bool operator()(const StateVectorRef &x, const StateVectorRef &y) const;

private:
  const StateVariables m_dimensions;
};

/** An object that computes a real-valued function over states. */
class StateFunction: public std::unary_function<StateVectorRef, double>
{
public:
  virtual ~StateFunction();

  /** \param x The state to evaluate
      \return The value of this function for the given state */
  virtual double operator()(const StateVectorRef &x) const = 0;
};

typedef boost::shared_ptr<const StateFunction> StateFunctionRef;

/** A function whose value is always zero. */
class ZeroStateFunction: public StateFunction
{
public:
  static StateFunctionRef create();

  virtual ~ZeroStateFunction();
  virtual double operator()(const StateVectorRef &x) const;

private:
  ZeroStateFunction();
};

/** A function whose value is constant. */
class ConstantStateFunction: public StateFunction
{
public:
  static StateFunctionRef create(double value);

  virtual ~ConstantStateFunction();
  virtual double operator()(const StateVectorRef &x) const;

private:
  ConstantStateFunction(double value);

  double m_value;
};

/** An object that computes a binary function over states. */
class StatePredicate: public std::unary_function<StateVectorRef, bool>
{
public:
  virtual ~StatePredicate();

  /** \param x The state to evaluate
      \return The value of this function for the given state */
  virtual bool operator()(const StateVectorRef &x) const = 0;

  virtual void debug(std::ostream &out) const;
};

typedef boost::shared_ptr<const StatePredicate> StatePredicateRef;

/** A binary function whose value is always true. */
class TrueStatePredicate: public StatePredicate
{
public:
  static StatePredicateRef create();

  virtual ~TrueStatePredicate();
  virtual bool operator()(const StateVectorRef &x) const;

private:
  TrueStatePredicate();
};

/** A binary function whose value is always false. */
class FalseStatePredicate: public StatePredicate
{
public:
  static StatePredicateRef create();

  virtual ~FalseStatePredicate();
  virtual bool operator()(const StateVectorRef &x) const;

private:
  FalseStatePredicate();
};

class NegationPredicate: public StatePredicate
{
public:
  static StatePredicateRef create(const StatePredicateRef &predicate);

  virtual ~NegationPredicate();
  virtual bool operator()(const StateVectorRef &x) const;

private:
  NegationPredicate(const StatePredicateRef &predicate);

  StatePredicateRef original;
};

/** A binary function that returns true for states that match fixed
    goal values on some dimensions. */
class GoalStatePredicate: public StatePredicate
{
public:
  /** Creates a new GoalStatePredicate with a given goal, specified as
      a mapping from state variable dimensions to desired values.
      \param begin InputIterator with value_type convertible to
                   std::pair<size_t, double>
      \param end InputIterator with value_type convertible to
                 std::pair<size_t, double> */
  template <class InputIterator>
  static StatePredicateRef create(InputIterator begin, InputIterator end) {
    return StatePredicateRef(new GoalStatePredicate(begin, end));
  }

  virtual ~GoalStatePredicate();
  virtual bool operator()(const StateVectorRef &x) const;

  virtual void debug(std::ostream &out) const;

private:
  template <class InputIterator>
  GoalStatePredicate(InputIterator begin, InputIterator end)
    : m_goal(begin, end)
  {}

  const std::map<size_t, double> m_goal;
};

// Utility stuff

/** Comparator that compares pairs only by the first part of each pair. */
template <class PairT>
struct LessFirst: public std::binary_function<PairT, PairT, bool>
{
  bool operator()(const PairT &x, const PairT &y) const {
    return x.first < y.first;
  }
};

/** Inserts an element into a set stored as a sorted vector.
    \param v A vector sorted using the default comparator.
    \param x An element to insert into the vector set
    \return A pair containing first, an iterator to the copied element
            in the vector set, and second, whether or not the vector
            increased in size */
template <class T>
inline std::pair<typename std::vector<T>::iterator, bool>
vector_set_insert(std::vector<T> &v, const T &x)
{
  typename std::vector<T>::iterator pos;
  pos = std::lower_bound(v.begin(), v.end(), x);
  if (pos == v.end() || x < *pos)
    return std::make_pair(v.insert(pos, x), true);
  return std::make_pair(pos, false);
}

/** Inserts an element into a set stored as a sorted vector.
    \param v A vector sorted using the comparator cmp.
    \param x An element to insert into the vector set.
    \param cmp A comparator
    \return A pair containing first, an iterator to the copied element
            in the vector set, and second, whether or not the vector
            increased in size */
template <class T, class Cmp>
inline std::pair<typename std::vector<T>::iterator, bool>
vector_set_insert(std::vector<T> &v, const T &x, Cmp cmp)
{
  typename std::vector<T>::iterator pos;
  pos = std::lower_bound(v.begin(), v.end(), x, cmp);
  if (pos == v.end() || cmp(x, *pos))
    return std::make_pair(v.insert(pos, x), true);
  return std::make_pair(pos, false);
}

/** Erases an element from a set stored as a sorted vector.
    \param v A vector sorted using the default comparator.
    \param x An element to erase
    \return True iff the size of the vector decreased. */
template <class T>
inline bool vector_set_erase(std::vector<T> &v, const T &x)
{
  typename std::vector<T>::iterator del;
  del = std::lower_bound(v.begin(), v.end(), x);
  if (del != v.end() && !(x < *del)) {
    v.erase(del);
    return true;
  }
  return false;
}

/** Erases an element from a set stored as a vector sorted with a
    given comparator.
    \param v A vector sorted using the default comparator.
    \param x An element to erase
    \param cmp A comparator
    \return True iff the size of the vector decreased. */
template <class T, class Cmp>
inline bool vector_set_erase(std::vector<T> &v, const T &x, Cmp cmp)
{
  typename std::vector<T>::iterator del;
  del = std::lower_bound(v.begin(), v.end(), x, cmp);
  if (del != v.end() && !cmp(x, *del)) {
    v.erase(del);
    return true;
  }
  return false;
}

/** Associates a given key with a given datum in a given map stored as
    a sorted vector of pairs.
    \param v A vector of pairs sorted using the default comparator and
             with no duplicate keys
    \param x A key
    \param y A datum
    \return An iterator to the associated key-datum pair in the vector */
template <class A, class B>
inline typename std::vector<std::pair<A,B> >::iterator
vector_map_assoc(std::vector<std::pair<A,B> > &v, const A &x, const B &y)
{
  typename std::pair<A,B> val(x,y);
  typename std::pair<typename std::vector<std::pair<A,B> >::iterator,bool> ret =
    vector_set_insert(v, val, LessFirst<std::pair<A,B> >());
  if (!ret.second) // x was already a key in v
    ret.first->second = y; // change data associated with x
  return ret.first;
}

template <class A, class B>
inline typename std::vector<std::pair<A,B> >::const_iterator
vector_map_find(const std::vector<std::pair<A,B> > &v, const A &x)
{
  typename std::pair<A,B> val(x, B());
  typename std::vector<std::pair<A,B> >::const_iterator ret =
    std::lower_bound(v.begin(), v.end(), val, LessFirst<std::pair<A,B> >());
  if (ret != v.end() && x < ret->first)
    ret = v.end();
  return ret;
}

template <class A, class B>
inline typename std::vector<std::pair<A,B> >::iterator
vector_map_find(std::vector<std::pair<A,B> > &v, const A &x)
{
  typename std::pair<A,B> val(x, B());
  typename std::vector<std::pair<A,B> >::iterator ret =
    std::lower_bound(v.begin(), v.end(), val, LessFirst<std::pair<A,B> >());
  if (ret != v.end() && x < ret->first)
    ret = v.end();
  return ret;
}

#endif