//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MAKE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MAKE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  //------------------------------------------------------------------------------------------------
  // make a pack of double
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values)
                          , bs::sse2_
                          , bd::target_<bs::pack_<bd::double_<Target>,bs::sse_>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;
    using storage_t = typename target_t::storage_type;

    static_assert ( sizeof...(Values) == 2
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return _mm_setr_pd(static_cast<typename target_t::value_type>(vs)...);
    }
  };

  //------------------------------------------------------------------------------------------------
  // make a pack of int64
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename V0, typename V1)
                          , bs::sse2_
                          , bd::target_<bs::pack_<bd::ints64_<Target>,bs::sse_>>
                          , bd::scalar_<bd::unspecified_<V0>>
                          , bd::scalar_<bd::unspecified_<V1>>
                          )
  {
    using target_t  = typename Target::type;

    BOOST_FORCEINLINE
    target_t operator()(Target const&, V0 const& v0, V1 const& v1) const BOOST_NOEXCEPT
    {
      return _mm_set_epi64x(v1, v0);
    }
  };

  //------------------------------------------------------------------------------------------------
  // make a pack of int32
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values)
                          , bs::sse2_
                          , bd::target_<bs::pack_<bd::ints32_<Target>,bs::sse_>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;

    static_assert ( sizeof...(Values) == 4
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return _mm_setr_epi32(static_cast<typename target_t::value_type>(vs)...);
    }
  };

  //------------------------------------------------------------------------------------------------
  // make a pack of int16
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values)
                          , bs::sse2_
                          , bd::target_<bs::pack_<bd::ints16_<Target>,bs::sse_>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;

    static_assert ( sizeof...(Values) == 8
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return _mm_setr_epi16(static_cast<typename target_t::value_type>(vs)...);
    }
  };

  //------------------------------------------------------------------------------------------------
  // make a pack of int8
  BOOST_DISPATCH_OVERLOAD ( make_
                          , (typename Target, typename... Values)
                          , bs::sse2_
                          , bd::target_<bs::pack_<bd::ints8_<Target>,bs::sse_>>
                          , bd::scalar_<bd::unspecified_<Values>>...
                          )
  {
    using target_t  = typename Target::type;

    static_assert ( sizeof...(Values) == 16
                  , "boost::simd::make - Invalid number of parameters"
                  );

    BOOST_FORCEINLINE target_t operator()(Target const&, Values const&... vs) const BOOST_NOEXCEPT
    {
      return _mm_setr_epi8(static_cast<typename target_t::value_type>(vs)...);
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_SQRTVALMAX_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_SQRTVALMAX_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct sqrtvalmax_ : boost::dispatch::constant_value_<sqrtvalmax_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,sqrtvalmax_,boost::dispatch::constant_value_<sqrtvalmax_>);

      struct value_map
      {
        template<typename X>
        static brigand::single_<0x5f7FFFFEU> value(boost::dispatch::single_<X> const&);
        template<typename X>
        static brigand::double_<0x5fEFFFFFFFFFFFFFULL> value(boost::dispatch::double_<X> const&);
        template<typename X>
        static std::integral_constant<X,15> value(boost::dispatch::uint8_<X> const&);

        template<typename X>
        static std::integral_constant<X,255> value(boost::dispatch::uint16_<X> const&);

        template<typename X>
        static std::integral_constant<X,65535> value(boost::dispatch::uint32_<X> const&);

        template<typename X>
        static std::integral_constant<X,4294967296ULL> value(boost::dispatch::uint64_<X> const&);
        template<typename X>
        static std::integral_constant<X,11> value(boost::dispatch::int8_<X> const&);

        template<typename X>
        static std::integral_constant<X,181> value(boost::dispatch::int16_<X> const&);

        template<typename X>
        static std::integral_constant<X,46340> value(boost::dispatch::int32_<X> const&);

        template<typename X>
        static std::integral_constant<X,3037000499LL> value(boost::dispatch::int64_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,sqrtvalmax_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::sqrtvalmax_,sqrtvalmax);
  }

  template<typename T> BOOST_FORCEINLINE auto Sqrtvalmax()
  BOOST_NOEXCEPT_DECLTYPE(detail::sqrtvalmax( boost::dispatch::as_<T>{}))
  {
    return detail::sqrtvalmax( boost::dispatch::as_<T>{} );
  }
} }

#endif


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    PS::F64 calcRadialVelocity() {
        PS::F64 r2 = this->pos * this->pos;
        PS::F64 rv = this->pos * this->vel;
        return (rv / sqrt(r2));
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

class Shell {
public:
    PS::S64 nump; // The number of particles in the shell
    PS::F64 mass; // The total mass in the shell
    PS::F64 ener; // The total internal energy in the shell
    PS::F64 rvel; // Radial velocity in the shell
    NR::Nucleon mele;

    Shell() {
        this->nump = 0;
        this->mass = 0.;
        this->ener = 0.;
        this->rvel = 0.;
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] = 0.;
        }
    }

    void increment(SPHAnalysis & sph) {
        this->nump += 1;
        this->mass += sph.mass;
        this->ener += sph.mass * sph.uene;
        this->rvel += sph.calcRadialVelocity();
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] += sph.mass * sph.cmps[k];
        }
    }

    void reduceAddition(Shell sloc) {
        this->nump = PS::Comm::getSum(sloc.nump);
        this->mass = PS::Comm::getSum(sloc.mass);
        this->ener = PS::Comm::getSum(sloc.ener);
        this->rvel = PS::Comm::getSum(sloc.rvel);
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] = PS::Comm::getSum(sloc.mele[k]);
        }
    }

};

