/*
 * Rody Oldenhuis
 * cosine Science & Computing BV
 * roldenhuis@cosine.nl
 *
 * PhysUnits.hh
 * Created: 31.08.2012 16:20:53 CEST
 */

#ifndef _PHYSUNITS_HH
#define _PHYSUNITS_HH

#include <thread>
#include <boost/date_time/posix_time/posix_time.hpp>

// global helper variables
namespace
{
    union endian_t {
        long l;
        char c[sizeof(long)];
    } __endian__t;
}

/// SI multiplier table
namespace si
{
    const double

    // names          prefixes
    yotta = 1e24,     Y  = yotta,
    zetta = 1e21,     Z  = zetta,
    exa   = 1e18,     E  = exa  ,
    peta  = 1e15,     P  = peta ,
    tera  = 1e12,     T  = tera ,
    giga  = 1e9,      G  = giga ,
    mega  = 1e6,      M  = mega ,
    kilo  = 1e3,      k  = kilo ,
    hecto = 1e2,      h  = hecto,
    deca  = 1e1,      da = deca ,

    deci  = 1e-1,     d  = deci ,
    centi = 1e-2,     c  = centi,
    milli = 1e-3,     m  = milli,
    micro = 1e-6,     mu = micro,
    nano  = 1e-9,     n  = nano ,
    pico  = 1e-12,    p  = pico ,
    femto = 1e-15,    f  = femto,
    atto  = 1e-18,    a  = atto ,
    zepto = 1e-21,    z  = zepto,
    yocto = 1e-24,    y  = yocto;
}

/// Binary prefixes
namespace binary
{
    const double

    // names          prefixes
    kibi = 1.*1024,   ki = kibi,
    mebi = ki*1024,   Mi = mebi,
    gibi = Mi*1024,   Gi = gibi,
    tebi = Gi*1024,   Ti = tebi,
    pebi = Ti*1024,   Pi = pebi,
    exbi = Pi*1024,   Ei = exbi,
    zebi = Ei*1024,   Zi = zebi,
    yobi = Zi*1024,   Yi = yobi;

}

/// Physics constants
namespace physics
{
    const double

    c     = 299792458,       // speed of light [m/s]
    au    = 1.49597870e11,   // unit distance [m]
    KSun  = 1370,            // Solar constant [W/m^2]

    // FIXME: (Rody Oldenhuis) DEPRACATED
    //    Using Solar mass (or masses in general) introduces terrible numerical
    //    inaccuracies everywhere due to current uncertainty in the value of
    //    Newton's G. Use standard gravitational parameters (=GM); these have
    //    up to 6 orders of magnitude better accuracy.
    G     = 6.67384e-11,     // Gravitational constant [m^3/kg/s^2] (CODATA 2010 recommended value)
    MSun  = 1.98855e30,      // Solar mass [kg]
    // -----------------------------------------

    k     = 1.3806488e-23,   // Boltzmann constant [J K^-1]
    sigma = 5.670373e-8,     // Stefan-Boltzmann constant [W m^-2 K^-4]

    // FIXME: (Rody Oldenhuis) Not really a "constant", more a unit...
    // Use class Length for these things.
    A     = 1.0e-10,         // Angstrom
    // -----------------------------------------

    NA    = 6.02214129e+23,  // Avogadro's constant [1/mol]
    h     = 6.62606957e-34,  // Planck's constant [Js]
    hbar  = 1.054571726e-34, // Reduced Plankc's constant, or Dirac's constant [Js]
    re    = 2.817940285e-15, // Classical electron radius [m]
    q     = 1.60217646e-19,  // Proton charge [C]

    // FIXME: (Rody Oldenhuis) DEPRECATED
    //   Only included because full conversion takes too much time.
    //   Find files in need of conversion with "grep 'physics::day' *.cc *.hh"
    //   and convert everything to class Time.
    s        = 1,
    minute   = 60*s,
    hour     = 60*minute,
    day      = 24*hour,
    jyear    = 365.25*day,
    jcentury = 100*jyear;
    // -----------------------------------------

}

/// Mathematical constants
namespace math
{
    const double

    pi         = 3.14159265358979323846264338327950288419716939937510,
    pio2       = 1.57079632679489661923132169163975144209858469968755,
    pio4       = 0.78539816339744830961566084581987572104929234984377,
    pio6       = 0.52359877559829887307710723054658381403286156656251,
    threepio2  = 4.71238898038468985769396507491925432629575409906265,
    twopi      = 6.28318530717958647692528676655900576839433879875020,
    tau        = twopi, // the TRUE circle constant!!
    twotau     = 12.5663706143591729538505735331180115367886775975004,
    oneopi     = 0.31830988618379067153776752674502872406891929148091, // 1/pi
    twoopi     = 0.63661977236758134307553505349005744813783858296182, // 2/pi
    oneosqrtpi = 0.56418958354775628694807945156077258584405062932900, // 1/sqrt(pi)
    twoosqrtpi = 1.12837916709551257389615890312154517168810125865800, // 2/sqrt(pi)

    sqrt2      = 1.41421356237309504880168872420969807856967187537694,
    sqrt3      = 1.73205080756887729352744634150587236694280525381038,
    sqrt1o2    = 0.70710678118654752440084436210484903928483593768847, // sqrt(1/2)
    sqrt1o3    = 0.57735026918962576450914878050195745564760175127012, // sqrt(1/3)

    epsilon    = std::numeric_limits<double>::epsilon(),
    dx         = epsilon,
    eps        = epsilon,
    INF        = std::numeric_limits<double>::infinity(),
    inf        = INF,
    NaN        = std::numeric_limits<double>::quiet_NaN(),
    nan        = NaN;

    // some things needfloat versions of the above
    const float

    epsilonf = std::numeric_limits<float>::epsilon(),
    dxf      = epsilonf,
    epsf     = epsilonf;

    // handy utilities
    // ----------------

    // signum: -1 for anything <0, +1 for anything >0, 0 for anything ==0.
    template <typename T>
    int signum(T in) {
        return (in>0)-(in<0);
    }

    // negative-value-correct modulus
    // NOTE: NOT the same as % or fmod()
    template <typename T>
    T mod(T x, T y) {
        return (0==y) ? x : x-y*std::floor(x/y);
    }

    // FIXME: (Rody Oldenhuis) DEPRECATED
    //   Only included because full conversion takes too much time.
    //   Find files in need of conversion with "grep 'math::deg' *.cc *.hh"
    //   and convert everything to class Angle.
    const double
        rad = 1.0,
        deg = pi/180.0;
    // -----------------------------------------
}

/// Constants specific to the system HIPSIM's being run on
namespace machine
{
    const unsigned int

    // get number of physical cores present in the system
    // platform-independent, > C++11
    numCores = std::thread::hardware_concurrency();

    // This machine's endianness
    const bool

    machine_is_msb = (__endian__t.l = 1, __endian__t.c[sizeof(long)-1] == 1),
    machine_is_lsb = (__endian__t.l = 1, __endian__t.c[sizeof(long)-1] != 1);

    // handy dandy utilities

    template<typename T>
    T change_endianness(T x)
    {
        char *b = (char *)(&x);
        switch (sizeof(T))
        {
            case 1:
                break;

            case 2:
                std::swap(b[0], b[1]);
                break;

            case 4:
                std::swap(b[0], b[3]);
                std::swap(b[1], b[2]);
                break;

            case 8:
                std::swap(b[0], b[7]);
                std::swap(b[1], b[6]);
                std::swap(b[2], b[5]);
                std::swap(b[3], b[4]);
                break;

            default:
                static const bool cannot_swap = true;
                assert(cannot_swap);
        }
        return x;
    }

    template<typename T>
    void write_msb(std::ostream & os, T x) {
        if (machine_is_lsb)
            x = change_endianness(x);
        os.write(reinterpret_cast<char *>(&x), sizeof(T));
    }

    template<typename T>
    void write_lsb(std::ostream & os, T x) {
        if (machine_is_msb)
            x = change_endianness(x);
        os.write(reinterpret_cast<char *>(&x), sizeof(T));
    }

    template<typename T>
    void read_msb(std::istream & is, T & x) {
        is.read(reinterpret_cast<char *>(&x), sizeof(T));
        if (machine_is_lsb)
        x = change_endianness(x);
    }

    template<typename T>
    void read_lsb(std::istream & is, T & x) {
        is.read(reinterpret_cast<char *>(&x), sizeof(T));
        if (machine_is_msb)
        x = change_endianness(x);
    }

}

// base classes for all units
// --------------------------

// fwd declare everything
namespace _ignore{
    class Unit;
    template<typename T>class UnitOperators;
}

// fwd declare everything
class Angle;      class Energy;
class Length;     class Force;
class Area;       class Temperature;
class Volume;     class Mass;
class Time;       class Speed;
class Density;    class Pressure;

// Math operators
template <typename T> T  abs(const _ignore::UnitOperators<T>& t);
template <typename T> T fabs(const _ignore::UnitOperators<T>& t);

// definition of unit base classes
namespace _ignore
{
    // Base class for all units
    class Unit
    {
    protected:
        double value;
        Unit(double value) : value(value) {}

    public:
        virtual ~Unit(){}

        Unit() : value(0.0) {}
        Unit(const Unit& obj) : value(obj.value) {}

        // enforce string representation
        virtual std::string toString() const = 0;

    };

