#ifndef INHOMOGENOUSCOUPLEDPDEODESOLVER_TEMPLATED_HPP_
#define INHOMOGENOUSCOUPLEDPDEODESOLVER_TEMPLATED_HPP_

#include "ChemChasteFeAssemblerCommon.hpp"
#include "ChemChasteVolumeAssembler.hpp"
#include "ChemChasteSurfaceAssembler.hpp"
#include "AbstractAssemblerSolverHybrid.hpp"
#include "AbstractDynamicLinearPdeSolver.hpp"
#include "AbstractLinearParabolicPdeSystemForCoupledOdeSystem.hpp"
#include "TetrahedralMesh.hpp"
#include "BoundaryConditionsContainer.hpp"
#include "AbstractInhomogenousOdeSystemForCoupledPdeSystem.hpp"
#include "InhomogenousParabolicPdeForCoupledOdeSystem_templated.hpp"
#include "AbstractOdeSystemForCoupledPdeSystem.hpp"
#include "CvodeAdaptor.hpp"
#include "BackwardEulerIvpOdeSolver.hpp"
#include "Warnings.hpp"
#include "VtkMeshWriter.hpp"
#include "StateVariableRegister.hpp"

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <boost/shared_ptr.hpp>

// Same as other class but the numberOfStateVariables to the ode terms are not all equal.
// The ode's change on a nodal basis

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM=ELEMENT_DIM, unsigned PROBLEM_DIM=1>
class InhomogenousCoupledPdeOdeSolverTemplated
    : public AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>,
      public AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>
{
private:

    /** Pointer to the mesh. */
    AbstractTetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* mpMesh;

    /** The PDE system to be solved. */
    InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* mpPdeSystem;

    /** Vector of pointers to ODE systems, defined at nodes. */
    std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> mOdeSystemsAtNodes;

    /** The values of the ODE system state variables, interpolated at a quadrature point. */
    std::vector<double> mInterpolatedOdeStateVariables;

    /** The ODE solvers. */
    std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> mpOdeSolvers;

    /**
     * A sampling timestep for writing results to file. Set to
     * PdeSimulationTime::GetPdeTimeStep() in the constructor;
     * may be overwritten using the SetSamplingTimeStep() method.
     */
    double mSamplingTimeStep;

    /** Whether ODE systems are present (if not, then the system comprises coupled PDEs only). */
    bool mOdeSystemsPresent;

    /** Meta results file for VTK. */
    out_stream mpVtkMetaFile;

    /**
     * Whether the output directory should be cleared before solve or not. False by default.
     * Can be changed when setting the output directory
     */
    bool mClearOutputDirectory;

    // for calculating the total mass, interpolate the nodal solution and add to sum
    bool mCalculateTotalMass = true;

    std::vector<double> mTotalMass = {0.0};

    std::vector<double> mOldTotalMass = {0.0};

    bool mInterpolateStateVariable = true;

 //   std::vector<double> mValueAtX(PROBLEM_DIM,0.0);

    // for calculating the trace over a line segment
    bool mCalculateSliceMass = true;

    unsigned mInterpolationNodeCount =0;

    unsigned mNodesInElement =3;

    bool mInterpolatePosition=true;

  //  ChastePoint<SPACE_DIM> mX(0,0,0);

    std::vector<std::vector<double>> mSliceMass;

    std::vector<std::vector<double>> mSlicePositions;

    /**
     * Write the current results to mpVtkMetaFile.
     */
    void WriteVtkResultsToFile();

    /**
     * @return the term to be added to the element stiffness matrix.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeMatrixTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * @return the term to be added to the element stiffness vector.
     *
     * @param rPhi The basis functions, rPhi(i) = phi_i, i=1..numBases
     * @param rGradPhi Basis gradients, rGradPhi(i,j) = d(phi_j)/d(X_i)
     * @param rX The point in space
     * @param rU The unknown as a vector, u(i) = u_i
     * @param rGradU The gradient of the unknown as a matrix, rGradU(i,j) = d(u_i)/d(X_j)
     * @param pElement Pointer to the element
     */
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> ComputeVectorTerm(
        c_vector<double, ELEMENT_DIM+1>& rPhi,
        c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
        ChastePoint<SPACE_DIM>& rX,
        c_vector<double,PROBLEM_DIM>& rU,
        c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
        Element<ELEMENT_DIM, SPACE_DIM>* pElement);

    /**
     * Reset the member variable mInterpolatedOdeStateVariables.
     */
    void ResetInterpolatedQuantities();

    /**
     * Update the member variable mInterpolatedOdeStateVariables by computing the
     * interpolated value of each ODE state variable at each Gauss point.
     *
     * @param phiI
     * @param pNode pointer to a Node
     */
    void IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode);

    /**
     * Initialise method: sets up the linear system (using the mesh to
     * determine the number of unknowns per row to preallocate) if it is not
     * already set up. Can use an initial solution as PETSc template,
     * or base it on the mesh size.
     *
     * @param initialSolution Initial solution (defaults to NULL) for PETSc to use as a template.
     */
    void InitialiseForSolve(Vec initialSolution=NULL);

    /**
     * Completely set up the linear system that has to be solved each timestep.
     *
     * @param currentSolution The current solution which can be used in setting up
     *  the linear system if needed (NULL if there isn't a current solution)
     * @param computeMatrix Whether to compute the LHS matrix of the linear system
     *   (mainly for dynamic solves).
     */
    void SetupLinearSystem(Vec currentSolution, bool computeMatrix);

public:

    /**
     * Constructor.
     *
     * @param pMesh pointer to the mesh
     * @param pPdeSystem pointer to the PDE system
     * @param pBoundaryConditions pointer to the boundary conditions.
     * @param odeSystemsAtNodes optional vector of pointers to ODE systems, defined at nodes
     * @param pOdeSolver optional pointer to an ODE solver (defaults to NULL)
     */
    InhomogenousCoupledPdeOdeSolverTemplated(TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
                                    InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
                                    BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
                                    std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes=std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*>(),
                                    std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> pOdeSolvers=std::vector<boost::shared_ptr<AbstractIvpOdeSolver>>());

    /**
     * Destructor.
     * If an ODE system is present, the pointers to the ODE system objects are deleted here.
     */
    ~InhomogenousCoupledPdeOdeSolverTemplated();

    /**
     * Overridden PrepareForSetupLinearSystem() method.
     * Pass the current solution to the PDE system to the ODE system and solve it over the next timestep.
     *
     * @param currentPdeSolution the solution to the PDE system at the current time
     */
    void PrepareForSetupLinearSystem(Vec currentPdeSolution);

    /**
     * Set mOutputDirectory.
     *
     * @param outputDirectory the output directory to use
     * @param clearDirectory whether to clear outputDirectory or not. Note that the actual clearing happens when you call SolveAndWriteResultsToFile().
     *                       False by default.
     */
    void SetOutputDirectory(std::string outputDirectory, bool clearDirectory=false);

    /**
     * Set mSamplingTimeStep.
     *
     * @param samplingTimeStep the sampling timestep to use
     */
    void SetSamplingTimeStep(double samplingTimeStep);

    /**
     * Solve the coupled PDE/ODE system over the pre-specified time interval,
     * and record results using mSamplingTimeStep.
     */
    void SolveAndWriteResultsToFile();

    /**
     * Write the solution to VTK. Called by SolveAndWriteResultsToFile().
     *
     * @param solution the solution of the coupled PDE/ODE system
     * @param numTimeStepsElapsed the number of timesteps that have elapsed
     */
    void WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed);

    /**
     * Get a pointer to the ODE system defined at a given node.
     *
     * @param index the global index of a node in the mpMesh
     * @return mOdeSystemsAtNodes[index]
     */
    AbstractOdeSystemForCoupledPdeSystem* GetOdeSystemAtNode(unsigned index);

    void ResetInterpolatedQuantitiesOnNewTimeStep();

    void OutputInterpolatedQuantitiesOnTimestep();
};