template <class Tsph>
void sphericalizeWhiteDwarf(PS::F64 rmax,
                            PS::F64 drad,
                            PS::F64vec axis,
                            PS::F64    thinv,
                            char * ofile,
                            Tsph & sph) {

    PS::F64 critcth = cos(M_PI / thinv);

    PS::S64 nbin = (PS::S64)(rmax / drad) + 1;
    Shell * sloc = (Shell *)malloc(sizeof(Shell) * nbin);

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos = sph[i].pos;
        sph[i].vel = sph[i].vel;
        PS::F64 rad1 = sqrt(sph[i].pos * sph[i].pos);
        PS::S64 ibin = (PS::S64)(rad1 / drad);
        if(ibin >= nbin) {
            continue;
        }
        PS::F64 cth  = (sph[i].pos * axis) / rad1;
        if(cth < critcth) {
            continue;
        }
        ///////////// Ad hoc method //////////////
        if(sph[i].istar == 0 && sph[i].cmps[1] > 0.2 && rad1 < 3e10) {
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                if(k != 12) {
                    sph[i].cmps[k] = 0.;
                } else {
                    sph[i].cmps[k] = 1.;
                }
            }
            //continue;
        }
        //////////////////////////////////////////
        sloc[ibin].increment(sph[i]);
    }

    Shell * sglb = (Shell *)malloc(sizeof(Shell) * nbin);
    for(PS::S64 ibin = 0; ibin < nbin; ibin++) {
        sglb[ibin].reduceAddition(sloc[ibin]);
    }

    if(PS::Comm::getRank() == 0) {
        PS::S64 nshl = 0;
        PS::F64 mass = 0.;
        PS::F64 ener = 0.;
        PS::F64 rvel = 0.;
        PS::F64 rad0 = 0.;
        PS::F64 menc = 0.;
        NR::Nucleon mele;
        FILE * fp = fopen(ofile, "w");
        for(PS::S64 ibin = 0; ibin < nbin; ibin++) {
            nshl += sglb[ibin].nump;
            mass += sglb[ibin].mass;
            rvel += sglb[ibin].rvel;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                mele[k] += sglb[ibin].mele[k];
            }
            if(nshl < 10000 && ibin != nbin - 1) {
            //if(nshl < 1000 && ibin != nbin - 1) {
            //if(nshl < 100 && ibin != nbin - 1) {
            //if(nshl < 10 && ibin != nbin - 1) {
                continue;
            }

            PS::F64 rad1   = drad * (ibin + 1);
            rvel  = rvel / (PS::S64)nshl;
            NR::Nucleon cmps;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                cmps[k] = mele[k] / mass;
            }

            fprintf(fp, "%+e %+e",
                    rad1, rvel);
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                fprintf(fp, " %+e", cmps[k]);
            }
            fprintf(fp, "\n");

            nshl = 0;
            mass = 0.;
            rvel = 0.;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                mele[k] = 0.;
            }
            rad0 = rad1;
        }
        fclose(fp);
    }

    free(sloc);
    free(sglb);

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::F64 rmax, drad, thinv;
    PS::F64vec axis;
    PS::S64 ibgn, iend;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lf%lf%lf", &rmax, &drad, &thinv);
    fscanf(fp, "%lf%lf%lf", &axis[0], &axis[1], &axis[2]);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fclose(fp);

    {
        PS::F64 vlen = sqrt(axis * axis);
        axis = (1. / vlen) * axis;
    }

    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            fprintf(stderr, "%s is not found.\n", tfile);
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/poly.dat", odir);
        sphericalizeWhiteDwarf(rmax, drad, axis, thinv, ofile, sph);

    }

    PS::Finalize();

    return 0;
}


#ifndef ABSTRACTPDESYSTEMMODIFIER_HPP_
#define ABSTRACTPDESYSTEMMODIFIER_HPP_

//#include <cxxtest/GlobalFixture.h>
//#include "PetscSetupAndFinalize.hpp"
#include "ChasteSerialization.hpp"
#include "ClassIsAbstract.hpp"
#include <boost/shared_ptr.hpp>

#include "AbstractCellBasedSimulationModifier.hpp"
#include "TetrahedralMesh.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include "AbstractOdeSystemForCoupledPdeSystem.hpp"
#include "AbstractLinearParabolicPdeSystemForCoupledOdeSystem.hpp"//mod

#include "AbstractBoundaryCondition.hpp"

#include "VtkMeshWriter.hpp" // shouldn't be needed? for #ifdef CHASTE_VTK bit

#include "ReplicatableVector.hpp"
#include "InhomogenousParabolicPdeForCoupledOdeSystem_templated.hpp"
#include "AbstractInhomogenousOdeSystemForCoupledPdeSystem.hpp"
#include "ChemicalDomainFieldForCellCoupling.hpp"

/**
 * An abstract modifier class containing functionality common to AbstractBoxDomainPdeModifier,
 * AbstractGrowingDomainPdeModifier and their subclasses, which solve a linear elliptic or
 * parabolic PDE coupled to a cell-based simulation.
 */
template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
class AbstractPdeSystemModifier : public AbstractCellBasedSimulationModifier<ELEMENT_DIM,SPACE_DIM>
{
protected:

    ChemicalDomainFieldForCellCoupling<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* mpCoupledDomainField;

    boost::shared_ptr<InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>> mpPdeSystem;

    /**
     * Shared pointer to a boundary condition object.
     */
    boost::shared_ptr<BoundaryConditionsContainer<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> > mpBoundaryConditionsContainer;

    std::vector<AbstractInhomogenousOdeSystemForCoupledPdeSystem*> pOdeSystemsAtNodes; // not needed?

    //boost::shared_ptr<AbstractIvpOdeSolver> pOdeSolver; // not needed?
    std::vector<boost::shared_ptr<AbstractIvpOdeSolver>> pOdeSolvers;

    /** The solution to the PDE problem at the current time step. */
    Vec mSolution; // serialised vector for pdeSystem

    /** Pointer to the finite element mesh on which to solve the PDE. */
    TetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* mpFeMesh;

    /** Store the output directory name. */
    std::string mOutputDirectory;

    /** Whether or not to calculate and output the gradient of the solution. */
    bool mOutputGradient;

    /**
     * Whether to output the PDE solution at each node of the FE mesh at output time steps.
     * Defaults to false.
     */
    bool mOutputSolutionAtPdeNodes;

    /** File that the values of the PDE solution are written out to. */
    out_stream mpVizPdeSolutionResultsFile;

    /**
     * Whether to delete the finite element mesh when we are destroyed.
     */
    bool mDeleteFeMesh;

    bool mIsDomainField = true;

public:

    AbstractPdeSystemModifier(ChemicalDomainFieldForCellCoupling<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* p_domain_field,
                        Vec solution=nullptr);

    /**
     * Destructor.
     */
    virtual ~AbstractPdeSystemModifier();

    /**
     * @return mpPdeSystem
     */
    boost::shared_ptr<InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>> GetPdeSystem();

    boost::shared_ptr<BoundaryConditionsContainer<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> > GetBoundaryConditionsContainer();

    /**
     * @return whether the PDE has an averaged source
     */
    bool HasAveragedSourcePde();