    // RO: use CRTP pattern to have the definition of the operators all in
    // one place, while guaranteeing that only same-type operators work. See
    // http://stackoverflow.com/questions/12742728/
    template <typename T>
    class UnitOperators
        : public Unit
    {
        friend T  abs<T>(const UnitOperators<T>& t);
        friend T fabs<T>(const UnitOperators<T>& t);

    protected:
        UnitOperators(double value) : Unit(value){}

    public:
        virtual ~UnitOperators(){}

        UnitOperators()             : Unit()    {}
        UnitOperators(const T& obj) : Unit(obj) {}

        // RO : use static_cast, and make this class a friend class of all
        // classes subclassing it, as per
        // http://stackoverflow.com/questions/12910883/
        T&     operator= (const T& rhs)       { if (this!=&rhs) value = rhs.value; return static_cast<T&>(*this); }

        bool   operator> (const T& rhs) const { return value >  rhs.value; }
        bool   operator< (const T& rhs) const { return value <  rhs.value; }
        bool   operator>=(const T& rhs) const { return value >= rhs.value; }
        bool   operator<=(const T& rhs) const { return value <= rhs.value; }
        bool   operator==(const T& rhs) const { return value == rhs.value; }
        bool   operator!=(const T& rhs) const { return value != rhs.value; }

        T&     operator+=(const T& rhs)       { value += rhs.value; return static_cast<T&>(*this); }
        T&     operator-=(const T& rhs)       { value -= rhs.value; return static_cast<T&>(*this); }
        T&     operator*=(double F)           { value *= F; return static_cast<T&>(*this); }
        T&     operator/=(double F)           { value /= F; return static_cast<T&>(*this); }

        T      operator+ (const T& rhs) const { return T(*this) += rhs; }
        T      operator- (const T& rhs) const { return T(*this) -= rhs; }
        T      operator- ()             const { return T(-value);}
        T      operator+ ()             const { return T(+value);}

        T      operator* (double F)     const { return T(*this) *= F; }
        T      operator/ (double F)     const { return T(*this) /= F; }
        double operator* (const T& rhs) const { return value*rhs.value; }
        double operator/ (const T& rhs) const { return value/rhs.value; }

        // some nice constant instances
        static const T zero        ;
        static const T dx          ;
        static const T infinitesmal;
        static const T small       ;
        static const T NaN         ;
        static const T nan         ;
        static const T inf         ;
        static const T infinite    ;
        static const T infinity    ;

    };

}

/**
 * @brief Object with units [angle]. Aimed at avoiding confusion between degrees, radians,...
 */

/**
 * Angle
 * avoid confusion between degrees, radians, gradians
 *
 * @see Length @see Time @see Mass @see Temperature @see Energy
 * @see Area @see Force @see Speed
 *
 */

class Angle final
    : public _ignore::UnitOperators<Angle>
{
    typedef _ignore::UnitOperators<Angle> base;
    friend class _ignore::UnitOperators<Angle>;

private:

    // value is always in radians
    explicit Angle(double value) : base(value) {};

public:
   ~Angle(){}

    // default value: 0
    Angle() : base(){};
    // copy constructor
    Angle(const Angle& a2) : base(a2){};
    Angle(const base&  a2) : base(a2){};

    /// Angle is instantiated through named constructors
    static Angle
        radians    (double rad),
        degrees    (double deg),
        arcminutes (double moa),
        arcseconds (double soa),
        GRADIANS   (double grad); // NOTE: capitalized to avoid
                                  // confusion w/ radians
    /// Angle's different return types
    double
        radians    () const,
        degrees    () const,
        arcminutes () const,
        arcseconds () const,
        GRADIANS   () const; // NOTE: capitalized to avoid
                             // confusion w/ radians
    /// alternatively, one can use various pre-defined angles
    static const Angle
        tau ,   _360,
        pi  ,   _180,
        pio2,   _90 ,
        pio4,   _45 ,
        pio6,   _30 ;

    /// wrap value into [-pi +pi) / [-180 +180)
    Angle& wrap_posneg();
    friend Angle wrap_posneg(const Angle& a); // free function

    /// wrap value into [0 2pi) / [0 360)
    Angle& wrap_positive();
    friend Angle wrap_positive(const Angle& a); // free function

    // NOTE: angles have no units -- it's OK for these
    // operators to return Angles
    using _ignore::UnitOperators<Angle>::operator/;
    using _ignore::UnitOperators<Angle>::operator*;
    Angle
        operator/ (const Angle& a2) const,
        operator* (const Angle& a2) const;

    /// human-readable string representation
    std::string toString() const;
};

/**
 * @brief Object with units of Length. Makes metric/imperial less prone to error.
 */

/**
 * Lengths
 * make metric <-> imperial a bit less prone to error
 *
 * @see Angle @see Time @see Mass @see Temperature @see Energy
 * @see Area @see Force @see Speed
 */
class Length final
    : public _ignore::UnitOperators<Length>
{
    typedef _ignore::UnitOperators<Length> base;
    friend class _ignore::UnitOperators<Length>;

private:
    // value is always in [m]
    explicit Length(double value) : base(value) {}

public:
    ~Length(){}

    // default
    Length() : base() {}
    // Copy constructor
    Length (const Length& L2) : base(L2) {}
    Length (const base&   L2) : base(L2) {}

    /// Length is instantiated through named constructors
    static Length
        meters           (double m)  ,     m (double   m),
        kilometers       (double km) ,    km (double  km),
        nanometers       (double nm) ,    nm (double  nm),
        microns          (double mu) ,    um (double  um),
        micrometers      (double mu) ,
        inches           (double in) ,    in (double  in),
        feet             (double ft) ,    ft (double  ft),
        foot             (double ft) ,
        yards            (double yrd),
        miles            (double mi) ,    mi (double  mi),
        nauticalMiles    (double nmi),   nmi (double nmi),
        astronomicalUnits(double au ),    AU (double  au),
        angstroms        (double a)  ,     A (double   a);

    /// Length can return various units of length
    double
        meters            () const,   m() const,
        kilometers        () const,  km() const,
        nanometers        () const,  nm() const,
        microns           () const,  um() const,
        micrometers       () const,
        inches            () const,  in() const,
        feet              () const,  ft() const,
        foot              () const,
        yards             () const,
        miles             () const,  mi() const,
        nauticalMiles     () const, nmi() const,
        astronomicalUnits () const,  AU() const,
        angstroms         () const,   A() const;

    /// alternatively, one can use various constant lengths
    static const Length

        // SI
        one_km, kilometer,          // 1 kilometer
        one_m,  meter,              // 1 meter
        one_mm, millimeter,         // 1 millimeter
        one_um, micrometer, micron, // 1 micron
        one_nm, nanometer,          // 1 nanometer

        // NON-SI
        one_AU, astronomicalUnit,
        inch, one_ft, yard, mile, nauticalMile, angstrom;

    ///cast operators
    operator Energy() const; // wavelength to energy

    /// Human-readable string representation
    std::string toString() const;
};

/**
 * @brief
 */

/**
 * Times
 * make hours, minute, seconds, etc. readable
 *
 * @see Angle @see Length @see Mass @see Temperature @see Energy
 * @see Area @see Force @see Speed
 */
class Time final
    : public _ignore::UnitOperators<Time>
{
    typedef _ignore::UnitOperators<Time> base;
    friend class _ignore::UnitOperators<Time>;

private:
    /// value is always given in seconds
    explicit Time(double value) : _ignore::UnitOperators<Time>(value){}

public:
   ~Time(){}

    // default value
    Time() : base(){}
    // Copy constructor
    Time (const Time& t2) : base(t2){}
    Time (const base& t2) : base(t2){}

    /// Time is instantiated through named constructors
    static Time
        microseconds(double ms),
        milliseconds(double ms),
        seconds     (double s),
        minutes     (double m),
        hours       (double h),
        days        (double d),
        years       (double y),
        centuries   (double c);

    /// Length can return various units of time
    double
        microseconds() const,
        milliseconds() const,
        seconds     () const,
        minutes     () const,
        hours       () const,
        days        () const,
        years       () const,
        centuries   () const;

    /// alternatively, one can use various pre-defined lengths
    static const Time
        microsecond,
        millisecond,
        second,
        minute,
        hour,
        day,
        year,
        century;

    // current unix time (seconds+microseconds)
    static Time now ();
    // boost version
    static boost::posix_time::ptime now(void *);

    /// Human-readable string representation
    std::string toString() const;
};

/**
 * @brief
 */

/**
 * Mass
 * make metric <-> imperial a bit less prone to error
 *
 * @see Angle @see Length @see Temperature @see Energy
 * @see Area @see Force @see Speed @see Time
 */
class Mass final
    : public _ignore::UnitOperators<Mass>
{
    typedef _ignore::UnitOperators<Mass> base;
    friend class _ignore::UnitOperators<Mass>;

private:

    // value is always in kg
    explicit Mass(double value) : base(value){}

public:
    ~Mass(){}

    // default value
    Mass() : base(){}
    // Copy constructor
    Mass (const Mass& m2) : base(m2){}
    Mass (const base& m2) : base(m2){}

    // nice named constructors
    static Mass
        kilogrammes (double kg ),  kg  (double kg ),  kilograms (double kg ),
        pounds      (double lbs),  lbs (double lbs),
        tonnes      (double T  ),
        longTonnes  (double LT ),
        shortTonnes (double sT ),
        stones      (double st ),
        ounces      (double st );

    // various return values
    double
        kilogrammes() const,    kg  () const,  kilograms () const,
        pounds     () const,    lbs () const,
        tonnes     () const,
        longTonnes () const,
        shortTonnes() const,
        stones     () const,
        ounces     () const;

    // constants
    static const Mass
        kilo,
        pound,
        tonne,
        longTonne,
        shortTonne,
        stone,
        ounce;

    // Human-readable string representation
    std::string toString() const;
};

/**
 * @brief
 */

/**
 * Temperature
 *
 * @see Angle @see Length @see Mass @see Energy
 * @see Area @see Force @see Speed @see Time
 */
class Temperature final
    : public _ignore::UnitOperators<Temperature>
{
    typedef _ignore::UnitOperators<Temperature> base;
    friend class _ignore::UnitOperators<Temperature>;

private:

    // value is always in K
    // NOTE: ensure temperature is POSITIVE
    explicit Temperature(double value) : base(value<0.0?0.0:value) {}

public:
   ~Temperature(){}

    // default value
    Temperature() : base(){}
    // copy constructor
    Temperature (const Temperature& T2) : base(T2){}
    Temperature (const base&        T2) : base(T2){}

    // nice named constructors
    static Temperature
        kelvin     (double K),
        celsius    (double C),
        fahrenheit (double F);

    // various return values
    double
        kelvin     () const,
        celsius    () const,
        fahrenheit () const;

    // constants
    static const Temperature
        K, // ZERO kelvin
        C, // ZERO celcius
        F; // ZERO fahrenheit

    // NOTE: negative temperatures do not exist; exclude unary minus operator
    Temperature& operator-();

    // cast operators
    operator Energy() const;

    // Human-readable string representation
    std::string toString() const;
};