///////////////////////////////////////////////////////////////////////////////////
// Implementation
///////////////////////////////////////////////////////////////////////////////////

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double, PROBLEM_DIM, SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm( - start"<<std::endl;
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_matrix<double, PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1)> matrix_term = zero_matrix<double>(PROBLEM_DIM*(ELEMENT_DIM+1), PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate matrix_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);

        // in general this should be looking to the domain field to determine diffusion properties, interpolation required?

        c_matrix<double, SPACE_DIM, SPACE_DIM> this_pde_diffusion_term = mpPdeSystem->ComputeDiffusionTerm(rX, pde_index, pElement);

        c_matrix<double, 1*(ELEMENT_DIM+1), 1*(ELEMENT_DIM+1)> this_stiffness_matrix =
            prod(trans(rGradPhi), c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>(prod(this_pde_diffusion_term, rGradPhi)) )
                + timestep_inverse * this_dudt_coefficient * outer_prod(rPhi, rPhi);

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            for (unsigned j=0; j<ELEMENT_DIM+1; j++)
            {
                matrix_term(i*PROBLEM_DIM + pde_index, j*PROBLEM_DIM + pde_index) = this_stiffness_matrix(i,j);
            }
        }
    }
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeMatrixTerm( - end"<<std::endl;
    return matrix_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm(
    c_vector<double, ELEMENT_DIM+1>& rPhi,
    c_matrix<double, SPACE_DIM, ELEMENT_DIM+1>& rGradPhi,
    ChastePoint<SPACE_DIM>& rX,
    c_vector<double,PROBLEM_DIM>& rU,
    c_matrix<double,PROBLEM_DIM,SPACE_DIM>& rGradU,
    Element<ELEMENT_DIM, SPACE_DIM>* pElement)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm( - start"<<std::endl;
    double timestep_inverse = PdeSimulationTime::GetPdeTimeStepInverse();
    c_vector<double, PROBLEM_DIM*(ELEMENT_DIM+1)> vector_term;
    vector_term = zero_vector<double>(PROBLEM_DIM*(ELEMENT_DIM+1));

    // Loop over PDEs and populate vector_term
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        // property of the pde not the ode systems
        double this_dudt_coefficient = mpPdeSystem->ComputeDuDtCoefficientFunction(rX, pde_index);

        // property of the ode systems; returns indexed state of the interpolated state variables, odes already solved
        double this_source_term = mpPdeSystem->ComputeSourceTerm(rX, rU, mInterpolatedOdeStateVariables, pde_index);

        c_vector<double, ELEMENT_DIM+1> this_vector_term;
        this_vector_term = (this_source_term + timestep_inverse*this_dudt_coefficient*rU(pde_index))* rPhi;

        for (unsigned i=0; i<ELEMENT_DIM+1; i++)
        {
            vector_term(i*PROBLEM_DIM + pde_index) = this_vector_term(i);
        }
    }
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ComputeVectorTerm( - end"<<std::endl;
    return vector_term;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ResetInterpolatedQuantities()
{
    mInterpolatedOdeStateVariables.clear();

    if (mOdeSystemsPresent)
    {
        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();
        mInterpolatedOdeStateVariables.resize(num_state_variables, 0.0);
    }

    // reset for interpolation of slice mass
  //  ChastePoint<SPACE_DIM> tempX(0,0,0)
  //  mX = tempX;
    mInterpolationNodeCount = 0;

  //  std::vector<double> temp(PROBLEM_DIM,0.0);

   // mValueAtX = temp;

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::IncrementInterpolatedQuantities(double phiI, const Node<SPACE_DIM>* pNode)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated::IncrementInterpolatedQuantities"<<std::endl;
    if (mOdeSystemsPresent)
    {
        unsigned matchedIndex;
        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();
        std::vector<std::string> pde_variable_register = mpPdeSystem->GetStateVariableRegister() -> GetStateVariableRegisterVector();

        for (unsigned i=0; i<num_state_variables; i++)
        {
            // each node may not have the total number of states in the ode system
            // select using the statevariable registers at the nodes, if state variable isn't present the value is 0

            if(mOdeSystemsAtNodes[pNode->GetIndex()] -> GetStateVariableRegister() -> IsStateVariablePresent(pde_variable_register[i]))
            {
                matchedIndex = mOdeSystemsAtNodes[pNode->GetIndex()] -> GetStateVariableRegister() -> RetrieveStateVariableIndex(pde_variable_register[i]);

                //rGetStateVariables() returns the current values of the state variables, index
                mInterpolatedOdeStateVariables[i] += phiI * mOdeSystemsAtNodes[pNode->GetIndex()]->rGetStateVariables()[matchedIndex];
                // rGetStateVariables if the value of the ode states variables at the nodes, size < num_state_vars in the pde system
            }

        }

    }

    if(mCalculateTotalMass)
    {
        // interpolate the previous timestep solution at nodes
        // sum the interpolated value

        unsigned num_state_variables = mpPdeSystem->GetStateVariableRegister()->GetNumberOfStateVariables();

        ReplicatableVector current_nodal_values(this->mInitialCondition);

        for (unsigned i=0; i<num_state_variables; i++)
        {
            mOldTotalMass[i] += phiI * current_nodal_values[PROBLEM_DIM*pNode->GetIndex()+i];
        }
    }

    if(mInterpolatePosition)
    {
     //   mX.rGetLocation() += phiI*pNode->rGetLocation();
    }

    if(mInterpolateStateVariable)
    {
    //    ReplicatableVector current_nodal_values(this->mInitialCondition);

    //    for (unsigned i=0; i<num_state_variables; i++)
     //   {
      //      mValueAtX[i] += phiI * current_nodal_values[PROBLEM_DIM*pNode->GetIndex()+i];
       // }

    }

    mInterpolationNodeCount++;

    if(mInterpolationNodeCount == mNodesInElement)
    {
        // position and state variable vector are fully known
        ChastePoint<SPACE_DIM> X = AbstractFeVolumeIntegralAssembler<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, true, true, NORMAL>::GetPosition();

        c_vector<double,PROBLEM_DIM> U = AbstractFeVolumeIntegralAssembler<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, true, true, NORMAL>::GetStateVariable();

        if(mCalculateTotalMass)
        {
            for (unsigned i=0; i<PROBLEM_DIM; i++)
            {
                mTotalMass[i] += phiI * U[i];
            }
        }

        if(mCalculateSliceMass)
        {
            //std::cout<<"y: "<<X[1]<<std::endl;
            if(2.85>X[1] && X[1]>2.8)
            {
                // store the x position and the state variable U
                std::vector<double> thisSlicePosition;
                std::vector<double> thisSliceMass;

                for(unsigned i=0; i<SPACE_DIM; i++)
                {
                    thisSlicePosition.push_back(X[i]);
                    //std::cout<<"x: "<<X[i]<<std::endl;
                }

                for(unsigned j=0; j<PROBLEM_DIM; j++)
                {
                    thisSliceMass.push_back(U[j]);
                }

                mSlicePositions.push_back(thisSlicePosition);
                mSliceMass.push_back(thisSliceMass);

            }
            // else ignore point
        }

    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::InitialiseForSolve(Vec initialSolution)
{
    if (this->mpLinearSystem == NULL)
    {
        unsigned preallocation = mpMesh->CalculateMaximumContainingElementsPerProcess() + ELEMENT_DIM;
        if (ELEMENT_DIM > 1)
        {
            // Highest connectivity is closed
            preallocation--;
        }
        preallocation *= PROBLEM_DIM;

        /*
         * Use the current solution (ie the initial solution) as the
         * template in the alternative constructor of LinearSystem.
         * This is to avoid problems with VecScatter.
         */
        this->mpLinearSystem = new LinearSystem(initialSolution, preallocation);
    }

    assert(this->mpLinearSystem);
    this->mpLinearSystem->SetMatrixIsSymmetric(true);
    this->mpLinearSystem->SetKspType("cg");
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetupLinearSystem(Vec currentSolution, bool computeMatrix)
{
    this->SetupGivenLinearSystem(currentSolution, computeMatrix, this->mpLinearSystem);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::InhomogenousCoupledPdeOdeSolverTemplated(
        TetrahedralMesh<ELEMENT_DIM, SPACE_DIM>* pMesh,
        InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pPdeSystem,
        BoundaryConditionsContainer<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>* pBoundaryConditions,
        std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> odeSystemsAtNodes,
        std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> pOdeSolvers)
    : AbstractAssemblerSolverHybrid<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM, NORMAL>(pMesh, pBoundaryConditions),
      AbstractDynamicLinearPdeSolver<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>(pMesh),
      mpMesh(pMesh),
      mpPdeSystem(pPdeSystem),
      mOdeSystemsAtNodes(odeSystemsAtNodes),
      mpOdeSolvers(pOdeSolvers),
      mSamplingTimeStep(DOUBLE_UNSET),
      mOdeSystemsPresent(false),
      mClearOutputDirectory(false)
{
    this->mpBoundaryConditions = pBoundaryConditions;
    /*
     * If any ODE systems are passed in to the constructor, then we aren't just
     * solving a coupled PDE system, in which case the number of ODE system objects
     * must match the number of nodes in the finite element mesh.
     */
    if (!mOdeSystemsAtNodes.empty())
    {
        mOdeSystemsPresent = true;
        assert(mOdeSystemsAtNodes.size() == mpMesh->GetNumNodes());

        /*
         * In this case, if an ODE solver is not explicitly passed into the
         * constructor, then we create a default solver.
         */
        if (!mpOdeSolvers[0])
        {
#ifdef CHASTE_CVODE
            for(unsigned i=0; i<mpOdeSolvers.size(); i++)
            {
                mpOdeSolvers[i].reset(new CvodeAdaptor);
            }

#else
            for(unsigned i=0; i<mOdeSystemsAtNodes.size(); i++)
            {
                mpOdeSolvers.push_back(new BackwardEulerIvpOdeSolver(mOdeSystemsAtNodes[i]->GetNumberOfStateVariables()));
            }

#endif //CHASTE_CVODE
        }
    }
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::~InhomogenousCoupledPdeOdeSolverTemplated()
{
    if (mOdeSystemsPresent)
    {
        for (unsigned i=0; i<mOdeSystemsAtNodes.size(); i++)
        {
            delete mOdeSystemsAtNodes[i];
        }
    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem(Vec currentPdeSolution)
{
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem( - start"<<std::endl;
    if (mOdeSystemsPresent)
    {
        double time = PdeSimulationTime::GetTime();
        double next_time = PdeSimulationTime::GetNextTime();
        double dt = PdeSimulationTime::GetPdeTimeStep();

        ReplicatableVector soln_repl(currentPdeSolution);
        std::vector<double> current_soln_this_node(PROBLEM_DIM);

        // Loop over nodes
        for (unsigned node_index=0; node_index<mpMesh->GetNumNodes(); node_index++)
        {
            // Store the current solution to the PDE system at this node
            for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
            {
                double current_soln_this_pde_this_node = soln_repl[PROBLEM_DIM*node_index + pde_index];

                current_soln_this_node[pde_index] = current_soln_this_pde_this_node;
            }

            // Pass it into the ODE system at this node, of full state space dimensions
            mOdeSystemsAtNodes[node_index]->SetPdeSolution(current_soln_this_node);

            // Solve ODE system at this node
            mpOdeSolvers[node_index]->SolveAndUpdateStateVariable(mOdeSystemsAtNodes[node_index], time, next_time, dt);

        }
    }
    //std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::PrepareForSetupLinearSystem( - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetOutputDirectory(std::string outputDirectory, bool clearDirectory)
{
    mClearOutputDirectory = clearDirectory;
    this->mOutputDirectory = outputDirectory;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SetSamplingTimeStep(double samplingTimeStep)
{
    assert(samplingTimeStep >= this->mIdealTimeStep);
    mSamplingTimeStep = samplingTimeStep;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile()
{
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile( - start"<<std::endl;
    // A number of methods must have been called prior to this method
    if (this->mOutputDirectory == "")
    {
        EXCEPTION("SetOutputDirectory() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mTimesSet == false)
    {
        EXCEPTION("SetTimes() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (this->mIdealTimeStep <= 0.0)
    {
        EXCEPTION("SetTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (mSamplingTimeStep == DOUBLE_UNSET)
    {
        EXCEPTION("SetSamplingTimeStep() must be called prior to SolveAndWriteResultsToFile()");
    }
    if (!this->mInitialCondition)
    {
        EXCEPTION("SetInitialCondition() must be called prior to SolveAndWriteResultsToFile()");
    }

//#ifdef CHASTE_VTK
    ResetInterpolatedQuantitiesOnNewTimeStep();
    // Create a .pvd output file
    //std::cout<<"outputfilehandeler: "<<this->mOutputDirectory<<std::endl;

    OutputFileHandler output_file_handler(this->mOutputDirectory, mClearOutputDirectory);

    mpVtkMetaFile = output_file_handler.OpenOutputFile("results.pvd");
    *mpVtkMetaFile << "<?xml version=\"1.0\"?>\n";
    *mpVtkMetaFile << "<VTKFile type=\"Collection\" version=\"0.1\" byte_order=\"LittleEndian\" compressor=\"vtkZLibDataCompressor\">\n";
    *mpVtkMetaFile << "    <Collection>\n";

    std::cout<<"Get output loc: "<<output_file_handler.GetChasteTestOutputDirectory()<<std::endl;
    std::cout<<"Get output dir full path: "<<output_file_handler.GetOutputDirectoryFullPath()<<std::endl;
    std::cout<<"Get relative path: "<<output_file_handler.GetRelativePath()<<std::endl;

    // Write initial condition to VTK
    Vec initial_condition = this->mInitialCondition;

    ReplicatableVector result_repl(this->mInitialCondition);

    WriteVtkResultsToFile(initial_condition, 0);

    // The helper class TimeStepper deals with issues such as small final timesteps so we don't have to
    TimeStepper stepper(this->mTstart, this->mTend, mSamplingTimeStep);

    std::vector<std::string> p_pde_stateVariableNames = mpPdeSystem -> GetStateVariableRegister() ->GetStateVariableRegisterVector();

    // make a vector of file streams to store slice values per pde dim
    std::vector<std::shared_ptr<ofstream> > sliceMassFiles;
    for(unsigned i=0; i<PROBLEM_DIM; i++)
    {
    //    std::ofstream sliceMassFile;
     //   std::string sliceMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sliceMass_"+p_pde_stateVariableNames[i]+".csv";
     //   std::cout<<"sliceMassFilename: "<<sliceMassFilename<<std::endl;
     //   sliceMassFile.open(sliceMassFilename);
     //   sliceMassFiles.push_back(sliceMassFile);
        std::shared_ptr<std::ofstream> sliceMassFile(new std::ofstream);
        std::string sliceMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sliceMass_"+p_pde_stateVariableNames[i]+".csv";
        std::cout<<"sliceMassFilename: "<<sliceMassFilename<<std::endl;
        sliceMassFile -> open(sliceMassFilename);
        sliceMassFiles.push_back(sliceMassFile);
    }

    std::ofstream slicePositionFile;
    std::string slicePositionFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"slicePosition.csv";
    std::cout<<"slicePositionFilename: "<<slicePositionFilename<<std::endl;
    slicePositionFile.open (slicePositionFilename);

    std::ofstream sumCoeffFile;
    std::string sumCoeffFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"sumCoeff.csv";
    std::cout<<"sumCoeffFilename: "<<sumCoeffFilename<<std::endl;
    sumCoeffFile.open (sumCoeffFilename);

    for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        double sumCoeff=0;
        for(unsigned node_index=0;node_index<mpMesh->GetNumNodes();node_index++)
        {
            sumCoeff += result_repl[PROBLEM_DIM*node_index + pde_index];
        }
        sumCoeffFile << sumCoeff<<",";
    }
    sumCoeffFile <<"\n";

    std::ofstream totalMassFile;
    std::string totalMassFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"totalMass.csv";
    std::cout<<"totalMassFilename: "<<totalMassFilename<<std::endl;
    totalMassFile.open (totalMassFilename);

    std::ofstream totalMassOldFile;
    std::string totalMassOldFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"totalMassOld.csv";
    std::cout<<"totalMassFilename: "<<totalMassOldFilename<<std::endl;
    totalMassOldFile.open (totalMassOldFilename);

    std::ofstream timeFile;
    std::string timeFilename = "/home/chaste/testoutput/"+this->mOutputDirectory+"time.csv";
    std::cout<<"timeFilename: "<<timeFilename<<std::endl;
    timeFile.open (timeFilename);

    if(mCalculateTotalMass)
    {
        for(unsigned pdeNum=1; pdeNum<PROBLEM_DIM;pdeNum++)
        {
            mTotalMass.push_back(0.0);
            mOldTotalMass.push_back(0.0);
        }
    }

    // Main time loop, interate the stepper
    while (!stepper.IsTimeAtEnd())
    {
        // Reset start and end times
        this->SetTimes(stepper.GetTime(), stepper.GetNextTime());

        timeFile << stepper.GetTime()<<"\n";

        ResetInterpolatedQuantitiesOnNewTimeStep();

        // Solve the system up to the new end time
        Vec soln = this->Solve();
        ReplicatableVector result_repl(soln);

        // Reset the initial condition for the next timestep
        if (this->mInitialCondition != initial_condition)
        {
            PetscTools::Destroy(this->mInitialCondition);
        }
        this->mInitialCondition = soln;

        // Move forward in time
        stepper.AdvanceOneTimeStep();

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
        {
            double sumCoeff=0;
            for(unsigned node_index=0;node_index<mpMesh->GetNumNodes();node_index++)
            {
                sumCoeff += result_repl[PROBLEM_DIM*node_index + pde_index];
            }
            sumCoeffFile << sumCoeff<<",";
        }
        sumCoeffFile << "\n";

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM;pde_index++)
        {
           totalMassFile << mTotalMass[pde_index]<<",";
        }
        totalMassFile <<"\n";

        for(unsigned pde_index=0; pde_index<PROBLEM_DIM;pde_index++)
        {
           totalMassOldFile << mOldTotalMass[pde_index]<<",";
        }
        totalMassOldFile <<"\n";

        // for a given timestep output the slice data
        for(unsigned pos=0; pos<mSlicePositions.size(); pos++)
        {
            // take the x value of the position
            slicePositionFile << mSlicePositions[pos][0]<<",";
            for(unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
            {
                *sliceMassFiles[pde_index] << mSliceMass[pos][pde_index]<<",";
            }
        }
        // set the new line for the next time trace
        slicePositionFile << "\n";
        for(unsigned i=0; i<PROBLEM_DIM; i++)
        {
            *sliceMassFiles[i] << "\n";
        }

        // Write solution to VTK
        WriteVtkResultsToFile(soln, stepper.GetTotalTimeStepsTaken());
    }

    // close all files
    timeFile.close();
    sumCoeffFile.close();
    totalMassFile.close();
    totalMassOldFile.close();
    slicePositionFile.close();
    for(unsigned i=0; i<PROBLEM_DIM; i++)
    {
        sliceMassFiles[i]->close();
    }

    // Restore saved initial condition to avoid user confusion!
    if (this->mInitialCondition != initial_condition)
    {
        PetscTools::Destroy(this->mInitialCondition);
    }
    this->mInitialCondition = initial_condition;

    // Close .pvd output file
    *mpVtkMetaFile << "    </Collection>\n";
    *mpVtkMetaFile << "</VTKFile>\n";
    mpVtkMetaFile->close();
//#else //CHASTE_VTK
// LCOV_EXCL_START // We only test this in weekly builds
//    WARNING("VTK is not installed and is required for this functionality");
// LCOV_EXCL_STOP
//#endif //CHASTE_VTK
//std::cout<<"InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::SolveAndWriteResultsToFile( - end"<<std::endl;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::WriteVtkResultsToFile(Vec solution, unsigned numTimeStepsElapsed)
{

#ifdef CHASTE_VTK

    // Create a new VTK file for this time step
    std::stringstream time;
    time << numTimeStepsElapsed;
    //std::cout<<this->mOutputDirectory<<std::endl;
    VtkMeshWriter<ELEMENT_DIM, SPACE_DIM> mesh_writer(this->mOutputDirectory, "results_"+time.str(), false);
    // need to ensure StateVariableRegister is defined

    std::vector<std::string> p_pde_stateVariableNames = mpPdeSystem -> GetStateVariableRegister() ->GetStateVariableRegisterVector();

    /*
     * We first loop over PDEs. For each PDE we store the solution
     * at each node in a vector, then pass this vector to the mesh
     * writer.
     */
    ReplicatableVector solution_repl(solution);

    unsigned num_nodes = mpMesh->GetNumNodes();
    for (unsigned pde_index=0; pde_index<PROBLEM_DIM; pde_index++)
    {
        // Store the solution of this PDE at each node
        std::vector<double> pde_index_data;
        pde_index_data.resize(num_nodes, 0.0);
        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            pde_index_data[node_index] = solution_repl[PROBLEM_DIM*node_index + pde_index];
        }

        // Add this data to the mesh writer
        std::stringstream data_name;
        data_name << "PDE variable " << p_pde_stateVariableNames[pde_index];
        mesh_writer.AddPointData(data_name.str(), pde_index_data);
    }

    if (mOdeSystemsPresent)
    {
        /*
         * We cannot loop over ODEs like PDEs, since the solutions are not
         * stored in one place. Therefore we build up a large 'vector of
         * vectors', then pass each component of this vector to the mesh
         * writer.
         */

        std::vector<std::vector<double> > ode_data;
        unsigned num_state_vars = mpPdeSystem -> GetStateVariableRegister() ->GetNumberOfStateVariables();
        ode_data.resize(num_state_vars);
        for (unsigned state_var_index=0; state_var_index<num_state_vars; state_var_index++)
        {
            ode_data[state_var_index].resize(num_nodes, 0.0);
        }

        for (unsigned node_index=0; node_index<num_nodes; node_index++)
        {
            std::vector<double> all_odes_this_node = mOdeSystemsAtNodes[node_index]->rGetStateVariables();
            // this could be of variable size, is of only the states that the ode modifies

            std::vector<std::string> ode_var_names = mOdeSystemsAtNodes[node_index]->GetStateVariableRegister() -> GetStateVariableRegisterVector();
            for (unsigned ode_index=0; ode_index<ode_var_names.size(); ode_index++)
            {
                // for each state variable in the ode system, find and update the corresponding variable in the pde system
                ode_data[mpPdeSystem -> GetStateVariableRegister() -> RetrieveStateVariableIndex(ode_var_names[ode_index])][node_index] = all_odes_this_node[ode_index];

            }

        }

        for (unsigned ode_index=0; ode_index<num_state_vars; ode_index++)
        {
            // ode_index is the state variable
            std::vector<double> ode_index_data = ode_data[ode_index];

            // Add this data to the mesh writer
            std::stringstream data_name;
            data_name << "ODE variable " << p_pde_stateVariableNames[ode_index];
            mesh_writer.AddPointData(data_name.str(), ode_index_data);
        }
    }

    mesh_writer.WriteFilesUsingMesh(*mpMesh);
    *mpVtkMetaFile << "        <DataSet timestep=\"";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << "\" group=\"\" part=\"0\" file=\"results_";
    *mpVtkMetaFile << numTimeStepsElapsed;
    *mpVtkMetaFile << ".vtu\"/>\n";
#endif // CHASTE_VTK

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
AbstractOdeSystemForCoupledPdeSystem* InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::GetOdeSystemAtNode(unsigned index)
{
    return mOdeSystemsAtNodes[index];
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::ResetInterpolatedQuantitiesOnNewTimeStep()
{

    if(mCalculateTotalMass)
    {
        // reset previous timstep value of total mass
        std::fill(mTotalMass.begin(), mTotalMass.end(), 0.0);

        std::fill(mOldTotalMass.begin(), mOldTotalMass.end(), 0.0);
    }

    if(mCalculateSliceMass)
    {
        mSliceMass = std::vector<std::vector<double>> ();

        mSlicePositions = std::vector<std::vector<double>> ();

    }

}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void InhomogenousCoupledPdeOdeSolverTemplated<ELEMENT_DIM, SPACE_DIM, PROBLEM_DIM>::OutputInterpolatedQuantitiesOnTimestep()
{

}

#endif


#ifndef MOUTH_STATUS_
#define MOUTH_STATUS_

#include <opencv2/opencv.hpp>
#include <dlib/opencv.h>

std::vector<cv::Point2d> get2dmouthPoints(dlib::full_object_detection &d)
{
  std::vector<cv::Point2d> mouthPoints;
  mouthPoints.push_back( cv::Point2d( d.part(50).x(), d.part(50).y() ) );    // 0  Top of upper lip, left
  mouthPoints.push_back( cv::Point2d( d.part(51).x(), d.part(51).y() ) );    // 1  Top of upper lip, mid
  mouthPoints.push_back( cv::Point2d( d.part(52).x(), d.part(52).y() ) );    // 2  Top of upper lip, right
  mouthPoints.push_back( cv::Point2d( d.part(56).x(), d.part(56).y() ) );    // 3  Bottom of bottom lip, right
  mouthPoints.push_back( cv::Point2d( d.part(57).x(), d.part(57).y() ) );    // 4  Bottom of bottom lip, mid
  mouthPoints.push_back( cv::Point2d( d.part(58).x(), d.part(58).y() ) );    // 5  Bottom of bottom lip, left
  mouthPoints.push_back( cv::Point2d( d.part(61).x(), d.part(61).y() ) );    // 6  Bottom of upper lip, left
  mouthPoints.push_back( cv::Point2d( d.part(62).x(), d.part(62).y() ) );    // 7  Bottom of upper lip, mid
  mouthPoints.push_back( cv::Point2d( d.part(63).x(), d.part(63).y() ) );    // 8  Bottom of upper lip, right
  mouthPoints.push_back( cv::Point2d( d.part(65).x(), d.part(65).y() ) );    // 9  Top of bottom lip, right
  mouthPoints.push_back( cv::Point2d( d.part(66).x(), d.part(66).y() ) );    // 10 Top of bottom lip, mid
  mouthPoints.push_back( cv::Point2d( d.part(67).x(), d.part(67).y() ) );    // 11 Top of bottom lip, left
  return mouthPoints;

}

bool checkMouth(dlib::full_object_detection &shape){
    bool mouthOpen;

    auto mouthPoints = get2dmouthPoints(shape);
    float lipDist = (float) sqrt(pow((mouthPoints[10].x - mouthPoints[7].x),2)+pow((mouthPoints[10].y - mouthPoints[7].y),2));
    float lipThickness = (float) sqrt(pow((mouthPoints[1].x - mouthPoints[7].x),2)+pow((mouthPoints[1].y - mouthPoints[7].y),2))/2+
    sqrt(pow((mouthPoints[4].x - mouthPoints[10].x),2)+pow((mouthPoints[4].y - mouthPoints[10].y),2))/2;

    if (lipDist >= 1.0*lipThickness){
        mouthOpen = true;
    } else {
        mouthOpen = false;
    }
    return(mouthOpen);
    // Print bool : mouth open, closed
}

#endif


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class TimeDepentendStokesProblem : public Problem
	{
	public:
		TimeDepentendStokesProblem(const std::string &name);

		virtual bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }

		bool is_time_dependent() const override { return is_time_dependent_; }
		bool is_constant_in_time() const override { return !is_time_dependent_; }
		virtual void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		virtual void set_parameters(const json &params) override;

	protected:
		bool is_time_dependent_;
	};

	class ConstantVelocity : public TimeDepentendStokesProblem
	{
	public:
		ConstantVelocity(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	};

	class TwoSpheres : public TimeDepentendStokesProblem
	{
	public:
		TwoSpheres(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
		void initial_density(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;
	};

	class DrivenCavity : public TimeDepentendStokesProblem
	{
	public:
		DrivenCavity(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	};

	class DrivenCavityC0 : public TimeDepentendStokesProblem
	{
	public:
		DrivenCavityC0(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	};

	class DrivenCavitySmooth : public TimeDepentendStokesProblem
	{
	public:
		DrivenCavitySmooth(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	};

	class Flow : public TimeDepentendStokesProblem
	{
	public:
		Flow(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

	private:
		int inflow_;
		int outflow_;

		int flow_dir_;

		double inflow_amout_;
		double outflow_amout_;
	};

class FlowWithObstacle : public TimeDepentendStokesProblem
{
public:
	FlowWithObstacle(const std::string &name);

	void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	bool is_rhs_zero() const override { return true; }

	void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	void set_parameters(const json &params) override;

private:
	double U_;
};

class Kovnaszy : public Problem
{
	public:
		Kovnaszy(const std::string &name);

		bool has_exact_sol() const override { return true; }
		bool is_rhs_zero() const override { return true; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return is_time_dependent_; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	public:
		double viscosity_;
		bool is_time_dependent_;
};

class CornerFlow : public TimeDepentendStokesProblem
{
public:
	CornerFlow(const std::string &name);

	void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	bool is_rhs_zero() const override { return true; }

	void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	void set_parameters(const json &params) override;

private:
	double U_;
};

class Lshape : public TimeDepentendStokesProblem
{
public:
	Lshape(const std::string &name);

	void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
	bool is_rhs_zero() const override { return true; }

	void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	void set_parameters(const json &params) override;

private:
	double U_;
};

	class UnitFlowWithObstacle : public TimeDepentendStokesProblem
	{
	public:
		UnitFlowWithObstacle(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return true; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

	private:
		double U_;
		int inflow_;
		int dir_;
	};

	class TaylorGreenVortexProblem : public Problem
	{
	public:
		TaylorGreenVortexProblem(const std::string &name);

		bool has_exact_sol() const override { return true; }
		bool is_rhs_zero() const override { return true; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return true; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	public:
		double viscosity_;
	};

	class StokesLawProblem : public Problem
	{
	public:
		StokesLawProblem(const std::string &name);

		bool has_exact_sol() const override { return true; }
		bool is_rhs_zero() const override { return true; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return is_time_dependent_; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	public:
		double viscosity_, radius;
		bool is_time_dependent_;
	};

	class Airfoil : public Problem
	{
	public:
		Airfoil(const std::string &name);

		bool has_exact_sol() const override { return true; }
		bool is_rhs_zero() const override { return true; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return is_time_dependent_; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	public:
		bool is_time_dependent_;
	};

	class SimpleStokeProblemExact : public ProblemWithSolution
	{
	public:
		SimpleStokeProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }

		void set_parameters(const json &params) override;

	private:
		int func_;
	};

	class SineStokeProblemExact : public ProblemWithSolution
	{
	public:
		SineStokeProblemExact(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override;

		bool is_scalar() const override { return false; }
	};

	class TransientStokeProblemExact : public Problem
	{
	public:
		TransientStokeProblemExact(const std::string &name);

		bool has_exact_sol() const override { return true; }
		bool is_rhs_zero() const override { return false; }
		bool is_scalar() const override { return false; }
		bool is_time_dependent() const override { return true; }
		bool is_constant_in_time() const override { return false; }

		void initial_solution(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;

		void exact(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void exact_grad(const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

	private:
		int func_;
		double viscosity_;
	};

} // namespace polyfem


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#pragma once

#include <mc_solver/ContactConstraint.h>

#include <Eigen/Core>
#include <memory>
#include <sstream>

typedef std::array<double, 3> array3d;

namespace mc_solver
{

struct ContactConstrCastResult
{
  tasks::qp::ContactAccConstr * acc;
  tasks::qp::ContactSpeedConstr * speed;
  tasks::qp::ContactPosConstr * pos;
};

ContactConstrCastResult get_contact_constr(ContactConstraint & cc)
{
  ContactConstrCastResult res{nullptr, nullptr, nullptr};
  res.acc = dynamic_cast<tasks::qp::ContactAccConstr *>(cc.contactConstr.get());
  res.speed = dynamic_cast<tasks::qp::ContactSpeedConstr *>(cc.contactConstr.get());
  res.pos = dynamic_cast<tasks::qp::ContactPosConstr *>(cc.contactConstr.get());
  return res;
}

} // namespace mc_solver


/*
 * Copyright (C) 2011-2014 Daniel Scharrer
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the author(s) be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/*!
 * \file
 *
 * SHA-1 hashing routines.
 */
#ifndef INNOEXTRACT_CRYPTO_SHA1_HPP
#define INNOEXTRACT_CRYPTO_SHA1_HPP

#include <boost/cstdint.hpp>

#include "crypto/iteratedhash.hpp"
#include "util/endian.hpp"

namespace crypto {

class sha1_transform {

public:

	typedef boost::uint32_t hash_word;
	typedef util::big_endian byte_order;
	static const size_t offset = 1;
	static const size_t block_size = 64;
	static const size_t hash_size = 20;

	static void init(hash_word * state);

	static void transform(hash_word * digest, const hash_word * data);
};

typedef iterated_hash<sha1_transform> sha1;

} // namespace crypto

#endif // INNOEXTRACT_CRYPTO_SHA1_HPP


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

/// @file
/// Implements full BTE calculations defined in beyondRTA.hpp.

#include <iostream>
#include <map>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <utilities.hpp>
#include <beyondRTA.hpp>

namespace alma {
namespace beyondRTA {
Eigen::MatrixXd calc_kappa(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    const std::vector<alma::Threeph_process>& threeph_procs,
    const std::vector<alma::Twoph_process>& twoph_procs,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    double T,
    bool iterative,
    boost::mpi::communicator& comm) {
    typedef std::array<int, 3> idx_triplet;
    typedef std::array<int, 2> idx_pair;

    // GATHER INFORMATION FOR FULL BTE SYSTEM AND DECLARE SOME VARIABLES

    int Ngridpoints = grid.nqpoints;

    // determine number of irreducible q-points
    int Nclasses = grid.get_nequivalences();

    int Nbranches = grid.get_spectrum_at_q(0).omega.size();
    int Ntot = Nclasses * Nbranches;

    // Unknowns in the linear system
    // (factor 3: each H unknown is a vector with x,y,z components)
    Eigen::VectorXd H(3 * Ntot);

    // stores heat capacities of the irreducible points
    Eigen::VectorXd C(Ntot);
    C.setConstant(0.0);

    // stores relaxation times of the irreducible points
    Eigen::VectorXd tau(Ntot);
    tau.setConstant(0.0);

    // stores phonon frequencies of the irreducible points
    Eigen::VectorXd omega(Ntot);
    omega.setConstant(0.0);

    // stores group velocity vectors of the irreducible points
    Eigen::MatrixXd vg(Ntot, 3);
    vg.fill(0.0);

    // table that maps a single-index value to the corresponding unknown
    std::map<int, int> idx_to_unknown;

    // table that maps an unknown to the single-index
    std::vector<int> unknown_to_idx;
    unknown_to_idx.resize(Ntot);

    // Gamma coefficients for 3-phonon processes
    std::map<idx_triplet, double> Gamma_plus;
    std::map<idx_triplet, double> Gamma_minus;

    // Gamma coefficients for 2-phonon processes
    std::map<idx_pair, double> Gamma2;

    // GATHER PHONON PROPERTIES FOR IRREDUCIBLE Q-POINTS

    int unknownCounter = 0;
    double C_factor = 1e27 * alma::constants::kB / Ngridpoints / poscar.V;

    // scan over all equivalence classes in the grid

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        int nqbase = grid.get_representative(nclass);
        auto spectrum = grid.get_spectrum_at_q(nqbase);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            // register this phonon mode in the mapping tables
            int single_idx = nbranch * Ngridpoints + nqbase;
            idx_to_unknown[single_idx] = unknownCounter;
            unknown_to_idx[unknownCounter] = single_idx;

            // store heat capacity
            C(unknownCounter) = C_factor * alma::bose_einstein_kernel(
                                               spectrum.omega[nbranch], T);

            // store relaxation time
            double mytau =
                (w0(nbranch, nqbase) == 0.) ? 0. : (1. / w0(nbranch, nqbase));
            tau(unknownCounter) = mytau;

            // store phonon frequency
            omega(unknownCounter) = spectrum.omega(nbranch);

            // store group velocity vector
            Eigen::Vector3d myvg = spectrum.vg.col(nbranch);
            vg(unknownCounter, 0) = myvg(0);
            vg(unknownCounter, 1) = myvg(1);
            vg(unknownCounter, 2) = myvg(2);

            // update unkownCounter
            unknownCounter++;
        }
    }

    // CALCULATE NON-RTA CONDUCTIVITY BY SOLVING LINEAR SYSTEM

    // build the list of Gamma values for 2-phonon processes

    for (std::size_t nproc = 0; nproc < twoph_procs.size(); nproc++) {
        // retrieve Gamma value
        alma::Twoph_process myprocess = twoph_procs.at(nproc);
        double Gamma = myprocess.compute_gamma(poscar, grid);

        // obtain single indices
        int idx1 = myprocess.alpha[0] * Ngridpoints + myprocess.q[0];
        int idx2 = myprocess.alpha[1] * Ngridpoints + myprocess.q[1];

        // store Gamma value in the map
        idx_pair key({{idx1, idx2}});
        Gamma2[key] = Gamma;
    } // done scanning over all 2-phonon processes

    // build the list of Gamma values for 3-phonon processes

    for (std::size_t nproc = 0; nproc < threeph_procs.size(); nproc++) {
        alma::Threeph_process myprocess = threeph_procs.at(nproc);
        double Gamma = myprocess.compute_gamma(grid, T);
        bool gammaplus = (myprocess.type == alma::threeph_type::absorption);

        // store Gamma value

        int idx1 = myprocess.alpha[0] * Ngridpoints + myprocess.q[0];
        int idx2 = myprocess.alpha[1] * Ngridpoints + myprocess.q[1];
        int idx3 = myprocess.alpha[2] * Ngridpoints + myprocess.q[2];

        idx_triplet key({{idx1, idx2, idx3}});

        if (gammaplus) {
            Gamma_plus[key] = Gamma;
        }
        else {
            Gamma_minus[key] = Gamma;
        }
    } // done scanning over all 3-phonon processes

    // build system of equations "A*H = B"

    Eigen::MatrixXd A(3 * Ntot, 3 * Ntot);
    A.fill(0.0);

    Eigen::VectorXd B(3 * Ntot);

    // x-components of omega*MFP_RTA
    B.segment(0, Ntot) = omega.array() * tau.array() * vg.col(0).array();
    // y-components of omega*MFP_RTA
    B.segment(Ntot, Ntot) = omega.array() * tau.array() * vg.col(1).array();
    // z-components of omega*MFP_RTA
    B.segment(2 * Ntot, Ntot) = omega.array() * tau.array() * vg.col(2).array();

    // contributions from 3-phonon absorption processes
    for (auto& it3 : Gamma_plus) {
        idx_triplet key = it3.first;
        double Gamma = it3.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // PROCESS CHILD POINT 1

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0; //

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) +=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }

        // PROCESS CHILD POINT 2

        // obtain unknownIndex for the parent of the non-irreducible point
        child_nq = key[2] % Ngridpoints;
        mybranch = key[2] / Ngridpoints;
        parent_nq = grid.getParentIdx(child_nq);
        colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        symmID = grid.getSymIdxToParent(child_nq);
        symmop_idx = symmID / 2;
        // account for time reversal if needed
        sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // contributions from 3-phonon emission processes

    for (auto& it3 : Gamma_minus) {
        idx_triplet key = it3.first;
        double Gamma = it3.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // PROCESS CHILD POINT 1

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    0.5 * R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }

        // PROCESS CHILD POINT 2

        // obtain unknownIndex for the parent of the non-irreducible point
        child_nq = key[2] % Ngridpoints;
        mybranch = key[2] / Ngridpoints;
        parent_nq = grid.getParentIdx(child_nq);
        colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        symmID = grid.getSymIdxToParent(child_nq);
        symmop_idx = symmID / 2;
        // account for time reversal if needed
        sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer, being its transpose,
        // corrected for sign.
        R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    0.5 * R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // Share the contributions from 3-phonon processes among cores and
    // add them together.
    if (comm.size() > 1) {
        Eigen::MatrixXd my_A(A);
        A.fill(0.);
        boost::mpi::all_reduce(
            comm, my_A.data(), my_A.size(), A.data(), std::plus<double>());
    }

    // contributions from 2-phonon processes
    for (auto& it2 : Gamma2) {
        idx_pair key = it2.first;
        double Gamma = it2.second;

        // obtain unknownIndex for the irreducible point
        int rowBase = idx_to_unknown[key[0]];

        // obtain unknownIndex for the parent of the non-irreducible point
        std::size_t child_nq = key[1] % Ngridpoints;
        std::size_t mybranch = key[1] / Ngridpoints;
        std::size_t parent_nq = grid.getParentIdx(child_nq);
        int colBase = idx_to_unknown[mybranch * Ngridpoints + parent_nq];

        // obtain the rotation matrix that maps the child point to its
        // irreducible parent
        std::size_t symmID = grid.getSymIdxToParent(child_nq);
        std::size_t symmop_idx = symmID / 2;
        // account for time reversal if needed
        double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

        Eigen::Matrix3d ROT_buffer;
        ROT_buffer.col(0) =
            syms.rotate_v(Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
        ROT_buffer.col(1) =
            syms.rotate_v(Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
        ROT_buffer.col(2) =
            syms.rotate_v(Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

        // obtain the rotation matrix R for which H_child = R*H_parent.
        // This is the inverse of ROT_buffer (being its transpose)
        // corrected for sign.
        Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

        // register contributions to the linear system

        for (int nRrow = 0; nRrow < 3; nRrow++) {
            for (int nRcol = 0; nRcol < 3; nRcol++) {
                A(rowBase + nRrow * Ntot, colBase + nRcol * Ntot) -=
                    R(nRrow, nRcol) * tau(rowBase) * Gamma;
            }
        }
    }

    // diagonal elements in the system
    for (int diag_idx = 0; diag_idx < 3 * Ntot; diag_idx++) {
        A(diag_idx, diag_idx) += 1.0;
    }

    // Normalise each row for better numerical stability

    for (int nrow = 0; nrow < 3 * Ntot; nrow++) {
        double scale = A.row(nrow).array().abs().maxCoeff();
        A.row(nrow) /= scale;
        B(nrow) /= scale;
    }

    // SOLVE SYSTEM

    if (iterative) {
        Eigen::BiCGSTAB<Eigen::MatrixXd> solver;
        solver.compute(A);

        // RTA solution to be used as initial guess
        Eigen::VectorXd H_guess(3 * Ntot);

        H_guess.segment(0, Ntot) =
            omega.array() * tau.array() * vg.col(0).array();
        H_guess.segment(Ntot, Ntot) =
            omega.array() * tau.array() * vg.col(1).array();
        H_guess.segment(2 * Ntot, Ntot) =
            omega.array() * tau.array() * vg.col(2).array();

        H = solver.solveWithGuess(B, H_guess);
    }

    else {
        H = A.partialPivLu().solve(B);
    }

    double rel_err = (A * H - B).norm() / B.norm();

    if (rel_err > 1e-3) {
        std::cout << "alma::beyondRTA::calc_kappa > WARNING:" << std::endl;
        std::cout << "solution of linear system might be unstable."
                  << std::endl;
        std::cout << "Relative error metric = " << rel_err << std::endl;
    }

    // PROCESS THE LINEAR SYSTEM SOLUTION

    // construct "generalised MFPs"
    Eigen::MatrixXd MFP_nonRTA(Ntot, 3);
    MFP_nonRTA.col(0) = H.segment(0, Ntot).array() / omega.array();
    MFP_nonRTA.col(1) = H.segment(Ntot, Ntot).array() / omega.array();
    MFP_nonRTA.col(2) = H.segment(2 * Ntot, Ntot).array() / omega.array();

    // fix potential NaN/Inf problems
    for (int n = 0; n < Ntot; n++) {
        if (omega(n) <= 0.0) {
            MFP_nonRTA(n, 0) = 0.0;
            MFP_nonRTA(n, 1) = 0.0;
            MFP_nonRTA(n, 2) = 0.0;
        }
    }

    // symmetrise obtained MFP_nonRTA

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        // obtain the q-index of the class representative
        std::size_t nq_parent = grid.get_representative(nclass);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int single_idx = nbranch * Ngridpoints + nq_parent;
            int unknownIndex = idx_to_unknown[single_idx];

            Eigen::Vector3d MFP_parent =
                MFP_nonRTA.row(unknownIndex).transpose();
            Eigen::Vector3d MFP_buffer =
                grid.copy_symmetry(nq_parent, syms, MFP_parent);

            MFP_nonRTA.row(unknownIndex) = MFP_buffer.transpose();
        }
    }

    // fix potential NaN/Inf problems
    for (int n = 0; n < Ntot; n++) {
        if (omega(n) <= 0.0) {
            MFP_nonRTA(n, 0) = 0.0;
            MFP_nonRTA(n, 1) = 0.0;
            MFP_nonRTA(n, 2) = 0.0;
        }
    }

    // CALCULATE NON-RTA CONDUCTIVITY

    Eigen::Matrix3d kappatensor;
    kappatensor.fill(0.0);

    for (int nclass = 0; nclass < Nclasses; nclass++) {
        // obtain the q-index of the class representative
        std::size_t nq_parent = grid.get_representative(nclass);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int single_idx = nbranch * Ngridpoints + nq_parent;
            int unknownIndex = idx_to_unknown[single_idx];

            Eigen::Vector3d MFP_parent =
                MFP_nonRTA.row(unknownIndex).transpose();

            for (auto nq_member : grid.get_equivalence(nclass)) {
                // obtain the group velocity vector of the class member
                Eigen::Vector3d vg_member =
                    (grid.get_spectrum_at_q(nq_member)).vg.col(nbranch);

                // obtain the rotation matrix that maps the class member
                // to its irreducible parent
                std::size_t symmID = grid.getSymIdxToParent(nq_member);
                std::size_t symmop_idx = symmID / 2;
                double sign_correction = (symmID % 2 == 0) ? 1.0 : -1.0;

                Eigen::Matrix3d ROT_buffer;
                ROT_buffer.col(0) = syms.rotate_v(
                    Eigen::Vector3d(1.0, 0.0, 0.0), symmop_idx, true);
                ROT_buffer.col(1) = syms.rotate_v(
                    Eigen::Vector3d(0.0, 1.0, 0.0), symmop_idx, true);
                ROT_buffer.col(2) = syms.rotate_v(
                    Eigen::Vector3d(0.0, 0.0, 1.0), symmop_idx, true);

                // obtain the rotation matrix R for which v_child = R*v_parent.
                // This is the inverse of ROT_buffer, being its transpose,
                // corrected for sign.
                Eigen::Matrix3d R = sign_correction * ROT_buffer.transpose();

                // calculate the non-RTA MFP vector of this mode
                Eigen::Vector3d MFP_member = R * MFP_parent;

                // obtain conductivity contribution
                Eigen::Matrix3d outer =
                    (1e-9 * MFP_member) * (1e3 * vg_member.transpose());
                kappatensor += C(unknownIndex) * outer;
            }
        }
    }

    // symmetrise kappa tensor

    Eigen::Matrix3d kappa_accumulated;
    kappa_accumulated.fill(0.0);

    for (std::size_t nsymm = 0; nsymm < syms.get_nsym(); nsymm++) {
        kappa_accumulated += syms.rotate_m<double>(kappatensor, nsymm, true);
    }

    kappatensor = kappa_accumulated / static_cast<double>(syms.get_nsym());

    // RETURN RESULT

    return kappatensor;
} // end of calc_kappa
} // end of namespace beyondRTA
} // end of namespace alma


/*
Copyright (c) 2020 Naomasa Matsubayashi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <boost/program_options.hpp>
#include <sndfile.h>
#include <OpenImageIO/imageio.h>
#include "ifm/fft.h"
#include "ifm/load_monoral.h"
#include "ifm/spectrum_image.h"

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("output,o", boost::program_options::value<std::string>(),  "出力ファイル")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("harmonic,H", boost::program_options::value<bool>()->default_value(false),  "倍音のみ")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能")
    ("denoise,d", boost::program_options::value<bool>()->default_value(true),  "ノイズ除去");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") || !params.count("output") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const std::string output_filename = params["output"].as<std::string>();
  const auto [audio,sample_rate] = ifm::load_monoral( input_filename, true );
  ifm::spectrum_image conv( params["note"].as<int>(), sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  std::cout << "delay: " << delay << std::endl;
  unsigned int width = conv.get_width();
  unsigned int active_width;
  std::vector< uint8_t > image_i;
  bool denoise = params["denoise"].as<bool>();
  if ( params["harmonic"].as<bool>() ) {
    active_width = width / 24;
    image_i.resize( image.size() / 24 );
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      for( unsigned int x = 0; x != active_width; ++x ) {
        auto l = denoise ?
          uint8_t( std::max( 10 * std::log10( image[ ( x + 1 ) * 24 + y * width ] ), -60.f ) * 255 / 60 + 255 ) :
          uint8_t( std::max( 10 * std::log10( image[ ( x + 1 ) * 24 + y * width ] ), -90.f ) * 255 / 90 + 255 );
        image_i[ x + y * active_width ] = l;
      }
    }
  }
  else {
    active_width = width;
    image_i.resize( image.size() * 3 );
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      for( unsigned int x = 0; x != active_width; ++x ) {
        auto l = denoise ?
          uint8_t( std::max( 10 * std::log10( image[ x + y * width ] ), -60.f ) * 255 / 60 + 255 ) :
          uint8_t( std::max( 10 * std::log10( image[ x + y * width ] ), -90.f ) * 255 / 90 + 255 );
        image_i[ ( x + y * active_width ) * 3 ] = l;
        image_i[ ( x + y * active_width ) * 3 + 1 ] = ( x % 24 ) ? l : uint8_t( 0 );
        image_i[ ( x + y * active_width ) * 3 + 2 ] = ( x % 24 ) ? l : uint8_t( 0 );
      }
    }
  }
  using namespace OIIO_NAMESPACE;
  ImageOutput *out = ImageOutput::create( output_filename );
  if ( !out ) {
    std::cerr << "Unable to open output file" << std::endl;
    return -1;
  }
  const unsigned int channels = params["harmonic"].as<bool>() ? 1 : 3;
  ImageSpec spec ( active_width, image_i.size() / active_width / channels, channels, TypeDesc::UINT8);
  out->open( output_filename, spec );
  out->write_image( TypeDesc::UINT8, image_i.data() );
  out->close();
}


#include <cassert>
#include <poll.h>
#include <signal.h>

#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include <Eigen/Dense>
#include <event2/event.h>
#include <nanomsg/nn.h>
#include <nanomsg/pair.h>
#include <nanomsg/reqrep.h>
#include <nanomsg/ws.h>
#include <sqlite3.h>
#include <xwiimote.h>

#include <argh.h>
#include <evhtp.h>
#include <json.hpp>
#include <sqlite_modern_cpp.h>

#include "logging.h"

using namespace std;
using namespace Eigen;
using json = nlohmann::json;
namespace fs = std::experimental::filesystem;

#define CHECKSQL(X, db, msg) do { int rc = X; if( rc != SQLITE_OK ) { \
  FATAL( "{}: {}", msg, sqlite3_errmsg( db )); }} while(0)

static constexpr const char *DATABASE = "/tmp/db.sqlite";
static bool s_should_quit = false;

static xwii_iface* WaitForBalanceBoard ()
{
  auto monitor = xwii_monitor_new( true, false );

  int fd = ::xwii_monitor_get_fd( monitor, true );
  if( fd == -1 )
  {
    FATAL( "XWii FD is invalid" );
  }

  INFO( "Connected to devices" );
  char *dev_path;
  string dev_name;
  while( (dev_path = ::xwii_monitor_poll( monitor )) != nullptr ) {
    INFO( "{}", dev_path );

    ::xwii_iface *dev;
    if( ::xwii_iface_new( &dev, dev_path ) < 0 )
    {
      FATAL( "Couldn't create interface for {}", dev_path );
    }

    char *dev_type;
    if( ::xwii_iface_get_devtype( dev, &dev_type ) < 0 )
    {
      FATAL( "Couldn't get devtype for {}", dev_path );
    }
    dev_name = dev_type;
    free( dev_type );
    free( dev_path );

    INFO( " is a {}", dev_name );

    if( dev_name == "balanceboard" )
    {
      return dev;
    }
  }

  ::xwii_monitor_unref( monitor );

  FATAL( "No balance boards connected" );

  return nullptr;
}

static void HandleBalanceBoard( const ::xwii_event &event,
                               const VectorXd &coefs )
{
//        tl = event.get_abs(2)[0]
//        tr = event.get_abs(0)[0]
//        br = event.get_abs(3)[0]
//        bl = event.get_abs(1)[0]
  Eigen::Vector4d values;
  values <<
    event.v.abs[0].x, event.v.abs[1].x, event.v.abs[2].x, event.v.abs[3].x;

  values = values / 100.0 * 2.20462;
  double weight = values.sum();
  weight = coefs[0]*weight*weight + coefs[1]*weight + coefs[2];

  fmt::print( "Values: {:6.2f} <== {:6.2f} {:6.2f} {:6.2f} {:6.2f}\r",
       weight, values[0], values[1], values[2], values[3] );
  std::cout << std::flush;
}

VectorXd GetCalibrationCoefficients ( sqlite::database &db )
{
  vector<double> scale_values_raw;
  vector<double> wii_values_raw;

  db << "SELECT scale, wii FROM calibration;"
    >> [&]( double scale, double wii ) {
      scale_values_raw.push_back( scale );
      wii_values_raw.push_back( wii );
    };

  VectorXd scale_values{ scale_values_raw.size() };
  VectorXd wii_values{ wii_values_raw.size() };

  for( size_t idx=0; idx < scale_values_raw.size(); ++idx )
  {
    scale_values( idx ) = scale_values_raw[ idx ];
    wii_values( idx ) = wii_values_raw[ idx ];
  }

  // Compute calibration coefficients
  MatrixXd A = MatrixXd::Ones( scale_values.rows(), 3 );
  A.col(0) = wii_values.asDiagonal()*wii_values;
  A.col(1) = wii_values;

  return (A.adjoint() * A).llt().solve( A.adjoint() * scale_values );
}

static VectorXd Sample( sqlite::database &db, ::xwii_iface *iface, int num )
{
  ::xwii_event event;
  int ret = 0;
  int fds_num;
  struct pollfd fds[2];

  memset(fds, 0, sizeof(fds));
  fds[0].fd = 0;
  fds[0].events = POLLIN;
  fds[1].fd = ::xwii_iface_get_fd(iface);
  fds[1].events = POLLIN;
  fds_num = 2;

  ret = xwii_iface_watch(iface, true);
  if (ret)
  {
    ERROR("Cannot initialize hotplug watch descriptor");
  }

  VectorXd coefs = GetCalibrationCoefficients( db );

  vector<double> raw_weights;
  while (num--) {
    ret = poll(fds, fds_num, -1);
    if (ret < 0) {
      if (errno != EINTR) {
        ret = -errno;
        ERROR("Cannot poll fds: {}", ret);
        break;
      }
    }

    ret = xwii_iface_dispatch(iface, &event, sizeof(event));
    if (ret)
    {
      if (ret != -EAGAIN)
      {
        ERROR("Read failed with err: {}", ret);
        break;
      }
    }

    if( event.type == XWII_EVENT_BALANCE_BOARD )
    {
      Eigen::Vector4d values;
      values <<
        event.v.abs[0].x, event.v.abs[1].x, event.v.abs[2].x, event.v.abs[3].x;

      values = values / 100.0 * 2.20462;
      double weight = values.sum();
      weight = coefs[0]*weight*weight + coefs[1]*weight + coefs[2];

      raw_weights.push_back( weight );

      fmt::print( "Values: {:6.2f} <== {:6.2f} {:6.2f} {:6.2f} {:6.2f}\r",
                 weight, values[0], values[1], values[2], values[3] );
      std::cout << std::flush;

      HandleBalanceBoard( event, coefs );
    }
  }

  VectorXd weights( raw_weights.size() );
  for( size_t i=0; i < raw_weights.size(); ++i )
  {
    weights(i) = raw_weights[i];
  }

  return weights;
}

void LoadDefaultCalibration ( sqlite::database &db )
{
  bool has_calibration_data = false;
  db << "SELECT name FROM sqlite_master WHERE type='table' AND name='calibration';"
    >> [&has_calibration_data]( string name ) {
      (void) name;
      has_calibration_data = true;
    };

  if( has_calibration_data ) {
    return;
  }
  else
  {
    WARN( "Missing calibration data, loading default values." );
  }

  db << "DROP TABLE IF EXISTS calibration;";
  db << "CREATE TABLE calibration( "
    "id INTEGER PRIMARY KEY,"
    "scale DOUBLE,"
    "wii DOUBLE );";

  vector<double> scale_values{ 71.5, 52., 193.5, 141., 146.5, 203., 145.5,
    173.5, 143.5 };
  vector<double> wii_values{ 63., 43.65, 185.5, 133.2, 138.6, 194.5, 137.5,
    165.2, 135.6 };
  assert( scale_values.size() == wii_values.size() );

  for( size_t idx=0; idx < scale_values.size(); ++idx )
  {
    db << "INSERT INTO calibration (scale, wii) values (?, ?);"
      << scale_values[idx]
      << wii_values[idx];
  }
}

sqlite::database Sqlite()
{
  sqlite::database db( DATABASE );
  string version;
  db << "SELECT SQLITE_VERSION();"
    >> []( string version ) {
      INFO( "Using SQLite3 version {}", version );
    };
  return db;
}

struct Header
{
  string key;
  string value;
};

void RootCallback( evhtp_request_t *req, void *data )
{
  (void) data;

  if( req->uri->path->full )
  {
    INFO( "Got a request for '{}'", req->uri->path->full );
  }
  else
  {
    INFO( "Unknown query" );
    evhtp_send_reply( req, EVHTP_RES_NOTFOUND );
    return;
  }

  string response = R"(

  <title>Unknown Resource</title>

Unknown Resource

Go Home

  )";

  fs::path file_path{ "."s + req->uri->path->full };
  if( file_path == fs::path( "./" ))
  {
    file_path = { "./index.html" };
  }

  if( fs::exists( file_path ) && fs::is_regular_file( file_path ))
  {
    ifstream file_in{ file_path };
    ostringstream buf;
    buf << file_in.rdbuf();
    response = buf.str();
  }
  else
  {
    WARN( "Unknown path '{}', using default message", file_path );
  }

  evbuffer_add( req->buffer_out, response.c_str(), response.size() );

  Header headers[] = {
    { "Content-Type", "text/html" },
    { "Content-Language", "en" }
  };
  for( const auto& header : headers )
  {
    evhtp_headers_add_header( req->headers_out, evhtp_header_new(
            header.key.c_str(), header.value.c_str(), 1, 1 ));
  }

  evhtp_send_reply( req, EVHTP_RES_OK );
}

void SigintHandler( evutil_socket_t fd, short event, void *arg )
{
  (void) fd;
  (void) event;
  evbase_t *evbase =  reinterpret_cast<evbase_t*>( arg );
  INFO( "Got SIGINT" );
  s_should_quit = true;
  event_base_loopbreak( evbase );
}

void HttpLoop ()
{
  evbase_t *evbase = event_base_new();
  evhtp_t *htp = evhtp_new( evbase, nullptr );
  evhtp_set_glob_cb( htp, "*", RootCallback, nullptr );

  evhtp_bind_socket( htp, "0.0.0.0", 8080, 2048 );

  event *signal_int = evsignal_new( evbase, SIGINT, SigintHandler, evbase );
  event_add( signal_int, nullptr );
  INFO( "Started HTTP server on port 8080" );
  event_base_loop( evbase, 0 );

  evhtp_unbind_socket( htp );
  event_free( signal_int );
  evhtp_free( htp );
  event_base_free( evbase );
  INFO( "Stopped HTTP server" );
}

int main(int argc, char **argv) {
  // DELETE THESE.  Used to suppress unused variable warnings.
  (void)argc;
  (void)argv;

  int socket = nn_socket( AF_SP, NN_REP );
  if( socket < 0 )
  {
    ERROR( "nn_socket failed: {}", nn_strerror( nn_errno() ));
    return -1;
  }
  int opt = NN_WS_MSG_TYPE_TEXT;
  //nn_setsockopt( socket, NN_WS, NN_WS_MSG_TYPE, &opt, sizeof(opt) );
  opt = 500;
  nn_setsockopt( socket, NN_SOL_SOCKET, NN_SNDTIMEO, &opt, sizeof(opt) );
  int endpoint_id = nn_bind( socket, "ws://*:8081" );
  if( endpoint_id < 0 )
  {
    ERROR( "nn_bind failed: {}", nn_strerror( nn_errno() ));
    return -1;
  }

  INFO( "Started NN WebSocket on port 8081" );

  thread http_thread( HttpLoop );

  while( 1 )
  {
    char *buf = nullptr;
    INFO( "Waiting on message" );
    int bytes = nn_recv( socket, &buf, NN_MSG, 0 );
    if( bytes >= 0 )
    {
      // Enforce c-string
      if( buf[bytes-1] != '\0' ) { buf[bytes-1] = '\0'; }
      auto j = json::parse( buf );
      nn_freemsg( buf );

      json r;
      string request = j["request"];
      if( request == "get_users" )
      {
        json users;
        json user;
        user["name"] = "jim";
        user["age"] = 33;
        user["weight"] = 185;
        user["id"] = 1;
        users.push_back( user );

        r["response"] = "all_users";
        r["users"] = users;
      }

      string response = r.dump();
      bytes = nn_send( socket, response.c_str(), response.size()+1, 0 );
      if( bytes < 0 )
      {
        WARN( "nn_send??: {}", nn_strerror( nn_errno() ));
      }
      else
      {
        INFO( "Sent reply: {}", response );
      }

    }
    else
    {
      break;
    }
  }

  http_thread.join();
  INFO( "Goodbye, user" );
  return 0;

  auto iface = WaitForBalanceBoard();

  uint64_t available_types = xwii_iface_available( iface );
  INFO( "Available types: {:x}", available_types );

  int ret =
    xwii_iface_open( iface, available_types | XWII_IFACE_BALANCE_BOARD );

  if( ret != 0 )
  {
    FATAL( "Can't open interface" );
  };

  auto db = Sqlite();
  LoadDefaultCalibration( db );
  Sample( db, iface, 100 );

  return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "Tudat/SimulationSetup/EstimationSetup/orbitDeterminationTestCases.h"

namespace tudat
{
namespace unit_tests
{
BOOST_AUTO_TEST_SUITE( test_estimation_from_positions )

//! This test checks, for double states/observables and double time, if the orbit determination correctly converges
//! when simulating data, perturbing the dynamical parameters, and then retrieving the original parameters
BOOST_AUTO_TEST_CASE( test_EstimationFromPosition )
{
    for( int simulationType = 0; simulationType < 6; simulationType++ )
    {

        std::cout << "=============================================== Running Case: " << simulationType << std::endl;

        // Simulate estimated parameter error.
        Eigen::VectorXd totalError;

        totalError = executePlanetaryParameterEstimation< double, double >( simulationType ).second;

        // Adjust tolerance based on simulation settings
        double toleranceMultiplier = 20.0;

        // Check error.
        for( unsigned int j = 0; j < 3; j++ )
        {
            BOOST_CHECK_SMALL( totalError( j ), toleranceMultiplier * 5.0E-3 );
        }

        for( unsigned int j = 0; j < 3; j++ )
        {
            BOOST_CHECK_SMALL( totalError( j + 3 ), toleranceMultiplier * 1.0E-7 );
        }

        BOOST_CHECK_SMALL( totalError( 6 ), toleranceMultiplier * 1.0E3 );
        std::cout << totalError.transpose( ) << std::endl;
    }

    std::pair< std::shared_ptr< simulation_setup::PodOutput< double > >,
    std::shared_ptr< simulation_setup::PodInput< double, double > > > podDataOutput;
    Eigen::VectorXd estimationError = tudat::unit_tests::executeEarthOrbiterParameterEstimation< double, double >(
                 podDataOutput );

    for( unsigned int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( estimationError( i ) ), 1.0E-5 );
        BOOST_CHECK_SMALL( std::fabs( estimationError( i + 3 ) ), 1.0E-8 );
        BOOST_CHECK_SMALL( std::fabs( estimationError( i + 18 ) ), 1.0E-4 );
    }

    BOOST_CHECK_SMALL( std::fabs( estimationError( 6 ) ), 5.0E-7 );
    BOOST_CHECK_SMALL( std::fabs( estimationError( 7 ) ), 5.0E-7 );
    BOOST_CHECK_SMALL( std::fabs( estimationError( 8 ) ), 5.0E-7 );

    BOOST_CHECK_SMALL( std::fabs( estimationError( 9 ) ), 5.0E-13 );
    BOOST_CHECK_SMALL( std::fabs( estimationError( 10 ) ), 5.0E-13 );
    BOOST_CHECK_SMALL( std::fabs( estimationError( 11 ) ), 5.0E-13 );

    for( unsigned int i = 0; i < 6; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( estimationError( i + 12 ) ), 5.0E-13 );
    }

}

BOOST_AUTO_TEST_SUITE_END( )

}

}