    /**
     * In the case where the PDE has an averaged source, set the source terms
     * using the information in the given mesh.
     *
     * @param pMesh Pointer to a tetrahedral mesh
     * @param pCellPdeElementMap map between cells and elements
     */
    void SetUpSourceTermsForAveragedSourcePde(TetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* pMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap=nullptr);

    /**
     * @return mSolution.
     */
    Vec GetSolution();

    /**
     * @return mSolution (used in archiving)
     */
    Vec GetSolution() const;

    /**
     * @return mpFeMesh.
     */
    TetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* GetFeMesh() const;

    /**
     * Overridden SetupSolve() method.
     *
     * Set mOutputDirectory and, if mOutputSolutionAtPdeNodes is set to true, open mpVizPdeSolutionResultsFile.
     * This method is overridden in subclasses.
     *
     * @param rCellPopulation reference to the cell population
     * @param outputDirectory the output directory, relative to where Chaste output is stored
     */
    virtual void SetupSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation, std::string outputDirectory);

    /**
     * Overridden UpdateAtEndOfTimeStep() method.
     *
     * As this method is pure virtual, it must be overridden in subclasses.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void UpdateAtEndOfTimeStep(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)=0;

    /**
     * Overridden UpdateAtEndOfOutputTimeStep() method,
     * after UpdateAtEndOfTimeStep() has been called.
     *
     * Output the solution to the PDE at each cell to VTK and, if mOutputSolutionAtPdeNodes is set to true,
     * output the solution to the PDE at each node of mpFeMesh to mpVizPdeSolutionResultsFile.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void UpdateAtEndOfOutputTimeStep(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    /**
     * Overridden UpdateAtEndOfSolve() method.
     *
     * If mOutputSolutionAtPdeNodes is set to true, close mpVizPdeSolutionResultsFile.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void UpdateAtEndOfSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation);

    /**
     * Set whether to calculate and save the gradient of the solution to CellData.
     *
     * @return mOutputGradient
     */
    bool GetOutputGradient();

    /**
     * Set whether to calculate and save the gradient of the solution to CellData.
     *
     * @param outputGradient whether to output the gradient
     */
    void SetOutputGradient(bool outputGradient);

    /**
     * Set mOutputSolutionAtPdeNodes.
     *
     * @param outputSolutionAtPdeNodes whether to output the PDE solution at each node of the FE mesh at output time steps
     */
    void SetOutputSolutionAtPdeNodes(bool outputSolutionAtPdeNodes);

    /**
     * Overridden OutputSimulationModifierParameters() method.
     *
     * Output any simulation modifier parameters to file.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputSimulationModifierParameters(out_stream& rParamsFile);

    void SetDomainField(AbstractDomainFieldTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>*);

    AbstractDomainFieldTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* GetDomainField();
};

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::AbstractPdeSystemModifier(ChemicalDomainFieldForCellCoupling<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* p_domain_field,
                                              Vec solution)
      : AbstractCellBasedSimulationModifier<ELEMENT_DIM,SPACE_DIM>(),
        mpCoupledDomainField(p_domain_field),
        mpPdeSystem(p_domain_field->ReturnSharedPtrPdeSystem()), // passed to AbstractCellBasedSimulation, as is a simulation modifier
        mpBoundaryConditionsContainer(p_domain_field->ReturnSharedPtrBoundaryConditionsContainer()), // passed to AbstractCellBasedSimulation
        mSolution(nullptr),
        mOutputDirectory(""),
        mOutputGradient(false),
        mOutputSolutionAtPdeNodes(false),
        mDeleteFeMesh(false)
{
    if (solution)
    {
        mSolution = solution;
    }
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::~AbstractPdeSystemModifier()
{
    if (mDeleteFeMesh and mpFeMesh!=nullptr)
    {
        delete mpFeMesh;
    }
    if (mSolution)
    {
        PetscTools::Destroy(mSolution);
    }
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
boost::shared_ptr<InhomogenousParabolicPdeForCoupledOdeSystemTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>> AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetPdeSystem()
{
    return mpPdeSystem;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
boost::shared_ptr<BoundaryConditionsContainer<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> > AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetBoundaryConditionsContainer()
{
    return mpBoundaryConditionsContainer;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>  // remove? undecided
bool AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::HasAveragedSourcePde()
{
    // if the pde function iherits from the averaged source pdes
    return false;//((boost::dynamic_pointer_cast<AveragedSourceInhomogenousParabolicPdeOdeSystem<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> >(mpPdeSystem) != nullptr));
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM> //remove? or change to linearParabolicPdeSystem?
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetUpSourceTermsForAveragedSourcePde(TetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* pMesh, std::map<CellPtr, unsigned>* pCellPdeElementMap)
{

    assert(HasAveragedSourcePde());

    /*
    if (boost::dynamic_pointer_cast<AveragedSourceInhomogenousParabolicPdeOdeSystem<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> >(mpPdeSystem) != nullptr)
    {
        boost::static_pointer_cast<AveragedSourceInhomogenousParabolicPdeOdeSystem<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM> >(mpPdeSystem)->SetupSourceTerms(*pMesh, pCellPdeElementMap);
    }

    if (boost::dynamic_pointer_cast<AveragedSourceEllipticPde<SPACE_DIM> >(mpPdeSystem) != nullptr)
    {
        boost::static_pointer_cast<AveragedSourceEllipticPde<SPACE_DIM> >(mpPdeSystem)->SetupSourceTerms(*pMesh, pCellPdeElementMap);
    }
    else if (boost::dynamic_pointer_cast<AveragedSourceParabolicPde<SPACE_DIM> >(mpPdeSystem) != nullptr)
    {
        boost::static_pointer_cast<AveragedSourceParabolicPde<SPACE_DIM> >(mpPdeSystem)->SetupSourceTerms(*pMesh, pCellPdeElementMap);
    }
    */
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
Vec AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetSolution()
{
    return mSolution;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
Vec AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetSolution() const
{
    return mSolution;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
TetrahedralMesh<ELEMENT_DIM,SPACE_DIM>* AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetFeMesh() const
{
    return mpFeMesh;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetupSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation, std::string outputDirectory)
{
    //std::cout<<"AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetupSolve - start"<<std::endl;
    // Cache the output directory
    this->mOutputDirectory = outputDirectory;

    if (mOutputSolutionAtPdeNodes)
    {

        if (PetscTools::AmMaster())
        {

            OutputFileHandler output_file_handler(outputDirectory+"/", false);

            mpVizPdeSolutionResultsFile = output_file_handler.OpenOutputFile("results.vizpdesolution");

        }
    }
    //std::cout<<"AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetupSolve - end"<<std::endl;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::UpdateAtEndOfOutputTimeStep(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)
{
    //std::cout<<"AbstractPdeSystemModifier - UpdateAtEndOfOutputTimeStep - start"<<std::endl;
    if (mOutputSolutionAtPdeNodes)
    {
        if (PetscTools::AmMaster())
        {
            (*mpVizPdeSolutionResultsFile) << SimulationTime::Instance()->GetTime() << "\t";

            assert(mpFeMesh != nullptr);

            for (unsigned i=0; i<mpFeMesh->GetNumNodes(); i++)
            {
                (*mpVizPdeSolutionResultsFile) << i << " ";
                const c_vector<double,SPACE_DIM>& r_location = mpFeMesh->GetNode(i)->rGetLocation();

                for (unsigned k=0; k<SPACE_DIM; k++)
                {
                    (*mpVizPdeSolutionResultsFile) << r_location[k] << " ";
                }

                assert(mSolution != nullptr); // remove, test against null vector

                ReplicatableVector solution_repl(mSolution);
                for(unsigned pd=0; pd<PROBLEM_DIM; pd++){
                    (*mpVizPdeSolutionResultsFile) << solution_repl[i+pd] << " ";
                }
            }

            (*mpVizPdeSolutionResultsFile) << "\n";
        }

    }

#ifdef CHASTE_VTK

    if (SPACE_DIM > 1)
    {
        std::ostringstream time_string;
        time_string << SimulationTime::Instance()->GetTimeStepsElapsed();

        ReplicatableVector solution_repl(mSolution);

        for(unsigned pdeDim=0; pdeDim<PROBLEM_DIM; pdeDim++)
        {
            std::string results_file = "pde_results_" + mpCoupledDomainField->GetDomainStateVariableRegister()->RetrieveStateVariableName(pdeDim) + "_" + time_string.str();
            VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>* p_vtk_mesh_writer = new VtkMeshWriter<ELEMENT_DIM,SPACE_DIM>(mOutputDirectory, results_file, false);
            std::vector<double> pde_solution;

            for (unsigned i=0; i<mpFeMesh->GetNumNodes(); i++)
            {
                pde_solution.push_back(solution_repl[i*PROBLEM_DIM+pdeDim]);
            }
            p_vtk_mesh_writer->AddPointData(mpCoupledDomainField->GetDomainStateVariableRegister()->RetrieveStateVariableName(pdeDim), pde_solution);

            p_vtk_mesh_writer->WriteFilesUsingMesh(*mpFeMesh);

            delete p_vtk_mesh_writer;
        }

    }
#endif //CHASTE_VTK
    //std::cout<<"AbstractPdeSystemModifier - UpdateAtEndOfOutputTimeStep - end"<<std::endl;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::UpdateAtEndOfSolve(AbstractCellPopulation<ELEMENT_DIM,SPACE_DIM>& rCellPopulation)
{
    if (mOutputSolutionAtPdeNodes)
    {
        if (PetscTools::AmMaster())
        {
            mpVizPdeSolutionResultsFile->close();
        }
    }
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
bool AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetOutputGradient()
{
    return mOutputGradient;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetOutputGradient(bool outputGradient)
{
    mOutputGradient = outputGradient;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetOutputSolutionAtPdeNodes(bool outputSolutionAtPdeNodes)
{
    mOutputSolutionAtPdeNodes = outputSolutionAtPdeNodes;
}

template<unsigned ELEMENT_DIM,unsigned SPACE_DIM,unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::OutputSimulationModifierParameters(out_stream& rParamsFile)
{
    // No parameters to output, so just call method on direct parent class
    AbstractCellBasedSimulationModifier<ELEMENT_DIM,SPACE_DIM>::OutputSimulationModifierParameters(rParamsFile);
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
void AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::SetDomainField(AbstractDomainFieldTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* pDomainField)
{
    mpCoupledDomainField = pDomainField;
    mIsDomainField = true;
}

template<unsigned ELEMENT_DIM, unsigned SPACE_DIM, unsigned PROBLEM_DIM>
AbstractDomainFieldTemplated<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>* AbstractPdeSystemModifier<ELEMENT_DIM,SPACE_DIM,PROBLEM_DIM>::GetDomainField()
{
    return mpCoupledDomainField;
}

#endif /*ABSTRACTPDEMODIFIER_HPP_*/


/*!
@file
Defines `boost::hana::MonadZero`.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_MONAD_ZERO_HPP
#define BOOST_HANA_MONAD_ZERO_HPP

#include <boost/hana/applicative/applicative.hpp>
#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/logical/logical.hpp>
#include <boost/hana/monad/monad.hpp>

namespace boost { namespace hana {
    //! @ingroup group-typeclasses
    //! `Monad`s with a neutral element.
    //!
    //! ## Minimal complete definition
    //! `zero`
    //!
    //! ## Laws
    //! ...
    struct MonadZero {
        BOOST_HANA_TYPECLASS(MonadZero);
        template <typename M>
        struct mcd;
    };

    //! ...
    //! @relates MonadZero
    template <typename M>
    constexpr auto zero = MonadZero<M>::zero_impl();

    //! ...
    //! @relates MonadZero
    //!
    //! ### Example
    //! ...
    BOOST_HANA_CONSTEXPR_LAMBDA auto filter = [](auto predicate, auto monad) {
        return MonadZero::instance<datatype_t<decltype(monad)>>::filter_impl(predicate, monad);
    };

    template <typename M>
    struct MonadZero::mcd {
        template <typename Pred, typename Monad>
        static constexpr auto filter_impl(Pred pred, Monad m) {
            return bind(m, [=](auto x) {
                return eval_if(pred(x),
                    [=](auto _) { return _(lift<M>)(x); },
                    [=](auto) { return zero<M>; }
                );
            });
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_MONAD_ZERO_HPP


/*
 * PseudoMeasurements.hpp
 *
 *  Created on: 02.08.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_PSEUDOMEASUREMENTS_HPP_
#define ESTIMATORS_PSEUDOMEASUREMENTS_HPP_

#include <Eigen/Core>

#include <glog/logging.h>
#include <ceres/ceres.h>

#include <Eigen_Utils.hpp>
#include "BatchEstimator.hpp"

/**
 * Setup a basic pseudo measurement so that other parts of the programm know its  dimensions
 */
#define SETUP_PSEUDO_MEASUREMENT(MEASURE_DIM)  \
	static constexpr int measure_dim=MEASURE_DIM; \
	static constexpr int state_dim=estimator_type::MODEL_TYPE::outer_size; \
	template<typename T>										\
	using MEASURE_TYPE=Eigen::Matrix<T,measure_dim,1>;

/**
 * Setup a pseudo measurement which contains a manifold with different inner size
 */
#define SETUP_PSEUDO_MEASUREMENT_WITH_MANIFOLD(MEASURE_DIM,MANIFOLD_DIM)  \
	SETUP_PSEUDO_MEASUREMENT(MEASURE_DIM)   \
	static constexpr int manifold_dim=MANIFOLD_DIM;   \
	template<typename T>										\
	using MANIFOLD_TYPE=Eigen::Matrix<T,manifold_dim,1>;

/**
 * Adds the necesarry function so that the UKF accepts the knowledge (for knowledge where alignment is not required)
 */
#define ADD_UKF_FUNCTION_PASSER  \
template<typename T>		\
MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state,const ALIGNMENT_TYPE<T> &alignment, void * prior) const {		\
			return this->operator()(state,prior);     \
		}

namespace zavi
::estimator::pseudo_measurement {

	/**
	 * Struct to contain relevant data for a function as Prior Knowledge
	 */
	template<typename ukf_type>
	struct FunctionMeasurement {
		FunctionMeasurement(const zavi::eigen_util::FuncCaller &function,std::shared_ptr<ukf_type> estimator ):function(function),ukf(estimator) {}
		zavi::eigen_util::FuncCaller function;
		std::shared_ptr<ukf_type> ukf;
	};
	/**
	 * pseudo measurement to return the position
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected position at state
	 */
	inline Eigen::Matrix<double, 3, 1> position_measurement(const Eigen::Matrix<double,9,1> &state, void *prior) {
		return state.block(0,0,3,1);
	}

	/**
	 * Functor to calculate the shortest distance between a function and a point
	 */
	template <typename functor>
	class DistanceFunctor {
	public:
		~DistanceFunctor() {}
		DistanceFunctor(functor & function):function(function),x(0),y(0),z(0) {};
		template<typename T>
		bool operator()(const T * const time,T* residual) const {
			T func[3];
			function(time,(T* )func);
			residual[0]=pow(x-func[0],2)+pow(y-func[1],2)+pow(z-func[2],2);

			return true;
		}

	public:
		/**
		 * Set the point to which to calculate the distance
		 * @param point  point with x y z coordinates
		 */
		void setPoint(const Eigen::Matrix<double,3,1> & point) {
			x=point(0,0);
			y=point(1,0);
			z=point(2,0);
		}

		functor function;     // evaluation function
	private:

		double x,y,z;// point coordinates
	};

	/**
	 * Struct to contain relevant data for a function as Prior Knowledge
	 */
	template <typename functor,typename cov_type,typename ukf_type>
	struct DistanceMeasurement {
		DistanceMeasurement(functor &function,const cov_type &cov, std::shared_ptr<ukf_type> estimator ):function(function),cov(cov),ukf(estimator) {}
		DistanceFunctor<functor> function;
		cov_type cov;
		std::shared_ptr<ukf_type> ukf;
	};
	/**
	 * pseudo measurement to return the distance to prior function
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected distance to function at state hard constriaint so it is always zero
	 */
	template <typename functor,typename ukf_type>
	Eigen::Matrix<double, 3, 1> distance_measurement(const Eigen::Matrix<double,9,1> &state, void * prior) {
		DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> *>(prior);
		container->function.function.give_real=false;     // tell function that the solver is active
		// The variable to solve for with its initial value.
		static double initial_x = 0;
		double x = initial_x;
		container->function.setPoint(state.block(0,0,3,1));
		// Build the problem.
		// Set up the only cost function (also known as residual). This uses
		// auto-differentiation to obtain the derivative (jacobian).
		ceres::CostFunction* cost_function =
		new ceres::AutoDiffCostFunction<DistanceFunctor<functor>, 1, 1>(new DistanceFunctor<functor>(container->function));
		ceres::Problem problem;

		problem.AddResidualBlock(cost_function, NULL, &x);

		// Run the solver!
		ceres:: Solver::Options options;
		options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = false;
		options.max_num_line_search_step_size_iterations=10;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		//std::cout << summary.BriefReport() << std::endl;
		initial_x=x;
		double result[3];
		container->function.function(&x,(double *)result);
		Eigen::Matrix<double,3,1> returner;
		returner(0,0)=result[0]-state(0,0);
		returner(1,0)=result[1]-state(1,0);
		returner(2,0)=result[2]-state(2,0);

		container->function.function.give_real=true;// tell function that the solver is finished
		return returner;

	}
	/**
	 * pseudo measurement to return the distance to prior function
	 * @param state  the current estimated state
	 * @param prior a maybe usefull prior
	 * @return an expected distance to function at state hard constriaint so it is always zero
	 */
	template <typename functor,typename ukf_type>
	Eigen::Matrix<double, 1, 1> distance_measurement_single(const Eigen::Matrix<double,9,1> &state, void * prior) {
		DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> *>(prior);
		container->function.function.give_real=false;     // tell function that the solver is active
		// The variable to solve for with its initial value.
		static double initial_x = 0;
		double x = initial_x;
		container->function.setPoint(state.block(0,0,3,1));
		// Build the problem.
		// Set up the only cost function (also known as residual). This uses
		// auto-differentiation to obtain the derivative (jacobian).
		ceres::CostFunction* cost_function =
		new ceres::AutoDiffCostFunction<DistanceFunctor<functor>, 1, 1>(new DistanceFunctor<functor>(container->function));
		ceres::Problem problem;

		problem.AddResidualBlock(cost_function, NULL, &x);

		// Run the solver!
		ceres:: Solver::Options options;
		options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = false;
		options.max_num_line_search_step_size_iterations=10;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		//std::cout << summary.BriefReport() << std::endl;
		initial_x=x;
		double result[1];
		container->function(&x,(double *)result);
		Eigen::Matrix<double,1,1> returner;
		returner(0,0)=result[0];

		container->function.function.give_real=true;// tell function that the solver is finished
		return returner;

	}

	/**
	 *  a pseudo distance measurement with a given function as ground truth
	 * @param plug  the imu plugin
	 * @param estimator  the function measurement struct
	 * @param time  time current time stamp
	 */
	template<typename functor,typename ukf_type>
	void pseudoDistanceFunctionMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix3d,ukf_type> *>(estimator);
		//plugin::IMU_Plugin *imu = static_cast<plugin::IMU_Plugin*>(plug);
		Eigen::Matrix<double,3,1> zeros=Eigen::Matrix<double,3,1>::Zero();
		container->ukf->measurementStep(zeros,time-last_time,distance_measurement<functor>,container->cov,container);
		last_time=time;
	}

	/**
	 *  a pseudo distance measurement with a given function as ground truth
	 * @param plug  the imu plugin
	 * @param estimator  the function measurement struct
	 * @param time  time current time stamp
	 */
	template<typename functor,typename ukf_type>
	void pseudoDistanceFunctionMeasurementSingle(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> * container= static_cast<DistanceMeasurement<functor,Eigen::Matrix<double,1,1>,ukf_type> *>(estimator);
		//plugin::IMU_Plugin *imu = static_cast<plugin::IMU_Plugin*>(plug);
		Eigen::Matrix<double,1,1> zeros=Eigen::Matrix<double,1,1>::Zero();
		container->ukf->measurementStep(zeros,time-last_time,distance_measurement_single<functor>,container->cov,container);
		last_time=time;
	}

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 * The Velocity in x must equal the norm of the velocity
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type>
	struct DrivingForwardConstrain {
		SETUP_PSEUDO_MEASUREMENT(3)
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state,void * prior) const {
			estimator_type * estimator=static_cast<estimator_type*>(prior);
			MEASURE_TYPE<T> body_velocity=estimator->getBoxModel().getOrientation(state).transpose()*estimator->getBoxModel().getVelocity(state)-getTarget(state,prior);
			return body_velocity;

		}
		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		MEASURE_TYPE<T> getTarget(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type * estimator=static_cast<estimator_type*>(prior);
			MEASURE_TYPE<T> returner=MEASURE_TYPE<T>::Zero();
			T norm=estimator->getBoxModel().getVelocity(state).norm();
			if(norm !=0.)     // ceres jet infinity fix
			returner(0,0)=norm;
			return returner;

		}

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.1;
		}
	};

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type>
	struct NoSideUpDriftConstrain {
		SETUP_PSEUDO_MEASUREMENT(2)
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			return ( (estimator_type::MODEL_TYPE::getOrientation(state).transpose()*estimator_type::MODEL_TYPE::getVelocity(state))) .template block<2,1>(1,0);

		}
		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.1;
		}
	};

	template<typename estimator_type>
	struct OnlyRollConstrain {
		static constexpr int measure_dim=2;     // measurement dimension
		static constexpr int state_dim=estimator_type::MODEL_TYPE::outer_size;
		template<typename T>
		using MEASURE_TYPE=Eigen::Matrix<T,measure_dim,1>;
		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			return eigen_util::inverseEulerRodriguez<T>(estimator_type::MODEL_TYPE::getOrientation(state)).template block<2,1>(1,0);

		}

		ADD_UKF_FUNCTION_PASSER
		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.01;
		}
	};

	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type, typename functor>
	struct FunctionOrientationMeasurement {

		SETUP_PSEUDO_MEASUREMENT_WITH_MANIFOLD(3,3)

		eigen_util::FuncCaller function;
		FunctionOrientationMeasurement<estimator_type,functor>() :function(functor()) {

		}

		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type* estimator= static_cast<estimator_type *>(prior);
			return eigen_util::boxMinusOrientation(estimator->getBoxModel().getOrientation(state),eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0),function,1e-5));

		}
		ADD_UKF_FUNCTION_PASSER

		template<typename T>
		static MANIFOLD_TYPE<T> boxminus(const MEASURE_TYPE<T> &a , const MEASURE_TYPE<T> &b) {
			return eigen_util::wrapAngles(b-a);
		}
		template<typename T>
		static MEASURE_TYPE<T> boxplus(const MEASURE_TYPE<T> &state , const MANIFOLD_TYPE<T> &delta) {
			return eigen_util::wrapAngles(state+delta);
		}

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.3;
		}
	};
	/**
	 * This Struct encapsulates the driving forward constrain
	 * The Velocity in y and z direction ( in body coordinates ) is 0
	 *
	 * It requires the estimator as prior
	 */
	template<typename estimator_type, typename functor>
	struct FunctionOrientationChangeMeasurement {
		SETUP_PSEUDO_MEASUREMENT(3)

		eigen_util::FuncCaller function;
		FunctionOrientationChangeMeasurement<estimator_type,functor>() :function(functor()) {

		}

		template<typename T>
		MEASURE_TYPE<T> operator()(const Eigen::Matrix<T,state_dim,1> &state, void * prior) const {
			estimator_type* estimator= static_cast<estimator_type *>(prior);
			Eigen::Matrix<T,3,1> current_axis=estimator->getBoxModel().getOrientationChange(state);
			Eigen::Matrix<T,3,1> target_axis=eigen_util::boxMinusOrientation(eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0)-1e-5,function,1e-5),eigen_util::orientationFromFunctor(state(estimator->getBoxModel().lambda_start,0)+1e-5,function,1e-5))/2e-5;
			//zavi::plot::liveStateDraw<6>(target_axis,"target_axis");
			/*if(current_axis.norm()!=0.)
			current_axis.normalize();*/
			/*if(target_axis.norm() !=0.)
			target_axis.normalize();*/
			return current_axis-target_axis*estimator->getBoxModel().getLambdaVelocity(state);

		}
		ADD_UKF_FUNCTION_PASSER

		template<typename T>
		static constexpr MEASURE_TYPE<T> getTarget() {
			return MEASURE_TYPE<T>::Zero();
		}
		static Eigen::Matrix<double,measure_dim,measure_dim> getNoise() {
			return Eigen::Matrix<double,measure_dim,measure_dim>::Identity()*0.5;
		}
	};

	/**
	 * pseudo ukf measurement
	 * @template measure_struct a struct with () operator to define the measure function
	 * @template ukf_type the type of the estimator
	 * @template measure_dim the size of the measurement
	 * @template measurement a measurement object containing the wanted result
	 * @template cov the covariance of the pseudo measurement
	 */
	template<template<typename,typename ...> class measure_struct,typename ukf_type,typename ... meas_args>
	void ukfMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		static measure_struct<ukf_type,meas_args ...> measure_function=measure_struct<ukf_type,meas_args ...>();
		ukf_type *ukf=static_cast<ukf_type*>(estimator);
		ukf->measurementStep(measure_function.template getTarget<double>(),time-last_time,measure_function,measure_function.getNoise(),estimator);     // No Prior is given since the circle_prior does not need an object
		last_time=time;
	}
	/**
	 * pseudo ukf measurement for manifolds
	 * @template measure_struct a struct with () operator to define the measure function
	 * @template ukf_type the type of the estimator
	 * @template measure_dim the size of the measurement
	 * @template measurement a measurement object containing the wanted result
	 * @template cov the covariance of the pseudo measurement
	 */
	template<template<typename,typename ...> class measure_struct,typename ukf_type,typename ... meas_args>
	void ukfManifoldMeasurement(plugin::SensorPlugin * plug,
			void* estimator, double time) {
		static double last_time=time-0.00001;
		typedef measure_struct<ukf_type,meas_args ...> MEASURE_STRUCT;
		static MEASURE_STRUCT measure_function=MEASURE_STRUCT();
		ukf_type *ukf=static_cast<ukf_type*>(estimator);
		//measure_function.template boxplus<double>(Eigen::Vector3d(), Eigen::Vector3d());
		ukf->measurementStepManifold(measure_function.template getTarget<double>(),time-last_time,measure_function,measure_function.getNoise(),MEASURE_STRUCT::boxplus,MEASURE_STRUCT::boxminus,estimator);
		last_time=time;
	}

	/**
	 * A cost function comparing the 1 state with the required circle trajectory
	 */
	template<template <typename T> class STATE_TYPE_T,typename MEASUREMENT_FUNCTION, int measure_dim >
	struct MeasurementDiffCostFunction {
		template<typename T>
		using MEASUREMENT_TYPE_T= Eigen::Matrix<T,measure_dim,1>;
		static inline MEASUREMENT_FUNCTION measurement_function=MEASUREMENT_FUNCTION();
		MeasurementDiffCostFunction(void * prior):prior(prior) {}

		template<typename T>
		bool operator()(const T * const a,const T* const alignment, T* result)const {
			static Eigen::Matrix<double,measure_dim,measure_dim> stiffnes=measurement_function.getNoise().diagonal().cwiseInverse().cwiseSqrt().asDiagonal();
			static Eigen::Matrix<double,measure_dim,1> measurement=measurement_function.template getTarget<double>();
			(Eigen::Map<MEASUREMENT_TYPE_T<T> > (result))=stiffnes*(measurement_function.template operator()<T>(Eigen::Map<const STATE_TYPE_T<T> >(a),Eigen::Map<const ALIGNMENT_TYPE<T> >(alignment),prior)-measurement);
			return true;
		}

		template<typename T>
		bool operator()(const T * const a, T* result)const {
			static Eigen::Matrix<double,measure_dim,measure_dim> stiffnes=measurement_function.getNoise().diagonal().cwiseInverse().cwiseSqrt().asDiagonal();
			static Eigen::Matrix<double,measure_dim,1> measurement=measurement_function.template getTarget<double>();
			(Eigen::Map<MEASUREMENT_TYPE_T<T> > (result))=stiffnes*(measurement_function.template operator()<T>(Eigen::Map<const STATE_TYPE_T<T> >(a),prior)-measurement);
			return true;
		}
		void * prior;
	};
	/**
	 * Adds the circular constraint to the ceres problem
	 * @param problem  ceres problem
	 * @param states list of all states
	 * @param inputs list of all inputs
	 * @param time_diffs list of all time diffs
	 */

	template<template<typename> class MEASUREMENT_FUNCTION, typename ESTIMATOR_TYPE,bool with_alignment=false>
	struct MeasurementConstraint: public ESTIMATOR_TYPE::BatchConstrain {
		static constexpr int measure_dim=MEASUREMENT_FUNCTION<ESTIMATOR_TYPE>::measure_dim;
		virtual ~MeasurementConstraint() {};
		typedef MeasurementDiffCostFunction<ESTIMATOR_TYPE::template STATE_TYPE_T,MEASUREMENT_FUNCTION<ESTIMATOR_TYPE>,measure_dim > COST_TYPE;
		virtual void operator()(ceres::Problem &problem,double * states,double * alignment, int state_size,void* data,std::vector<double> &time_diffs, BatchEstimator<typename ESTIMATOR_TYPE::MODEL_TYPE> * estimator) {
			if(with_alignment) {
				ceres::CostFunction* cost_function=new ceres::AutoDiffCostFunction<COST_TYPE,measure_dim,ESTIMATOR_TYPE::MODEL_TYPE::outer_size, ALIGNMENT_SIZE>(new COST_TYPE(estimator));
				for(int i=0; i < state_size;i++) {
					problem.AddResidualBlock(cost_function,NULL,&states[i*ESTIMATOR_TYPE::MODEL_TYPE::outer_size], alignment);
				}
			}
			else {
				ceres::CostFunction* cost_function=new ceres::AutoDiffCostFunction<COST_TYPE,measure_dim,ESTIMATOR_TYPE::MODEL_TYPE::outer_size>(new COST_TYPE(estimator));
				for(int i=0; i < state_size;i++) {
					problem.AddResidualBlock(cost_function,NULL,&states[i*ESTIMATOR_TYPE::MODEL_TYPE::outer_size]);
				}
			}
		}
	};

}
//zavi::estimator::pseudo_measurement