/// Combined unit classes / operations

/// Area = Length * Length
class Area final
    : public _ignore::UnitOperators<Area>
{
    typedef _ignore::UnitOperators<Area> base;
    friend class _ignore::UnitOperators<Area>;

private:
    // value is always in m²
    explicit Area (double value) : base(value){}

public:
   ~Area(){}

    // default value
    Area() : base(){}
    // copy constructor
    Area (const Area& E2) : base(E2){}
    Area (const base& E2) : base(E2){}

    // nice named constructors
    static Area
        squareMeters     (double m2  ),    m2 (double m2),
        squareKilometers (double m2  ),   km2 (double km2),
        squareMillimeters(double m2  ),   mm2 (double mm2),
        ares             (double are ),
        acres            (double acre),
        hectares         (double ha  );

    // various return values
    double
        squareMeters     () const,    m2 () const,
        squareKilometers () const,   km2 () const,
        squareMillimeters() const,   mm2 () const,
        ares             () const,
        acres            () const,
        hectares         () const;

    // constants
    const static Area
        squareMeter,
        squareKilometer,
        squareMillimeter,
        are,
        acre,
        hectare;

    // Human-readable string representation
    std::string toString() const;

};

/// Volume = Length * Length * Length
class Volume final
    : public _ignore::UnitOperators<Volume>
{
    typedef _ignore::UnitOperators<Volume> base;
    friend class _ignore::UnitOperators<Volume>;

private:
    // value is always in m³
    explicit Volume (double value) : base(value){}

public:
   ~Volume(){}

    // default value
    Volume() : base(){}
    // copy constructors
    Volume (const Volume& V2) : base(V2){}
    Volume (const base&   V2) : base(V2){}

    // nice named constructors
    static Volume
        cubicKilometers (double km3  ),
        cubicMeters     (double  m3  ),
        cubicCentimeters(double cm3  ),
        cubicMillimeters(double mm3  ),
        cubicFeet       (double ft3  ),
        cubicInches     (double in3  ),
        litres          (double l    ),
        gallons         (double g    ), // imperial gallon
        USgallons       (double g    ); // US gallon

    // various return values
    double
        cubicKilometers  () const,
        cubicMeters      () const,
        cubicCentimeters () const,
        cubicMillimeters () const,
        cubicFeet        () const,
        cubicInches      () const,
        litres           () const,
        gallons          () const,
        USgallons        () const;

    // constants
    const static Volume
        cubicKilometer,    km3,
        cubicMeter,         m3,
        cubicCentimeter,   cm3,
        cubicMillimeter,   mm3,
        cubicFoot,         ft3,
        cubicInch,         in3,
        litre,               l,
        gallon,              g,
        USgallon,           Ug;

    // Human-readable string representation
    std::string toString() const;

};

/// Energy = Mass * Length / Time
class Energy final
    : public _ignore::UnitOperators<Energy>
{
    typedef _ignore::UnitOperators<Energy> base;
    friend class _ignore::UnitOperators<Energy>;

private:
    // value is always in Joules
    explicit Energy (double value) : base(value){}

public:
   ~Energy(){}

    // default value
    Energy() : base(){}
    // copy constructor
    Energy (const Energy& E2) : base(E2){}
    Energy (const base&   E2) : base(E2){}

    // nice named constructors
    static Energy
        joules        (double J),
        electronVolts (double eV);

    // various return values
    double
        joules       () const,
        electronVolts() const;

    // handy constants
    static const Energy
        joule       ,   J,
        electronVolt,   eV;

    // cast operators
    operator Length()       const; // to wavelength
    operator Temperature () const; // to temperature

    // Human-readable string representation
    std::string toString() const;
};

/// Force = Mass * Length / Time / Time
class Force final
    : public _ignore::UnitOperators<Force>
{
    typedef _ignore::UnitOperators<Force> base;
    friend class _ignore::UnitOperators<Force>;

private:

    // value is always in Newtons
    explicit Force(double value) : base(value){}

public:
   ~Force(){}

    // default value
    Force() : base(){}
    // copy constructor
    Force (const Force& F2) : base(F2){}
    Force (const base&  F2) : base(F2){}

    // nice named constructors
    static Force
        newtons (double N),  N (double n);

    // various return values
    double
        newtons() const,   N () const;

    // constants
    static const Force
        newton;

    /// Human-readable string representation
    std::string toString() const;
};

/// Speed = Length / Time
class Speed final
    : public _ignore::UnitOperators<Speed>
{
    typedef _ignore::UnitOperators<Speed> base;
    friend class _ignore::UnitOperators<Speed>;

private:

    // value is always given in m/s
    explicit Speed (double value) : base(value){}

public:
    ~Speed(){}

    // default value
    Speed() : base(){}
    // copy  constructor
    Speed (const Speed& s2) : base(s2){}
    Speed (const base&  s2) : base(s2){}

    // nice named constructors
    static Speed
        metersPerSecond     (double mps),    mps  (double mps),
        knots               (double knt),    kt   (double knt),
        kilometersPerHour   (double kph),    kph  (double kph),
        kilometersPerSecond (double kps),    kps  (double kps),
        milesPerHour        (double mph),    miph (double mph),
        milesPerSecond      (double mps),    mips (double mps);

    // various return values
    double
        metersPerSecond     () const,    mps  () const,
        knots               () const,    kt   () const,
        kilometersPerHour   () const,    kph  () const,
        kilometersPerSecond () const,    kps  () const,
        milesPerHour        () const,    miph () const,
        milesPerSecond      () const,    mips () const;

    // constants
    static const Speed
        meterPerSecond    ,
        knot              ,
        kilometerPerHour  ,
        kilometerPerSecond,
        milePerHour       ,
        milePerSecond     ,

        of_light          , c;

    // NOTE: in principle, speed is a magnitude and thus always positive.
    // However, negative speeds are used, so include unary minus operator.

    // Human-readable string representation
    std::string toString() const;
};

/// Pressure = Force / Area
class Pressure final
    : public _ignore::UnitOperators<Pressure>
{
    typedef _ignore::UnitOperators<Pressure> base;
    friend class _ignore::UnitOperators<Pressure>;

private:

     // value is always given in N/m²
    explicit Pressure (double value) : base(value){}

public:
    ~Pressure(){}

    // default value
    Pressure() : base(){}
    // copy  constructors
    Pressure (const Pressure& p2) : base(p2){}
    Pressure (const base&     p2) : base(p2){}

    // nice named constructors
    static Pressure
        pascals     (double Pa ),    Pa (double Pa ),
        kiloPascals (double kPa),   kPa (double kPa),
        bars        (double bar),
        atmospheres (double atm),   atm(double atm);

    // various return values

    // constants

    /// Human-readable string representation
    std::string toString() const;
};

/// Density = Mass / Volume
class Density final
    : public _ignore::UnitOperators<Density>
{
    typedef _ignore::UnitOperators<Density> base;
    friend class _ignore::UnitOperators<Density>;

private:

     // value is always given in kg/m³
    explicit Density (double value) : base(value){}

public:
    ~Density(){}

    // default value
    Density() : base(){}
    // copy  constructors
    Density (const Density& d2) : base(d2){}
    Density (const base&    d2) : base(d2){}

    // nice named constructors
    static Density
        kilogramsPerCubicMeter  (double kgm3),
        gramsPerCubicCentimeter (double gcm3),
        tonnesPerCubicMeter     (double Mgm3),
        kilogramsPerLiter       (double kgl ),
        poundsPerCubicInch      (double lbscuin),
        poundsPerCubicFoot      (double lbscuft);

    // various return values
    double
        kilogramsPerCubicMeter  (),
        gramsPerCubicCentimeter (),
        tonnesPerCubicMeter     (),
        kilogramsPerLiter       (),
        poundsPerCubicInch      (),
        poundsPerCubicFoot      ();

    // constants
    static const Density
        kgm3,    kilogramPerCubicMeter,
        gcm3,    gramPerCubicCentimeter,
        Mgm3,    tonPerCubicMeter,         Tm3,
        kgl ,    kilogramPerLiter,
        lbscuin, poundPerCubicInch,        lbsin,
        lbscuft, poundPerCubicFoot,        lbsft;

    /// Human-readable string representation
    std::string toString() const;

};

/// overload various relevant math operators

// Angle
// ---------------------------------

// wrap value into [-pi +pi) / [-180 +180)
Angle wrap_posneg(const Angle& a);

// wrap value into [0 2pi) / [0 360)
Angle wrap_positive(const Angle& a);

// regular trig functions
double sin  (const Angle& a);
double cos  (const Angle& a);
double tan  (const Angle& a);
// FIXME: namespace conflict:
//double csc  (const Angle& a);
double sec  (const Angle& a);
double cot  (const Angle& a);

// hyperbolic trig functions
double sinh (const Angle& a);
double cosh (const Angle& a);
double tanh (const Angle& a);
double csch (const Angle& a);
double sech (const Angle& a);
double coth (const Angle& a);

// Inverse trig functions need to be overloaded by return type,
// which is "obviously" not possible. Therefore, define a few
// leaf helper classes:

namespace _ignore
{
    class Invtrig : public UnitOperators<Angle> {
    protected:
        double value;
    public:
        operator double();
        operator Angle ();
        std::string toString() const; // NOTE: leave unimplemented
    };
}

// regular inverse functions
class Asin   final : public _ignore::Invtrig { public: Asin (double value); };
class Acos   final : public _ignore::Invtrig { public: Acos (double value); };
class Atan   final : public _ignore::Invtrig { public: Atan (double value); };
class Atan2  final : public _ignore::Invtrig { public: Atan2(double y, double x); };
class Acsc   final : public _ignore::Invtrig { public: Acsc (double value); };
class Asec   final : public _ignore::Invtrig { public: Asec (double value); };
class Acot   final : public _ignore::Invtrig { public: Acot (double value); };

// hyperbolic inverse functions
class Asinh  final : public _ignore::Invtrig { public: Asinh(double value); };
class Acosh  final : public _ignore::Invtrig { public: Acosh(double value); };
class Atanh  final : public _ignore::Invtrig { public: Atanh(double value); };
class Acsch  final : public _ignore::Invtrig { public: Acsch(double value); };
class Asech  final : public _ignore::Invtrig { public: Asech(double value); };
class Acoth  final : public _ignore::Invtrig { public: Acoth(double value); };

/// Make  std::cout << {class}  work
std::ostream& operator<<(std::ostream& target, const _ignore::Unit& U);

/// Make some cross-unit transformations valid

// sqrt(Area) = Length
Length sqrt(const Area& A);

// Length * Length = Area
Area   operator*(const Length& L1, const Length& L2);

// Length * Area = Volume
Volume operator*(const Length&  L, const Area&   A);
Volume operator*(const Area&    A, const Length& L);

// Mass / Volume = Density
Density operator/(const Mass&   M, const Volume& V);

// Length / Time = Speed
Speed operator/ (const Length&  L, const Time&   t);

// Speed * Time = Length
Length operator*(const Speed&   s, const Time&   t);
Length operator*(const Time&    t, const Speed&  s);

// Length * Angle = (arc)Length
Length operator*(const Length&  L, const Angle&  a);
Length operator*(const Angle&   a, const Length& L);

// Force * Length = Energy
Energy operator*(const Force&   F, const Length& L);
Energy operator*(const Length&  L, const Force&  F);

// Force / Area = Pressure
Pressure operator/(const Force& F, const Area&   A);

// Handy dandy physics utilities
// (defined HERE because otherwise the unit classes are incomplete)
namespace physics
{
    // FIXME: (Rody Oldenhuis) DEPRECATED
    // replaced by operator Length() in class Energy
    inline double energy2wavelength(const double e) {
        BOOST_ASSERT(e > 0);
        return h*c/e;
    }
    inline Length energy2wavelength(const Energy& E){
        BOOST_ASSERT(E > Energy::zero);
        return Length::meters(h*c/E.joules());
    }
    // -----------------------------------------

    // FIXME: (Rody Oldenhuis) DEPRECATED
    // replaced by operator Energy() in class Length
    inline double wavelength2energy(const double w) {
        BOOST_ASSERT(w > 0);
        return h*c/w;
    }
    inline Energy wavelength2energy(const Length& w) {
        BOOST_ASSERT(w > Length::zero);
        return Energy::joules(h*c/w.meters());
    }
    // -----------------------------------------

    // FIXME: (Rody Oldenhuis) DEPRECATED
    // replaced by operator Temperature() in class Energy
    inline double energy2temperature(const double e) {
        BOOST_ASSERT(e >= 0);
        return e/k;
    }
    inline Temperature energy2temperature(const Energy& E) {
        BOOST_ASSERT(E >= Energy::zero);
        return Temperature::kelvin(E.joules()/k);
    }
    // -----------------------------------------

    // FIXME: (Rody Oldenhuis) DEPRECATED
    // replaced by operator Energy() in class Temperature
    inline double temperature2energy(const double T) {
        BOOST_ASSERT(T >= 0);
        return T*k;
    }
    inline Energy temperature2energy(const Temperature& T) {
        BOOST_ASSERT(T >= Temperature::zero);
        return Energy::joules(T.kelvin()*k);
    }
    // -----------------------------------------
}

#endif


#ifndef INVKIN_H_INCLUDED
#define INVKIN_H_INCLUDED

#include "pinocchio/math/rpy.hpp"
#include "pinocchio/spatial/explog.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>

class InvKin
{
public:
    InvKin();
    InvKin(double dt_in);

    Eigen::Matrix<double, 1, 3> cross3(Eigen::Matrix<double, 1, 3> left, Eigen::Matrix<double, 1, 3> right);

    Eigen::MatrixXd refreshAndCompute(const Eigen::MatrixXd& contacts,
                                      const Eigen::MatrixXd& goals, const Eigen::MatrixXd& vgoals, const Eigen::MatrixXd& agoals,
                                      const Eigen::MatrixXd& posf, const Eigen::MatrixXd& vf, const Eigen::MatrixXd& wf,
                                      const Eigen::MatrixXd& af, const Eigen::MatrixXd& Jf);
    Eigen::MatrixXd get_q_step();
    Eigen::MatrixXd get_dq_cmd();

private:
    // Inputs of the constructor
    double dt;  // Time step of the contact sequence (time step of the MPC)

    // Matrices initialisation
    Eigen::Matrix<double, 4, 3> feet_position_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> feet_velocity_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> feet_acceleration_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 1, 4> flag_in_contact = Eigen::Matrix<double, 1, 4>::Zero();

    Eigen::Matrix<double, 12, 12> invJ = Eigen::Matrix<double, 12, 12>::Zero();
    Eigen::Matrix<double, 1, 12> acc = Eigen::Matrix<double, 1, 12>::Zero();
    Eigen::Matrix<double, 1, 12> x_err = Eigen::Matrix<double, 1, 12>::Zero();
    Eigen::Matrix<double, 1, 12> dx_r = Eigen::Matrix<double, 1, 12>::Zero();

    Eigen::Matrix<double, 4, 3> pfeet_err = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> vfeet_ref = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 4, 3> afeet = Eigen::Matrix<double, 4, 3>::Zero();
    Eigen::Matrix<double, 1, 3> e_basispos = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> abasis = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> e_basisrot = Eigen::Matrix<double, 1, 3>::Zero();
    Eigen::Matrix<double, 1, 3> awbasis = Eigen::Matrix<double, 1, 3>::Zero();

    Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(12, 1);
    Eigen::MatrixXd q_step = Eigen::MatrixXd::Zero(12, 1);
    Eigen::MatrixXd dq_cmd = Eigen::MatrixXd::Zero(12, 1);

    // Gains
    double Kp_flyingfeet = 100.0; // 1000
    double Kd_flyingfeet = 2.0 * std::sqrt(Kp_flyingfeet); // 5.0 *
};

template <typename _Matrix_Type_>
_Matrix_Type_ pseudoInverse(const _Matrix_Type_& a, double epsilon = std::numeric_limits<double>::epsilon())
{
    Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
    double tolerance = epsilon * static_cast<double>(std::max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
    return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
}
#endif  // INVKIN_H_INCLUDED


#include "gtest/gtest.h"
#include "test_convergence.h"
#include <boost/math/constants/constants.hpp>

// TODO: test estimation with model error
class KalmanConvergenceTest: public ConvergenceTest {
    public:
        void simulate(size_t N = default_simulation_length) {
            for (unsigned int i = 0; i < N; ++i) {
                m_x = m_bicycle->update_state(m_x);

                auto z = m_bicycle->calculate_output(m_x);
                z(0) += m_r0(m_gen);
                z(1) += m_r1(m_gen);

                m_kalman->time_update();
                m_kalman->measurement_update(z);

                // stop simulation if frame is horizontal (bike has fallen)
                if ((m_x[1] >= boost::math::constants::half_pi<model::real_t>()) ||
                        (m_x[1] < -boost::math::constants::half_pi<model::real_t>())) {
                    break;
                }
            }
        }

        void simulate_with_random_steer_input(size_t N = default_simulation_length) {
            // observed steer torques in experiments are between 1 to 3 Nm
            std::normal_distribution<> ru = std::normal_distribution<>(0, 2); // Nm, steer torque
            model::BicycleWhipple::input_t u = model::BicycleWhipple::input_t::Zero();

            for (unsigned int i = 0; i < N; ++i) {
                u(1) = ru(m_gen);
                m_x = m_bicycle->update_state(m_x, u);

                auto z = m_bicycle->calculate_output(m_x, u);
                z(0) += m_r0(m_gen);
                z(1) += m_r1(m_gen);

                m_kalman->time_update(u);
                m_kalman->measurement_update(z);

                // stop simulation if frame is horizontal (bike has fallen)
                if ((m_x[1] >= boost::math::constants::half_pi<model::real_t>()) ||
                        (m_x[1] < -boost::math::constants::half_pi<model::real_t>())) {
                    break;
                }
            }
        }
};

class KalmanConvergenceTest_HighSpeed: public KalmanConvergenceTest { };
class KalmanConvergenceTest_LowSpeed: public KalmanConvergenceTest { };

TEST_P(KalmanConvergenceTest_HighSpeed, ZeroInput) {
    simulate();
    test_state_near(m_kalman->x(), x_true());
}

TEST_P(KalmanConvergenceTest_HighSpeed, RandomInput) {
    simulate_with_random_steer_input();
    test_state_near(m_kalman->x(), x_true());
}

INSTANTIATE_TEST_CASE_P(
    ConvergenceRange_3_10,
    KalmanConvergenceTest_HighSpeed,
    ::testing::Range(static_cast<model::real_t>(3.0),
        static_cast<model::real_t>(10.0),
        static_cast<model::real_t>(0.5)));

TEST_P(KalmanConvergenceTest_LowSpeed, ZeroInput) {
    simulate_with_random_steer_input(200); // 100 iterations instead of 1000
    test_state_near(m_kalman->x(), x_true());
}

TEST_P(KalmanConvergenceTest_LowSpeed, RandomInput) {
    simulate_with_random_steer_input(200); // 100 iterations instead of 1000
    test_state_near(m_kalman->x(), x_true());
}

INSTANTIATE_TEST_CASE_P(
    ConvergenceRange_1_3,
    KalmanConvergenceTest_LowSpeed,
    ::testing::Range(static_cast<model::real_t>(1.0),
        static_cast<model::real_t>(3.0),
        static_cast<model::real_t>(0.5)));


/* boost random/uniform_real_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_real_distribution.hpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#ifndef BOOST_RANDOM_UNIFORM_REAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_UNIFORM_REAL_DISTRIBUTION_HPP

#include <iosfwd>
#include <ios>
#include <istream>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/signed_unsigned_tools.hpp>
#include <boost/type_traits/is_integral.hpp>

namespace boost {
namespace random {
namespace detail {

template<class Engine, class T>
T generate_uniform_real(
    Engine& eng, T min_value, T max_value,
    boost::mpl::false_  /** is_integral<Engine::result_type> */)
{
    for(;;) {
        typedef T result_type;
        typedef typename Engine::result_type base_result;
        result_type numerator = static_cast<T>(eng() - (eng.min)());
        result_type divisor = static_cast<T>((eng.max)() - (eng.min)());
        BOOST_ASSERT(divisor > 0);
        BOOST_ASSERT(numerator >= 0 && numerator <= divisor);
        T result = numerator / divisor * (max_value - min_value) + min_value;
        if(result < max_value) return result;
    }
}