#endif /* ESTIMATORS_PSEUDOMEASUREMENTS_HPP_ */


// -*-C++-*-
/*
  (c) Copyright 2012, Hewlett-Packard Development Company, LP

  See the file named COPYING for license details
*/

/** @file
    test program for DataSeries
*/

#include <boost/bind.hpp>

#include <Lintel/LintelLog.hpp>

#include <DataSeries/Extent.hpp>
#include <DataSeries/ExtentField.hpp>

using namespace std;
using boost::format;

const string test_xml =
        "<ExtentType namespace=\"ssd.hpl.hp.com\" name=\"scale-tests\" version=\"1.0\" >\n"
        "  <field type=\"double\" name=\"s-1\" pack_scale=\"1.0\" />\n"
        "  <field type=\"double\" name=\"s-1-nowarn\" pack_scale=\"1.0\" pack_scale_warn=\"no\" />\n"
        "  <field type=\"double\" name=\"s-0.1\" pack_scale=\"0.1\" pack_scale_warn=\"yes\" />\n"
        "</ExtentType>\n";

vector<string> log_msgs;
void saveAppender(const std::string &msg, const LintelLog::LogType type) {
    SINVARIANT(type == LintelLog::Warn);
    log_msgs.push_back(msg);
}

void testSimple() {
    const ExtentType::Ptr type(ExtentTypeLibrary::sharedExtentTypePtr(test_xml));

    Extent::Ptr e(new Extent(type));
    ExtentSeries s(e);
    DoubleField scale_1(s, "s-1");
    DoubleField scale_1_nowarn(s, "s-1-nowarn");
    DoubleField scale_0_1(s, "s-0.1");

    s.newRecord(); // round down | exact
    scale_1.set(0.2);
    scale_1_nowarn.set(0.2);
    scale_0_1.set(0.2);

    s.newRecord(); // round up | exact
    scale_1.set(1.8);
    scale_1_nowarn.set(1.8);
    scale_0_1.set(1.8);

    s.newRecord(); // round warning on 0.1 scaling
    scale_1.set(2.08);
    scale_1_nowarn.set(2.08);
    scale_0_1.set(2.08);

    LintelLog::addAppender(boost::bind(saveAppender, _1, _2));
    Extent::ByteArray packed_data;
    e->packData(packed_data, Extent::compression_algs[Extent::compress_mode_lzf].compress_flag, 9, NULL, NULL, NULL);
    SINVARIANT(log_msgs.size() == 2);
    SINVARIANT(log_msgs[0] == "Warning, while packing field s-1 of record 1, error was > 10%:\n"
               "  (0.2 / 1 = 0.20, round() = 0)\n");
    SINVARIANT(log_msgs[1] == "Warning, while packing field s-0.1 of record 3, error was > 10%:\n"
               "  (2.08 / 0.1 = 20.80, round() = 21)\n");

    e->unpackData(packed_data, false);

    s.setExtent(e);
    SINVARIANT(Double::eq(scale_1(), 0));
    SINVARIANT(Double::eq(scale_1_nowarn(), 0));
    SINVARIANT(Double::eq(scale_0_1(), 0.2));

    s.next();
    SINVARIANT(Double::eq(scale_1(), 2));
    SINVARIANT(Double::eq(scale_1_nowarn(), 2));
    SINVARIANT(Double::eq(scale_0_1(), 1.8));

    s.next();
    SINVARIANT(Double::eq(scale_1(), 2));
    SINVARIANT(Double::eq(scale_1_nowarn(), 2));
    SINVARIANT(Double::eq(scale_0_1(), 2.1));
    cout << "Simple test passed.\n";
}