template<class Engine, class T>
T generate_uniform_real(
    Engine& eng, T min_value, T max_value,
    boost::mpl::true_  /** is_integral<Engine::result_type> */)
{
    for(;;) {
        typedef T result_type;
        typedef typename Engine::result_type base_result;
        result_type numerator = static_cast<T>(subtract<base_result>()(eng(), (eng.min)()));
        result_type divisor = static_cast<T>(subtract<base_result>()((eng.max)(), (eng.min)())) + 1;
        BOOST_ASSERT(divisor > 0);
        BOOST_ASSERT(numerator >= 0 && numerator <= divisor);
        T result = numerator / divisor * (max_value - min_value) + min_value;
        if(result < max_value) return result;
    }
}

template<class Engine, class T>
inline T generate_uniform_real(Engine& eng, T min_value, T max_value)
{
    typedef typename Engine::result_type base_result;
    return generate_uniform_real(eng, min_value, max_value,
        boost::is_integral<base_result>());
}

}

/**
 * The class template uniform_real_distribution models a \random_distribution.
 * On each invocation, it returns a random floating-point value uniformly
 * distributed in the range [min..max).
 */
template<class RealType = double>
class uniform_real_distribution
{
public:
    typedef RealType input_type;
    typedef RealType result_type;

    class param_type
    {
    public:

        typedef uniform_real_distribution distribution_type;

        /**
         * Constructs the parameters of a uniform_real_distribution.
         *
         * Requires min <= max
         */
        explicit param_type(RealType min_arg = RealType(0.0),
                            RealType max_arg = RealType(1.0))
          : _min(min_arg), _max(max_arg)
        {
            BOOST_ASSERT(_min <= _max);
        }

        /** Returns the minimum value of the distribution. */
        RealType a() const { return _min; }
        /** Returns the maximum value of the distribution. */
        RealType b() const { return _max; }

        /** Writes the parameters to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._min << " " << parm._max;
            return os;
        }

        /** Reads the parameters from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            RealType min_in, max_in;
            if(is >> min_in >> std::ws >> max_in) {
                if(min_in <= max_in) {
                    parm._min = min_in;
                    parm._max = max_in;
                } else {
                    is.setstate(std::ios_base::failbit);
                }
            }
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._min == rhs._min && lhs._max == rhs._max; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:

        RealType _min;
        RealType _max;
    };

    /**
     * Constructs a uniform_real_distribution. @c min and @c max are
     * the parameters of the distribution.
     *
     * Requires: min <= max
     */
    explicit uniform_real_distribution(
        RealType min_arg = RealType(0.0),
        RealType max_arg = RealType(1.0))
      : _min(min_arg), _max(max_arg)
    {
        BOOST_ASSERT(min_arg <= max_arg);
    }
    /** Constructs a uniform_real_distribution from its parameters. */
    explicit uniform_real_distribution(const param_type& parm)
      : _min(parm.a()), _max(parm.b()) {}

    /**  Returns the minimum value of the distribution */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _min; }
    /**  Returns the maximum value of the distribution */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _max; }

    /**  Returns the minimum value of the distribution */
    RealType a() const { return _min; }
    /**  Returns the maximum value of the distribution */
    RealType b() const { return _max; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_min, _max); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _min = parm.a();
        _max = parm.b();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Returns a value uniformly distributed in the range [min, max). */
    template<class Engine>
    result_type operator()(Engine& eng) const
    { return detail::generate_uniform_real(eng, _min, _max); }

    /**
     * Returns a value uniformly distributed in the range
     * [param.a(), param.b()).
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm) const
    { return detail::generate_uniform_real(eng, parm.a(), parm.b()); }

    /** Writes the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, uniform_real_distribution, ud)
    {
        os << ud.param();
        return os;
    }

    /** Reads the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, uniform_real_distribution, ud)
    {
        param_type parm;
        if(is >> parm) {
            ud.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two distributions will produce identical sequences
     * of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(uniform_real_distribution, lhs, rhs)
    { return lhs._min == rhs._min && lhs._max == rhs._max; }

    /**
     * Returns true if the two distributions may produce different sequences
     * of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(uniform_real_distribution)

private:
    RealType _min;
    RealType _max;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_INT_HPP


// test_find_location.cpp

// Copyright John Maddock 2007.
// Copyright Paul A. Bristow 2007.

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

// Basic sanity test for find_location function.

// Default domain error policy is
// #define BOOST_MATH_DOMAIN_ERROR_POLICY throw_on_error

#include <pch.hpp>

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // Default type double.
  using boost::math::normal_distribution; // All floating-point types.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy;
#include <boost/math/distributions/pareto.hpp> // for cauchy_distribution
  using boost::math::pareto;
#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
  using boost::math::complement;// will be needed by users who want complement,
#include <boost/math/policies/policy.hpp>
  using boost::math::policies::policy;

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // for test_main
#include <boost/test/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE_FRACTION, BOOST_CHECK_EQUAL...

#include <iostream>
#include <iomanip>
  using std::cout; using std::endl; using std::fixed;
  using std::right; using std::left; using std::showpoint;
  using std::showpos; using std::setw; using std::setprecision;

#include <limits>
  using std::numeric_limits;

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{ // Parameter only provides the type, float, double... value ignored.

  // Basic sanity checks, test data may be to double precision only
  // so set tolerance to 100 eps expressed as a fraction,
  // or 100 eps of type double expressed as a fraction,
  // whichever is the larger.

  RealType tolerance = (std::max)
      (boost::math::tools::epsilon<RealType>(),
      static_cast<RealType>(std::numeric_limits<double>::epsilon()));
   tolerance *= 100; // 100 eps as a fraction.

  cout << "Tolerance for type " << typeid(RealType).name()  << " is "
    << setprecision(3) << tolerance  << " (or " << tolerance * 100 << "%)." << endl;

  BOOST_CHECK_THROW( // Probability outside 0 to 1.
       find_location<normal_distribution<RealType> >(
       static_cast<RealType>(0.), static_cast<RealType>(-1.), static_cast<RealType>(0.) ),
       std::domain_error);

  normal_distribution<RealType> n; // standard N(0,1)
  BOOST_CHECK_EQUAL(n.location(), 0); // aka mean.
  BOOST_CHECK_EQUAL(n.scale(), 1); // aka standard_deviation.

   // Check for 'bad' arguments.
  BOOST_CHECK_THROW(find_location<normal>(0., -1., 0.), std::domain_error); // p below 0 to 1.
  BOOST_CHECK_THROW(find_location<normal>(0., 2., 0.), std::domain_error); // p above 0 to 1.
  BOOST_CHECK_THROW(find_location<normal>(numeric_limits<double>::infinity(), 0.5, 0.),
    std::domain_error); // z not finite.
  BOOST_CHECK_THROW(find_location<normal>(numeric_limits<double>::quiet_NaN(), -1., 0.),
    std::domain_error); // z not finite
  BOOST_CHECK_THROW(find_location<normal>(0., -1., numeric_limits<double>::quiet_NaN()),
    std::domain_error); // scale not finite

  BOOST_CHECK_THROW(find_location<normal>(complement(0., -1., 0.)), std::domain_error); // p below 0 to 1.
  BOOST_CHECK_THROW(find_location<normal>(complement(0., 2., 0.)), std::domain_error); // p above 0 to 1.
  BOOST_CHECK_THROW(find_location<normal>(complement(numeric_limits<double>::infinity(), 0.5, 0.)),
    std::domain_error); // z not finite.
  BOOST_CHECK_THROW(find_location<normal>(complement(numeric_limits<double>::quiet_NaN(), -1., 0.)),
    std::domain_error); // z not finite
  BOOST_CHECK_THROW(find_location<normal>(complement(0., -1., numeric_limits<double>::quiet_NaN())),
    std::domain_error); // scale not finite

  //// Check for ab-use with unsuitable distribution(s) when concept check implemented.
  // BOOST_CHECK_THROW(find_location<pareto>(0., 0.5, 0.), std::domain_error); // pareto can't be used with find_location.

  // Check doesn't throw when an ignore_error for domain_error policy is used.
  using boost::math::policies::policy;
  using boost::math::policies::domain_error;
  using boost::math::policies::ignore_error;

  // Define a (bad?) policy to ignore domain errors ('bad' arguments):
  typedef policy<domain_error<ignore_error> > ignore_domain_policy;
  // Using a typedef is convenient, especially if it is re-used.

  BOOST_CHECK_NO_THROW(find_location<normal>(0, -1, 1,
    ignore_domain_policy())); // probability outside [0, 1]
  BOOST_CHECK_NO_THROW(find_location<normal>(numeric_limits<double>::infinity(), -1, 1,
    ignore_domain_policy())); // z not finite.

  BOOST_CHECK_NO_THROW(find_location<normal>(complement(0, -1, 1,
    ignore_domain_policy()))); // probability outside [0, 1]
  BOOST_CHECK_NO_THROW(find_location<normal>(complement(numeric_limits<double>::infinity(), -1, 1,
    ignore_domain_policy()))); // z not finite.

  // Find location to give a probability p (0.05) of z (-2)
  RealType sd = static_cast<RealType>(1); // normal default standard deviation = 1.
  RealType z = static_cast<RealType>(-2); // z to give prob p
  RealType p = static_cast<RealType>(0.05); // only 5% will be below z
  RealType l = find_location<normal_distribution<RealType> >(z, p, sd);
  // cout << z << " " << p << " " << sd << " " << l << endl;

  normal_distribution<RealType> np05pc(l, sd); // Same standard_deviation (scale) but with mean(location) shifted.
  //  cout << "Normal distribution with mean = " << l << " has " << "fraction <= " << z << " = "  << cdf(np05pc, z) << endl;
  // Check cdf such that only fraction p really is below offset mean l.
  BOOST_CHECK_CLOSE_FRACTION(p, cdf(np05pc, z), tolerance);

  // Check that some policies can be applied (though not used here).
  l = find_location<normal_distribution<RealType> >(z, p, sd, policy<>()); // Default policy, needs using boost::math::policies::policy;
  l = find_location<normal_distribution<RealType> >(z, p, sd, boost::math::policies::policy<>()); // Default policy, fully specified.
  l = find_location<normal_distribution<RealType> >(z, p, sd, ignore_domain_policy()); // find_location with new policy, using typedef.
  l = find_location<normal_distribution<RealType> >(z, p, sd, policy<domain_error<ignore_error> >()); // New policy, without typedef.

  // Check that can use the complement version.
  RealType q = 1 - p; // complement.
  // cout << "find_location<normal_distribution<RealType> >(complement(z, q, sd)) = " << endl;
  l = find_location<normal_distribution<RealType> >(complement(z, q, sd));

  normal_distribution<RealType> np95pc(l, sd); // Same standard_deviation (scale) but with mean(location) shifted
  //  cout << "Normal distribution with mean = " << l << " has " << "fraction <= " << z << " = "  << cdf(np95pc, z) << endl;
  BOOST_CHECK_CLOSE_FRACTION(q, cdf(np95pc, z), tolerance);

} // template <class RealType>void test_spots(RealType)

BOOST_AUTO_TEST_CASE( test_main )
{
  // Basic sanity-check spot values.

  // (Parameter value, arbitrarily zero, only communicates the floating-point type).
  test_spots(0.0F); // Test float.
  test_spots(0.0); // Test double.
  #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_spots(0.0L); // Test long double.
  #if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x0582))
    test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
  #endif
  #else
     std::cout << "<note>The long double tests have been disabled on this platform "
        "either because the long double overloads of the usual math functions are "
        "not available at all, or because they are too inaccurate for these tests "
        "to pass.</note>" << std::cout;
  #endif

} // BOOST_AUTO_TEST_CASE( test_main )

/*

Output is:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_find_location.exe"
Running 1 test case...
Tolerance for type float is 1.19e-005 (or 0.00119%).
Tolerance for type double is 2.22e-014 (or 2.22e-012%).
Tolerance for type long double is 2.22e-014 (or 2.22e-012%).
Tolerance for type class boost::math::concepts::real_concept is 2.22e-014 (or 2.22e-012%).
*** No errors detected

*/


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_init_functions

#include <boost/test/unit_test.hpp>

#include <limbo/acqui.hpp>
#include <limbo/bayes_opt/boptimizer.hpp>
#include <limbo/init.hpp>
#include <limbo/tools/macros.hpp>

using namespace limbo;

struct Params {
    struct bayes_opt_bobase : public defaults::bayes_opt_bobase {
        BO_PARAM(bool, stats_enabled, false);
    };

    struct bayes_opt_boptimizer : public defaults::bayes_opt_boptimizer {
    };

    struct stop_maxiterations {
        BO_PARAM(int, iterations, 0);
    };

    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 0.01);
    };

    struct kernel_maternfivehalves : public defaults::kernel_maternfivehalves {
        BO_PARAM(double, sigma_sq, 1);
        BO_PARAM(double, l, 0.25);
    };

    struct acqui_ucb : public defaults::acqui_ucb {
    };

#ifdef USE_NLOPT
    struct opt_nloptnograd : public defaults::opt_nloptnograd {
    };
#elif defined(USE_LIBCMAES)
    struct opt_cmaes : public defaults::opt_cmaes {
    };
#else
    struct opt_gridsearch : public defaults::opt_gridsearch {
    };
#endif
};

struct fit_eval {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, 1);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        double res = 0;
        for (int i = 0; i < x.size(); i++)
            res += 1 - (x[i] - 0.3) * (x[i] - 0.3) + sin(10 * x[i]) * 0.2;
        return tools::make_vector(res);
    }
};

BOOST_AUTO_TEST_CASE(no_init)
{
    std::cout << "NoInit" << std::endl;
    using Init_t = init::NoInit<Params>;
    using Opt_t = bayes_opt::BOptimizer<Params, initfun<Init_t>>;

    Opt_t opt;
    opt.optimize(fit_eval());
    BOOST_CHECK(opt.observations().size() == 0);
    BOOST_CHECK(opt.samples().size() == 0);
}

BOOST_AUTO_TEST_CASE(random_lhs)
{
    std::cout << "LHS" << std::endl;
    struct MyParams : public Params {
        struct init_lhs {
            BO_PARAM(int, samples, 10);
        };
    };

    using Init_t = init::LHS<MyParams>;
    using Opt_t = bayes_opt::BOptimizer<MyParams, initfun<Init_t>>;

    Opt_t opt;
    opt.optimize(fit_eval());
    BOOST_CHECK(opt.observations().size() == 10);
    BOOST_CHECK(opt.samples().size() == 10);
    for (size_t j = 0; j < opt.samples().size() - 1; ++j) {
        const Eigen::VectorXd& x = opt.samples()[j];
        std::cout << x.transpose() << std::endl;
        for (int i = 0; i < x.size(); ++i) {
            BOOST_CHECK(x[i] >= 0);
            BOOST_CHECK(x[i] <= 1);
            BOOST_CHECK(i == 0 || x[i] != x[0]);
        }
    }
}

BOOST_AUTO_TEST_CASE(random_sampling)
{
    std::cout << "RandomSampling" << std::endl;
    struct MyParams : public Params {
        struct init_randomsampling {
            BO_PARAM(int, samples, 10);
        };
    };

    using Init_t = init::RandomSampling<MyParams>;
    using Opt_t = bayes_opt::BOptimizer<MyParams, initfun<Init_t>>;

    Opt_t opt;
    opt.optimize(fit_eval());
    BOOST_CHECK(opt.observations().size() == 10);
    BOOST_CHECK(opt.samples().size() == 10);
    for (size_t j = 0; j < opt.samples().size() - 1; ++j) {
        const Eigen::VectorXd& x = opt.samples()[j];
        std::cout << x.transpose() << std::endl;
        for (int i = 0; i < x.size(); ++i) {
            BOOST_CHECK(x[i] >= 0);
            BOOST_CHECK(x[i] <= 1);
            BOOST_CHECK(i == 0 || x[i] != x[0]);
        }
    }
}

BOOST_AUTO_TEST_CASE(random_sampling_grid)
{
    std::cout << "RandomSamplingGrid" << std::endl;
    struct MyParams : public Params {
        struct init_randomsamplinggrid {
            BO_PARAM(int, samples, 10);
            BO_PARAM(int, bins, 4);
        };
    };

    using Init_t = init::RandomSamplingGrid<MyParams>;
    using Opt_t = bayes_opt::BOptimizer<MyParams, initfun<Init_t>>;

    Opt_t opt;
    opt.optimize(fit_eval());
    BOOST_CHECK(opt.observations().size() == 10);
    BOOST_CHECK(opt.samples().size() == 10);
    for (size_t j = 0; j < opt.samples().size() - 1; ++j) {
        const Eigen::VectorXd& x = opt.samples()[j];
        std::cout << x.transpose() << std::endl;
        for (int i = 0; i < x.size(); ++i) {
            BOOST_CHECK(x[i] >= 0);
            BOOST_CHECK(x[i] <= 1);
            BOOST_CHECK(x[i] == 0 || x[i] == 0.25 || x[i] == 0.5 || x[i] == 0.75 || x[i] == 1.0);
        }
    }
}

BOOST_AUTO_TEST_CASE(grid_sampling)
{
    std::cout << "GridSampling" << std::endl;
    struct MyParams : public Params {
        struct init_gridsampling {
            BO_PARAM(int, bins, 4);
        };
    };

    using Init_t = init::GridSampling<MyParams>;
    using Opt_t = bayes_opt::BOptimizer<MyParams, initfun<Init_t>>;

    Opt_t opt;
    opt.optimize(fit_eval());
    std::cout << opt.observations().size() << std::endl;
    BOOST_CHECK(opt.observations().size() == 25);
    BOOST_CHECK(opt.samples().size() == 25);
    for (size_t j = 0; j < opt.samples().size() - 1; ++j) {
        const Eigen::VectorXd& x = opt.samples()[j];
        std::cout << x.transpose() << std::endl;
        for (int i = 0; i < x.size(); ++i) {
            BOOST_CHECK(x[i] >= 0);
            BOOST_CHECK(x[i] <= 1);
            BOOST_CHECK(x[i] == 0 || x[i] == 0.25 || x[i] == 0.5 || x[i] == 0.75 || x[i] == 1.0);
        }
    }
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifdef Cygwin
// availability of std::to_string
#define _GLIBCXX_USE_C99 1
#endif

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"
#include "utilities/kaskopt.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;

#include "integrate-navierStokes.hh"
//#include "integrate-Limex.hh"

#include "navierStokes.hh"

struct Initial_1Value
{
  using Scalar = double;
  static int const components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  Initial_1Value(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const& localCoordinate) const
  {
    Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);

    if (component==0)               // vector u = (u_1,u_2)
    {
      std::cout << "Error: wrong call of Initial_1Value" << std::endl;
      return 0;
    }
    else if (component==1)          // scalar p
    {
      return 0;
    }
    else
      assert("wrong index!\n"==0);

    return 0;
  }

private:
  int component;
};

struct Initial_2Value
{
  using Scalar = double;
  static int const components = 2;
  using ValueType = Dune::FieldVector<Scalar,components>;

  Initial_2Value(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const& localCoordinate) const
  {
    Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);

    ValueType result(0);
    if (component==0)                    // vector u = (u_1,u_2), concrete
    {
      return result;
    }
    else
      assert("wrong index!\n"==0);

    return result;
  }