int main() {
    testSimple();
    return 0;
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include <snark/math/angle.h>
#include "coordinates.h"

namespace snark { namespace spherical {

const double coordinates::epsilon = 1e-6;

bool coordinates::operator==( const coordinates& rhs ) const
{ return is_near( rhs, epsilon ); }

bool coordinates::is_near( const coordinates& c, double epsilon ) const
{
    double dist = std::abs( longitude - c.longitude );
    return comma::math::equal( latitude, c.latitude, epsilon ) && comma::math::equal( 0, std::min( dist, 2 * M_PI - dist ), epsilon );
}

/// limits longitude to [-PI, PI)
static double limit( const double longitude )
{
    // todo: this is just a quick patch, not a real fix; what if longitude equals e.g. -3*M_PI?
    // todo: seriously quick and dirty, to fix anti merridian crossing
    return longitude < -M_PI ? longitude + 2 * M_PI : longitude >= M_PI ? longitude - 2 * M_PI : longitude;
}

coordinates::coordinates( const double latitude, const double longitude ) : latitude( latitude ), longitude( limit( longitude ) )
{
}

coordinates::coordinates( const snark::bearing_elevation& be ) : latitude( be.elevation() ), longitude( limit( be.bearing() ) )
{
}

coordinates::coordinates( const Eigen::Vector3d& xyz )
{
    snark::range_bearing_elevation rbe( xyz );
    latitude = rbe.elevation();
    longitude = limit( rbe.bearing() );
}

Eigen::Vector3d to_navigation_frame( const coordinates& c, const Eigen::Vector3d& v )
{
    const Eigen::Matrix3d& r1 = Eigen::AngleAxis< double >( -c.longitude, Eigen::Vector3d( 0, 0, 1 ) ).toRotationMatrix();
    const Eigen::Matrix3d& r2 = Eigen::AngleAxis< double >( c.latitude + M_PI / 2, Eigen::Vector3d( 0, 1, 0 ) ).toRotationMatrix();
    return r2 * r1 * v;
}

coordinates& coordinates::operator+=( const coordinates& rhs )
{
    double d = latitude + rhs.latitude;
    static const double epsilon = 0.00005;
    if( comma::math::equal( d, M_PI / 2, epsilon ) ) { d = M_PI / 2; }
    else if( comma::math::equal( d, -M_PI / 2, epsilon ) ) { d = -M_PI / 2; }
    else if( d > M_PI / 2 || d < -M_PI / 2 ) { COMMA_THROW( comma::exception, "adding " << ( latitude * 180 / M_PI ) << " and " << ( rhs.latitude * 180 / M_PI ) << " gives invalid latitude of " << ( d * 180 / M_PI ) << " degress" ); }
    latitude = d;
    longitude += rhs.longitude;
    while( longitude < -M_PI ) { longitude += 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    while( this->longitude >= M_PI ) { longitude -= 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    return *this;
}

coordinates::operator std::string() const
{
    const std::pair< double, double >& c = to_degrees();
    return (boost::format("%.2f") % c.first ).str() + "," + (boost::format("%.2f") % c.second).str();
}

bool coordinates::is_near( const Eigen::Vector3d& c, double _epsilon ) const
{
    return ( to_cartesian() - c ).lpNorm<Eigen::Infinity>() < _epsilon;
}

coordinates coordinates::from_degrees(double latitude, double longitude)
{
    return coordinates( snark::math::radians( snark::math::degrees( latitude ) ).value,
                        snark::math::radians( snark::math::degrees( longitude ) ).value );
}

std::pair< double, double > coordinates::to_degrees() const
{
    return std::make_pair( snark::math::degrees( snark::math::radians( latitude ) ).value
                         , snark::math::degrees( snark::math::radians( longitude ) ).value );
}

} } // namespace snark { namespace spherical {