private:
  int component;
};

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  int verbosityOpt = 1;  // print to console if arguments are changed
  bool dump = false; // do not write properties into file
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  std::cout << "Start Navier-Stokes tutorial programm." << std::endl;

  boost::timer::cpu_timer totalTimer;

  constexpr int dim = 2;

  constexpr int uIdx  = 0;
  constexpr int pIdx  = 1;

  constexpr double rho    = 1.0;
  constexpr double lambda = 0.0;
  constexpr double mu     = 0.01;   // ----> Reynold number Re = 100

  using Grid = Dune::UGGrid<dim>;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,Grid::LeafGridView> >;
  //using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,Grid::LeafGridView> >;
  using Spaces = vector<H1Space const*,H1Space const*>;
  using VariableDescriptions = vector<Variable<SpaceIndex<1>,Components<2>,VariableId<uIdx> >,
                                      Variable<SpaceIndex<0>,Components<1>,VariableId<pIdx> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<>::type;
  using Functional = NavierStokesFunctional<double,VariableSet>;

  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  constexpr int neq   = Functional::TestVars::noOfVariables;

  std::cout << std::endl;
  std::cout << "density rho     = "  << rho    << std::endl;
  std::cout << "viscosity mu    = "  << mu     << std::endl;

  double dt, dtMax, tStart, tEnd, rTolT, aTolT, rTolX, aTolX, writeInterval;

  tStart = 0.0;

  // command line parameters
  int verbosity = getParameter(pt, "verbosity", 1);
  int extrapolOrder = getParameter(pt, "extrapolOrder", 0);
  int maxTimeSteps  = getParameter(pt, "maxTimeSteps", 51);

  int refinements = getParameter(pt, "refinements", 6);
  int order = getParameter(pt, "order", 2); // order for velocity space, order for pressure space is (order-1)
  std::string empty;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  bool direct = getParameter(pt, s, 0);

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  DirectType directType = static_cast<DirectType>(getParameter(pt, s, 4));  // 4: DirectType::UMFPACK3264

  std::cout << std::endl;
  std::cout << "order of extrapolation in Limex : " << extrapolOrder << std::endl;
  std::cout << "original mesh shall be refined  : " << refinements << " times" << std::endl;
  std::cout << "discretization order in space   : " << order << std::endl;
  //std::cout << "direct solver                   : " << directType << std::endl;
  std::cout << std::endl;

  boost::timer::cpu_timer gridTimer;
  // grid generation
  Dune::FieldVector<double,dim> x0(0.0), length(1.0);
  //GridManager<Grid> gridManager( createRectangle<Grid>(x0,length,0.5));   // mesh 2
  GridManager<Grid> gridManager( createRectangle<Grid>(x0,length,1.0));     // mesh 1
  gridManager.globalRefine(refinements);

  std::cout << "Initial grid: trs=" << gridManager.grid().size(0)
            << " eds=" << gridManager.grid().size(1) << " pts=" << gridManager.grid().size(2)
            << std::endl << std::endl;
  //std::cout << "computing time for generation of initial mesh: " << boost::timer::format(gridTimer.elapsed());

  // construct involved spaces.

  H1Space pressureSpace(gridManager,gridManager.grid().leafGridView(),order-1);
  H1Space velocitySpace(gridManager,gridManager.grid().leafGridView(),order);

  Spaces spaces(&pressureSpace,&velocitySpace);

  // construct variable list.
  // concrete 0: u = (u_1,u_2), velocity
  //          1: p              pressure
  std::string varNames[2] = { "u", "p" };

  VariableSet variableSet(spaces,varNames);

  // construct variational functional.
  Functional F(tStart,rho,lambda,mu);

  VariableSet::VariableSet x(variableSet);
  VariableSet::VariableSet dx(variableSet);

  F.time(tStart);
  x = 0;
  F.scaleInitialValue<uIdx>(Initial_2Value(uIdx),x);      // u
  F.scaleInitialValue<pIdx>(Initial_1Value(pIdx),x);      // p

  std::cout << "nvars = " << nvars << std::endl;
  std::cout << "  neq = " << neq   << std::endl;

  tStart = 0.0;
  tEnd   = 800.0;
  dt     = 2e-2;
  dtMax  = 2e-2;

  std::cout << "start time = "   << tStart       << std::endl;
  std::cout << "final time = "   << tEnd         << std::endl;
  std::cout << "step size = "    << dt           << std::endl;
  std::cout << "maxTimeSteps = " << maxTimeSteps << std::endl;
  std::cout << std::endl;

  rTolT = 1.0e+30;		//2e-5;		//1e-5/8.0;
  aTolT = 1.0e+30;		//2e-5;		//1e-5/8.0;
  rTolX = 1.0e+12;		//1e-4;   1e-5*2.0;
  aTolX = 1.0e+12;		//1e-4;   1e-5*2.0;

  std::cout << "aTolT = " << aTolT << std::endl;
  std::cout << "rTolT = " << rTolT << std::endl;
  std::cout << "aTolX = " << aTolX << std::endl;
  std::cout << "rTolX = " << rTolX << std::endl;
  std::cout << std::endl;

  std::vector<VariableSet::VariableSet> solutions;

  x = integrate(gridManager,F,variableSet,spaces,
                dt,dtMax,tEnd,maxTimeSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
                std::back_inserter(solutions),writeInterval,x,DirectType::MUMPS,
                verbosity);

  //writeVTKFile(gridManager.grid().leafGridView(),x,"navierStokes", IoOptions(), order);
  //std::cout << "graphical output finished, data in VTK format is written into file stokes.vtu \n";

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End Navier-Stokes tutorial program" << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_linestring_templated
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED

#include <iostream>
#include <deque>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>

// Adapt any deque to Boost.Geometry Linestring Concept
BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::deque)

int main()
{
    std::deque<boost::geometry::model::d2::point_xy<double> > line(2);
    boost::geometry::assign_values(line[0], 1, 1);
    boost::geometry::assign_values(line[1], 2, 2);

    // Boost.Geometry algorithms work on any deque now
    std::cout << "Length: "  << boost::geometry::length(line) << std::endl;
    std::cout << "Line: "  << boost::geometry::dsv(line) << std::endl;

    return 0;
}

//]

//[register_linestring_templated_output
/*`
Output:
[pre
Length: 1.41421
Line: ((1, 1), (2, 2))
]
*/
//]


/*
 * parameters.hpp
 *
 *  Created on: Apr 23, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

#include "../math/typedefs.hpp"
#include "../math/container_traits.hpp"
#include "interpolation_method.hpp"

namespace tsdf{
template<typename ScalarContainer>
struct Parameters {
	typedef typename math::ContainerWrapper<ScalarContainer>::Coordinates Coordinates;
	typedef typename ScalarContainer::Scalar Scalar;
	typedef Eigen::Matrix<Scalar, 3, 3, Eigen::ColMajor> Mat3;
	Parameters(Scalar depth_unit_ratio = (Scalar)0.001,
			Mat3 projection_matrix = Mat3::Identity(),
			Scalar near_clipping_distance = (Scalar)0.05,
			Coordinates array_offset = Coordinates(-64),
			Coordinates field_shape = Coordinates(128),
			Scalar voxel_size = (Scalar)0.004,
			int narrow_band_width_voxels = 20,
			FilteringMethod interpolation_method = FilteringMethod::NONE, //TODO: refactor to filtering_method
			Scalar smoothing_factor = (Scalar)1.0
			);
	Scalar depth_unit_ratio = (Scalar)0.001; //meters
	Mat3 projection_matrix;
	Scalar near_clipping_distance = 0.05; //meters
	Coordinates array_offset = Coordinates(-64); //voxels
	Coordinates field_shape = Coordinates(128); //voxels
	Scalar voxel_size = 0.004; //meters
	int narrow_band_width_voxels = 20; //voxels
	FilteringMethod interpolation_method = FilteringMethod::NONE;
	Scalar smoothing_factor = (Scalar)1.0; // gaussian covariance scale for EWA
};

}  // namespace tsdf


#ifndef PYTHONIC_INCLUDE_NUMPY_RECIPROCAL_HPP
#define PYTHONIC_INCLUDE_NUMPY_RECIPROCAL_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/rec.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME reciprocal
#define NUMPY_NARY_FUNC_SYM boost::simd::rec
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


/*
 * File: DescStat.hpp
 * Part of commonpp.
 *
 * Distributed under the 2-clause BSD licence (See LICENCE.TXT file at the
 * project root).
 *
 * Copyright (c) 2015 Thomas Sanchez.  All rights reserved.
 *
 */
#pragma once

#include <mutex>
#include <string>
#include <utility>
#include <vector>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_tail_quantile.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <boost/noncopyable.hpp>

#include <commonpp/metric/MetricValue.hpp>
#include <commonpp/metric/detail/types.hpp>
#include <commonpp/metric/reservoir/types.hpp>

namespace commonpp
{
namespace metric
{
class Metrics;

namespace type
{

// Descriptive statistic.
// It will calculate several features in order to summarize a reservoir.
class DescStat : private boost::noncopyable
{
    using Stats = boost::accumulators::stats<
        boost::accumulators::tag::mean,
        boost::accumulators::tag::min,
        boost::accumulators::tag::max,
        boost::accumulators::tag::count,
        boost::accumulators::tag::variance(boost::accumulators::lazy),
        boost::accumulators::tag::tail_quantile<boost::accumulators::right>>;

    using Accumulator = boost::accumulators::accumulator_set<double, Stats>;

    using WStats = boost::accumulators::stats<
        boost::accumulators::tag::weighted_tail_quantile<boost::accumulators::right>,
        boost::accumulators::tag::weighted_mean,
        boost::accumulators::tag::min,
        boost::accumulators::tag::max,
        boost::accumulators::tag::count,
        boost::accumulators::tag::weighted_variance(boost::accumulators::lazy)>;

    using WAccumulator =
        boost::accumulators::accumulator_set<double, WStats, double>;

public:
    template <typename Reservoir>
    static MetricValue getMetrics(const Reservoir& reservoir)
    {
        return _getMetrics(reservoir, Reservoir::tag);
    }

    template <typename Reservoir>
    static MetricValue _getMetrics(const Reservoir& reservoir,
                                   reservoir::SimpleReservoirTag)
    {
        namespace bacc = boost::accumulators;

        MetricValue result;
        using bacc::count;
        using bacc::max;
        using bacc::mean;
        using bacc::min;
        using bacc::tail_quantile;
        using bacc::variance;

        auto acc = reservoir.template visit<DescStat>();

        if (count(acc) == 0)
        {
            return result;
        }

        result.push(mean(acc), "mean");
        result.push(min(acc), "min");
        result.push(max(acc), "max");
        {
            auto var = variance(acc);
            if (var >= 0)
            {
                result.push(::sqrt(var), "stdev");
            }
        }

        result.push(tail_quantile(acc, bacc::quantile_probability = 0.50),
                    "median");
        result.push(tail_quantile(acc, bacc::quantile_probability = 0.75),
                    "p75");
        result.push(tail_quantile(acc, bacc::quantile_probability = 0.95),
                    "p95");
        result.push(tail_quantile(acc, bacc::quantile_probability = 0.99),
                    "p99");

        return result;
    }

    template <typename Reservoir>
    static MetricValue _getMetrics(const Reservoir& reservoir,
                                   reservoir::WeightedReservoirTag)
    {
        namespace bacc = boost::accumulators;

        MetricValue result;
        using bacc::count;
        using bacc::max;
        using bacc::min;
        using bacc::weighted_mean;
        using bacc::weighted_tail_quantile;
        using bacc::weighted_variance;

        WAccumulator acc = reservoir.template visit<DescStat>();

        if (count(acc) == 0)
        {
            return result;
        }

        result.push(weighted_mean(acc), "mean");
        result.push(min(acc), "min");
        result.push(max(acc), "max");
        {
            auto var = weighted_variance(acc);
            if (var >= 0)
            {
                result.push(::sqrt(var), "stdev");
            }
        }
        result.push(weighted_tail_quantile(acc, bacc::quantile_probability = 0.50),
                    "median");
        result.push(weighted_tail_quantile(acc, bacc::quantile_probability = 0.75),
                    "p75");
        result.push(weighted_tail_quantile(acc, bacc::quantile_probability = 0.95),
                    "p95");
        result.push(weighted_tail_quantile(acc, bacc::quantile_probability = 0.99),
                    "p99");

        return result;
    }

    struct WeightedProxy
    {
        WeightedProxy(size_t nb_values)
        : acc(boost::accumulators::right_tail_cache_size = nb_values)
        {
        }

        WeightedProxy& operator()(double w, double v)
        {
            acc(v, boost::accumulators::weight = w);
            return *this;
        }

        operator WAccumulator&()
        {
            return acc;
        }

        operator const WAccumulator&() const
        {
            return acc;
        }

        WAccumulator acc;
    };

    static WeightedProxy createAccumulator(size_t nb_values,
                                           reservoir::WeightedReservoirTag)
    {
        return WeightedProxy(nb_values);
    }

    static Accumulator createAccumulator(size_t nb_values,
                                         reservoir::SimpleReservoirTag)
    {
        return Accumulator(
            boost::accumulators::tag::tail<boost::accumulators::right>::cache_size =
                nb_values);
    }
};
} // namespace type
} // namespace metric
} // namespace commonpp


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp>

using namespace std;
using namespace boost::numeric::ublas;
using namespace boost::random;

int main () {
    boost::mt19937 rng;
    boost::uniform_int<> binary(0, 1);

    matrix<int> m1(10, 10), m2(3, 3);

    int h1 = m1.size1(), w1 = m1.size2();
    int h2 = m2.size1(), w2 = m2.size2();
    int h  = h1 * h2,    w  = w1 * w2;

    for (unsigned i = 0; i < h1; ++ i)
        for (unsigned j = 0; j < w1; ++ j)
            m1(i, j) = binary(rng);

    for (unsigned i = 0; i < h2; ++ i)
        for (unsigned j = 0; j < w2; ++ j)
            m2(i, j) = binary(rng);

    cout << m1 << endl;
    cout << m2 << endl;

    for (unsigned i = 0; i < h; ++ i ) {
        for (unsigned j = 0; j < w; ++ j)
            cout << m1(i/h2, j/w2) * m2(i%h2, j%w2) << ' ';
        cout << endl;
    }

    return 0;
}


// Copyright (c) 2018 Steven Watanabe
//
// Distributed under the Boost Software License Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS2_DIMENSIONS_HPP_INCLUDED
#define BOOST_UNITS2_DIMENSIONS_HPP_INCLUDED

#include <boost/units2/unit.hpp>
#include <boost/units2/def.hpp>
#include <boost/units2/dimensions.hpp>
#include <ratio>

namespace boost {
namespace units2 {
namespace si {

BOOST_UNITS2_DEF(meter,length);
BOOST_UNITS2_DEF(gram,mass);
BOOST_UNITS2_DEF(second,time);
BOOST_UNITS2_DEF(kelvin,temperature);
BOOST_UNITS2_DEF(mole,amount);
BOOST_UNITS2_DEF(ampere,current);
BOOST_UNITS2_DEF(candela,luminous_intensity);

// Extra units that are techically dimensionless
BOOST_UNITS2_DEF(radian,angle);
BOOST_UNITS2_DEF(steradian,solid_angle);

// kilograms are actually the base unit, but for the sake
// of naming consistency, we're defining it this way, since
// it doesn't change the behavior significantly.
inline constexpr const auto kilogram = std::kilo() * gram;

inline constexpr const auto hertz = pow<-1>(second);
inline constexpr const auto newton = meter*kilogram/pow<2>(second);
inline constexpr const auto pascal = newton/pow<2>(meter);
inline constexpr const auto joule = newton*meter;
inline constexpr const auto watt = joule/second;
inline constexpr const auto couloumb = second * ampere;
inline constexpr const auto volt = watt/ampere;
inline constexpr const auto farad = couloumb/volt;
inline constexpr const auto ohm = volt/ampere;
inline constexpr const auto siemens = ampere/volt;
inline constexpr const auto weber = volt*second;
inline constexpr const auto tesla = weber/pow<2>(meter);
inline constexpr const auto henry = weber/ampere;
// celsius = kelvin + std::ratio<27315,100>
inline constexpr const auto lumen = candela*steradian;
inline constexpr const auto lux = lumen/pow<2>(meter);
inline constexpr const auto becquerel = pow<-1>(second);
inline constexpr const auto gray = joule/kilogram;
inline constexpr const auto sievert = joule/kilogram;
inline constexpr const auto katal = mole/second;

}
}
}

#endif


/* Copyright (c) 2016 - 2020, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef ELECTRON_BEAM_HH
#define ELECTRON_BEAM_HH

#include <deal.II/base/function_parser.h>

#include <boost/property_tree/ptree.hpp>

namespace adamantine
{
/**
 * This structure stores all the physical properties necessary to define an
 * electron beam.
 */
struct BeamProperty
{
public:
  /**
   * Absolute penetration of the electron beam into the material where 99% of
   * the beam energy is absorbed.
   */
  double depth;
  /**
   * Energy conversion efficiency on the surface.
   */
  double energy_conversion_eff;
  /**
   * Efficiency of beam control.
   */
  double control_eff;
  /**
   * Square of the beam diameter.
   */
  double diameter_squared;
  /**
   * Maximum power of the beam.
   */
  double max_power;
};

/**
 * This class describes the evolution of an electron beam source.
 */
template <int dim>
class ElectronBeam : public dealii::Function<dim>
{
public:
  /**
   * Constructor.
   * \param[in] database requires the following entries:
   *   - energy_conversion_efficiency: double in \f$[0,1]\f$
   *   - control_efficiency: double in \f$[0,1]\f$
   *   - depth: double in \f$[0,\infty)\f$
   *   - diameter: double in \f$[0,\infty)\f$
   *   - max_power: double in \f$[0, \infty)\f$ [optional: if not
   *   defined, current and voltage need to be defined]
   *   - current: double in \f$[0, \infty)\f$ [optional: if defined
   *   voltage should be defined too, if not defined max_power
   *   should be defined]
   *   - voltage: double in \f$[0,\infty)\f$ [optional: if defined
   *   current should be defined too, if not defined max_power
   *   should be defined]
   *   - input_file: name of the csv file that contains the successive
   *   position of the electron beam [optional: if not defined then
   *   abscissa and, in 3D, ordinate need to be defined]
   *   - delimiter: delimiting character used in input_file
   *   [required if input_file is defined]
   *   - abscissa: string, abscissa of the beam as a function of time
   *   (e.g. "(t-1) * (t-2)") [optional: need to be defined if input_file
   *   is not defined]
   *   - ordinate: string, ordinate of the beam as a function of time
   *   [required only for three dimensional calculation and if input_file
   *   is not defined]
   */
  ElectronBeam(boost::property_tree::ptree const &database);

  /**
   * Set the maximum height of the domain. This is the height at which the
   * electron beam penetrate the material.
   */
  void set_max_height(double height);

  /**
   * Compute the heat source at a given point at the current time.
   */
  double value(dealii::Point<dim> const &point,
               unsigned int const component = 0) const override;

  /**
   * Reset the current time and the position to the last saved state.
   */
  void rewind_time();

  /**
   * Save the current time and the position in the list of successive positions
   * of the beam.
   */
  void save_time();

private:
  /**
   * Flag is true if the beam is a point source of which the position is read
   * from a
   * file.
   */
  bool _is_point_source;

  /**
   * Height of the domain.
   */
  double _max_height;
  /**
   * Structure of the physical properties of the electron beam.
   */
  BeamProperty _beam;
  /**
   * Function that describes the position of the beam on the surface.
   */
  std::array<std::unique_ptr<dealii::Function<1>>, dim - 1> _position;
};

template <int dim>
inline void ElectronBeam<dim>::set_max_height(double height)
{
  _max_height = height;
}
} // namespace adamantine

#endif


#define _WEIGHTED_MAT_

#include <armadillo>

using arma::cx_mat;

struct weightedMat
{
  cx_mat lieMat;
  double cost;
};


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"
#include <Eigen/IterativeLinearSolvers>

template<typename T> void test_bicgstab_T()
{
  BiCGSTAB<SparseMatrix<T>, DiagonalPreconditioner<T> > bicgstab_colmajor_diag;
  BiCGSTAB<SparseMatrix<T>, IdentityPreconditioner    > bicgstab_colmajor_I;
  BiCGSTAB<SparseMatrix<T>, IncompleteLUT<T> >           bicgstab_colmajor_ilut;
  //BiCGSTAB<SparseMatrix<T>, SSORPreconditioner<T> >     bicgstab_colmajor_ssor;

  CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_diag)  );
//   CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_I)     );
  CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_ilut)     );
  //CALL_SUBTEST( check_sparse_square_solving(bicgstab_colmajor_ssor)     );
}

void test_bicgstab()
{
  CALL_SUBTEST_1(test_bicgstab_T<double>());
  CALL_SUBTEST_2(test_bicgstab_T<std::complex<double> >());
}