//
// Created by Samuel Jackson on 08/03/2016.
//

#include <memory>
#include <iostream>
#include <Eigen/Dense>

#include "Molecule.h"
#include "SystemParameters.h"
#include "Simulation.h"

using namespace Eigen;
using namespace Molly;

Simulation::Simulation(SystemParameters params)
        : params(params), step_limit(10), step_count(0), vir_sum(0), u_sum(0), cell_matrix(params.r_cutoff) {

}

void Simulation::set_up() {
    print_header();
    init_props();
    init_cells();
    init_coordinates();
}

void Simulation::init_props() {
    // create unit cell of volume x * y * z
    unit_cell << 10, 10, 1;
    // scale the unit cell size by density to create the bound box
    region = unit_cell * (1. / sqrt(params.density));
    // one cell for each unit of volume
    num_mols = unit_cell.prod();
    mols.resize(num_mols);

    initial_magnitude = std::sqrt(3 * (1. - 1./num_mols) * params.temperature);
}

void Simulation::init_cells() {
    // create cells proportional to cell cut off distance.
    cell_size = region / params.r_cutoff ;
    cell_size(0) = std::ceil(cell_size(0));
    cell_size(1) = std::ceil(cell_size(1));
    cell_size(2) = std::ceil(cell_size(2));
    cell_matrix.resize(cell_size(0), cell_size(1), cell_size(2));
}

void Simulation::init_coordinates() {
    Vector3d gap = region.cwiseQuotient(unit_cell);
    std::vector<Molecule_ptr> mols;
    mols.resize(num_mols);

    int n = 0;
    for (int nx = 0; nx < unit_cell(0); ++nx) {
        for (int ny = 0; ny < unit_cell(1); ++ny) {
            for(int nz = 0; nz < unit_cell(2); ++nz) {
                // set up position
                mols[n] = std::make_shared<Molecule>();
                mols[n]->r(0) = nx+0.5;
                mols[n]->r(1) = ny+0.5;
                mols[n]->r(2) = nz+0.5;

                mols[n]->r = mols[n]->r.cwiseProduct(gap);
                mols[n]->r += -0.5 * region;

                // set up initial velocity
                mols[n]->rv = Vector3d::Random();
                mols[n]->rv.normalize();
                mols[n]->rv *= initial_magnitude;
                ++n;
            }
        }
    }
}

void Simulation::run() {
    int n = 0;
    while(n < step_limit) {
        single_step(n);
        ++n;
    }

}

void Simulation::single_step(int step) {
    step_count = step;
    time_now = step_count * params.delta_time;

    integrate(1);
    apply_boundary_checks();
    compute_forces();
    integrate(2);
    evaluate_properties();

    if (step_count % params.step_average == 0) {
        average_properties();
        print_summary();
        clear_properties();
    }
}

void Simulation::compute_forces() {
    Vector3d dr = Vector3d::Zero();
    double rr = 0.;
    double cut_off = params.r_cutoff * params.r_cutoff;

    // reset sums for this iteration
    u_sum = 0.;
    vir_sum = 0.;

    // reset acceleration vectors
    for(auto iter = mols.begin(); iter != mols.end(); ++iter) {
        iter->ra.fill(0);
    }

    // compute interactions between molecules
    for (int i=0; i < mols.size()-1; ++i) {
        for(int j=i+1; j < mols.size(); ++j) {
            // find difference between mols i and j
            dr = mols[i].r - mols[j].r;
            // apply boundary checks to position difference
            apply_boundary_check(dr);

            rr = dr.squaredNorm();

            if(rr < cut_off) {
                dr*= potential(rr);
                mols[i].ra += dr;
                mols[j].ra -= dr;
            }
        }
    }
}

/**
 * Lennard Jones potential function
 */
double Simulation::potential(double rr) {
    double rri = 1./rr;
    double rri3 = rri * rri * rri;
    double fc_val = 48. * rri3 * (rri3 - 0.5) * rri;
    u_sum += 4. * rri3 * (rri3 - 1.) + 1.;
    vir_sum += fc_val * rr;
    return fc_val;
}

/**
 * Uses leapfrog integration method.
 *
 */
void Simulation::integrate(int part) {

    for (auto iter = mols.begin(); iter != mols.end(); ++iter) {
        Molecule mol = *iter;
        switch(part) {
            case 1:
                // update velocity to by half a time step.
                // update position one whole step.
                mol.rv += 0.5 * params.delta_time * mol.ra;
                mol.r += params.delta_time * mol.rv;
                *iter = mol;
                break;
            case 2:
                // just update velocity by half a time step.
                mol.rv += 0.5 * params.delta_time * mol.ra;
                *iter = mol;
                break;
            default:
                throw std::runtime_error("Invalid part operation for integrate");
        }
    }
}

void Simulation::apply_boundary_checks() {
    for(auto iter = mols.begin(); iter != mols.end(); ++iter) {
        apply_boundary_check(iter->r);
    }
}

void Simulation::apply_boundary_check(Vector3d& vector) {
    update_bound(vector(0), region(0));
    update_bound(vector(1), region(1));
    update_bound(vector(2), region(2));
}

void Simulation::update_bound(double& value, const double& bound) {
    value = (value >= 0.5 * bound) ? value - bound : value;
    value = (value < -0.5 * bound) ? value + bound : value;
}

void Simulation::evaluate_properties() {
    v_sum = Vector3d::Zero();
    double v_sqrd_sum = 0.;

    for(auto iter = mols.cbegin(); iter != mols.cend(); ++iter) {
        v_sum += iter->rv;
        v_sqrd_sum += iter->rv.squaredNorm();
    }

    double kin_energy = 0.5 * v_sqrd_sum / num_mols;
    kinetic_energy.set_value(kin_energy);
    total_energy.set_value(kin_energy + (u_sum / num_mols));
    pressure.set_value(params.density * (v_sqrd_sum + vir_sum) / (num_mols / 3));
}

void Simulation::average_properties() {
    kinetic_energy.average(params.step_average);
    total_energy.average(params.step_average);
    pressure.average(params.step_average);
}

void Simulation::print_summary() {
    printf("%5d %8.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f\n",
           step_count,
           time_now,
           v_sum.sum() / num_mols,
           total_energy.get_sum(),
           total_energy.get_sum_sqrd(),
           kinetic_energy.get_sum(),
           kinetic_energy.get_sum_sqrd(),
           pressure.get_sum(),
           pressure.get_sum_sqrd());
}

void Simulation::clear_properties() {
    kinetic_energy.clear();
    total_energy.clear();
    pressure.clear();
}

void Simulation::print_header() {
    printf("MOLLY\n--------------------------------------------------------------------------------------------\n");
    printf("%5s %8s %7s %7s %7s %7s %7s %7s %7s\n",
           "step", "time", "sum", "tot_sum", "tot_sq", "kin_sum", "kin_sq", "press_sum", "press_sq");
}


/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2016, Guan-Horng Liu.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * 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.
*   * Neither the name of the the copyright holder nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  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.
*
* Author:  Guan-Horng Liu
*********************************************************************/

#include "r_s_planner/reeds_shepp.h"
#include <boost/math/constants/constants.hpp>

namespace
{
    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.

    const double pi = boost::math::constants::pi<double>();
    const double twopi = 2. * pi;
    const double RS_EPS = 1e-6;
    const double ZERO = 10*std::numeric_limits<double>::epsilon();

    inline double mod2pi(double x)
    {
        double v = fmod(x, twopi);
        if (v < -pi)
            v += twopi;
        else
            if (v > pi)
                v -= twopi;
        return v;
    }
    inline void polar(double x, double y, double &r, double &theta)
    {
        r = sqrt(x*x + y*y);
        theta = atan2(y, x);
    }
    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
    {
        double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
        double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
        tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
        omega = mod2pi(tau - u + v - phi) ;
    }

    // formula 8.1 in Reeds-Shepp paper
    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
    {
        polar(x - sin(phi), y - 1. + cos(phi), u, t);
        if (t >= -ZERO)
        {
            v = mod2pi(phi - t);
            if (v >= -ZERO)
            {
                assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
                assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
                assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
                return true;
            }
        }
        return false;
    }
    // formula 8.2
    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double t1, u1;
        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
        u1 = u1*u1;
        if (u1 >= 4.)
        {
            double theta;
            u = sqrt(u1 - 4.);
            theta = atan2(2., u);
            t = mod2pi(t1 + theta);
            v = mod2pi(t - phi);
            assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
            assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
            return t>=-ZERO && v>=-ZERO;
        }
        return false;
    }
    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
    }
    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
        polar(xi, eta, u1, theta);
        if (u1 <= 4.)
        {
            u = -2.*asin(.25 * u1);
            t = mod2pi(theta + .5 * u + pi);
            v = mod2pi(phi - t + u);
            assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO;
        }
        return false;
    }
    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
            Lmin = L;
        }
        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
            Lmin = L;
        }
        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
    }
    // formula 8.7
    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
        if (rho <= 1.)
        {
            u = acos(rho);
            tauOmega(u, -u, xi, eta, phi, t, v);
            assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
            return t>=-ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.8
    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
        if (rho>=0 && rho<=1)
        {
            u = -acos(rho);
            if (u >= -.5 * pi)
            {
                tauOmega(u, u, xi, eta, phi, t, v);
                assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
            Lmin = L;
        }
        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
            Lmin = L;
        }

        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
            Lmin = L;
        }
        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
    }
    // formula 8.9
    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            double r = sqrt(rho*rho - 4.);
            u = 2. - r;
            t = mod2pi(theta + atan2(r, -2.));
            v = mod2pi(phi - .5*pi - t);
            assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
            assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.10
    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(-eta, xi, rho, theta);
        if (rho >= 2.)
        {
            t = theta;
            u = 2. - rho;
            v = mod2pi(t + .5*pi - phi);
            assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
            assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - .5*pi, L;
        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
            Lmin = L;
        }

        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
    }
    // formula 8.11 *** TYPO IN PAPER ***
    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            u = 4. - sqrt(rho*rho - 4.);
            if (u <= ZERO)
            {
                t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
                v = mod2pi(t - phi);
                assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - pi, L;
        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
            Lmin = L;
        }
        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
    }

    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
    {
        ReedsSheppStateSpace::ReedsSheppPath path;
        CSC(x, y, phi, path);
        CCC(x, y, phi, path);
        CCCC(x, y, phi, path);
        CCSC(x, y, phi, path);
        CCSCC(x, y, phi, path);
        return path;
    }
}

const ReedsSheppStateSpace::ReedsSheppPathSegmentType
ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
};

ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
    double t, double u, double v, double w, double x)
    : type_(type)
{
    length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
}

double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
{
    return rho_ * reedsShepp(q0, q1).length();
}

ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
{
    double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
    double c = cos(q0[2]), s = sin(q0[2]);
    double x = c*dx + s*dy, y = -s*dx + c*dy;
    return ::reedsShepp(x/rho_, y/rho_, dth);
}

std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> ReedsSheppStateSpace::type(double q0[3], double q1[3])
{
    ReedsSheppPath path = reedsShepp(q0, q1);
    std::vector<ReedsSheppStateSpace::ReedsSheppPathSegmentType> type_list;
    for (int i=0;i<5;++i)
        type_list.push_back(path.type_[i]);
    return type_list;
}

void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points)
{
    ReedsSheppPath path = reedsShepp(q0, q1);
    length = rho_ * path.length();

    for (double seg=0.0; seg<=length; seg+=step_size) {
        double qnew[3] = {};
        interpolate(q0, path, seg/rho_, qnew);
        std::vector<double> v(qnew, qnew + sizeof qnew / sizeof qnew[0]);
        points.push_back(v);
    }
    return;
}

void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[3])
{

    if (seg < 0.0) seg = 0.0;
    if (seg > path.length()) seg = path.length();

    double phi, v;

    s[0] = s[1] = 0.0;
    s[2] = q0[2];

    for (unsigned int i=0; i<5 && seg>0; ++i)
    {
        if (path.length_[i]<0)
        {
            v = std::max(-seg, path.length_[i]);
            seg += v;
        }
        else
        {
            v = std::min(seg, path.length_[i]);
            seg -= v;
        }
        phi = s[2];
        switch(path.type_[i])
        {
            case RS_LEFT:
                s[0] += ( sin(phi+v) - sin(phi));
                s[1] += (-cos(phi+v) + cos(phi));
                s[2] = phi + v;
                break;
            case RS_RIGHT:
                s[0] += (-sin(phi-v) + sin(phi));
                s[1] += ( cos(phi-v) - cos(phi));
                s[2] = phi - v;
                break;
            case RS_STRAIGHT:
                s[0] += (v * cos(phi));
                s[1] += (v * sin(phi));
                break;
            case RS_NOP:
                break;
        }
    }

    s[0] = s[0] * rho_ + q0[0];
    s[1] = s[1] * rho_ + q0[1];
}


#include <boost/mpl11/integer.hpp>

using size_t = decltype(sizeof(int));

constexpr struct plus_ {
    template <typename X, typename Y>
    constexpr auto operator()(X x, Y y) { return x + y; }
} plus{};

template <typename T, size_t N, typename F, typename State>
constexpr State homogeneous_foldl(F f, State s, const T (&array)[N]) {
    for (size_t i = 0; i < N; ++i)
        s = f(s, array[i]);
    return s;
}

template <typename ...xs>
using sum = boost::mpl11::integer_c<
    decltype(homogeneous_foldl<size_t, sizeof...(xs)>(plus, 0, {xs::value...})),
    homogeneous_foldl<size_t, sizeof...(xs)>(plus, 0, {xs::value...})
>;

<%= render('_main.erb') %>


/* Boost test/det.cpp
 * test protected and unprotected rounding on an unstable determinant
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <boost/test/minimal.hpp>
#include "bugs.hpp"

#define size 8

template<class I>
void det(I (&mat)[size][size]) {
  for(int i = 0; i < size; i++)
    for(int j = 0; j < size; j++)
      mat[i][j] = I(1) / I(i + j + 1);

  for(int i = 0; i < size - 1; i++) {
    int m = i, n = i;
    typename I::base_type v = 0;
    for(int a = i; a < size; a++)
      for(int b = i; b < size; b++) {
        typename I::base_type  w = abs(mat[a][b]).lower();
        if (w > v) { m = a; n = b; v = w; }
      }
    if (n != i)
      for(int a = 0; a < size; a++) {
        I t = mat[a][n];
        mat[a][n] = mat[a][i];
        mat[a][i] = t;
      }
    if (m != i)
      for(int b = i; b < size; b++) {
        I t = mat[m][b];
        mat[m][b] = mat[m][i];
        mat[m][i] = t;
      }
    if (((m + n) & 1) == 1) { };
    I c = mat[i][i];
    for(int j = i + 1; j < size; j++) {
      I f = mat[j][i] / c;
      for(int k = i; k < size; k++)
        mat[j][k] -= f * mat[i][k];
    }
    if (in_zero(c)) return;
  }
}

namespace my_namespace {

using namespace boost;
using namespace numeric;
using namespace interval_lib;

template<class T>
struct variants {
  typedef interval<T> I_op;
  typedef typename change_rounding<I_op, save_state<rounded_arith_std<T> > >::type I_sp;
  typedef typename unprotect<I_op>::type I_ou;
  typedef typename unprotect<I_sp>::type I_su;
  typedef T type;
};

}

template<class T>
bool test() {
  typedef my_namespace::variants<double> types;
  types::I_op mat_op[size][size];
  types::I_sp mat_sp[size][size];
  types::I_ou mat_ou[size][size];
  types::I_su mat_su[size][size];
  det(mat_op);
  det(mat_sp);
  { types::I_op::traits_type::rounding rnd; det(mat_ou); }
  { types::I_sp::traits_type::rounding rnd; det(mat_su); }
  for(int i = 0; i < size; i++)
    for(int j = 0; j < size; j++) {
      typedef types::I_op I;
      I d_op = mat_op[i][j];
      I d_sp = mat_sp[i][j];
      I d_ou = mat_ou[i][j];
      I d_su = mat_su[i][j];
      if (!(equal(d_op, d_sp) && equal(d_sp, d_ou) && equal(d_ou, d_su)))
        return false;
    }
  return true;
}

int test_main(int, char *[]) {
  BOOST_CHECK(test<float>());
  BOOST_CHECK(test<double>());
  BOOST_CHECK(test<long double>());
# ifdef __BORLANDC__
  ::detail::ignore_warnings();
# endif
  return 0;
}


#include "solvers/SolverBoxPGS.h"

#include "contact/Contact.h"
#include "rigidbody/RigidBody.h"
#include "rigidbody/RigidBodySystem.h"

#include <Eigen/Dense>

namespace
{
    static inline void multAndSub(const JBlock& G, const Eigen::Vector3f& x, const Eigen::Vector3f& y, const float a, Eigen::VectorXf& b)
    {
            b -= a * G.col(0) * x(0);
            b -= a * G.col(1) * x(1);
            b -= a * G.col(2) * x(2);
            b -= a * G.col(3) * y(0);
            b -= a * G.col(4) * y(1);
            b -= a * G.col(5) * y(2);
    }

	// Computes the right-hand side vector of the Schur complement system:
    //      b = gamma*phi/h - J*vel - dt*JMinvJT*force
    //
    static inline void buildRHS(Contact* c, float h, Eigen::VectorXf& b)
    {
        const float gamma = h * c->k / (h * c->k + c->b);       // error reduction parameter
        b = -gamma * c->phi / h;

        multAndSub(c->J0, c->body0->xdot, c->body0->omega, 1.0f, b);
        multAndSub(c->J1, c->body1->xdot, c->body1->omega, 1.0f, b);

        if( !c->body0->fixed )
        {
            multAndSub(c->J0Minv, c->body0->f, c->body0->tau, h, b);
        }
        if( !c->body1->fixed )
        {
            multAndSub(c->J1Minv, c->body1->f, c->body1->tau, h, b);
        }
    }

    // Loop over all other contacts for a body and compute modifications to the rhs vector b:
    //           x -= (JMinv*Jother^T) * lambda_other
    //
    static inline void accumulateCoupledContacts(Contact* c, const JBlock& JMinv, RigidBody* body, Eigen::VectorXf& b)
    {
        if( body->fixed )
            return;

        for(Contact* cc : body->contacts)
        {
            if( cc != c )
            {
                if( body == cc->body0 )
                    b -= JMinv * (cc->J0.transpose() * cc->lambda);
                else
                    b -= JMinv * (cc->J1.transpose() * cc->lambda);
            }
        }
    }

    // Solve the Boxed LCP problem for a single contact and isotropic Coulomb friction.
    // The solution vector, @a x, contains the impulse the non-interpenetration constraint in x(0), and
    // the friction constraints in x(1) and x(2)
    //
    // The solution is projected to the lower and upper bounds imposed by the box model.
    //
    // Inputs:
    //    x - contains three impulse variables (non-interpenetration + two friction)
    //    b - rhs vector
    //    mu - the friction coefficient
    static inline void solveContact(const Eigen::Matrix3f& A, const Eigen::VectorXf& b, Eigen::VectorXf& x, const float mu)
    {
        // Normal impulse is projected to [0, inf]
        //
        x(0) = std::max(0.0f, (b(0) - A(0,1) * x(1) - A(0,2) * x(2) ) / A(0,0) );

        // Next, friction impulses are projected to [-mu * x(0), mu * x(1)]
        //
        x(1) = std::max(-mu*x(0), std::min(mu*x(0), ( b(1) - A(1,0) * x(0) - A(1,2) * x(2) ) / A(1,1) ));
        x(2) = std::max(-mu*x(0), std::min(mu*x(0), ( b(2) - A(2,0) * x(0) - A(2,1) * x(1) ) / A(2,2) ));
    }
}

SolverBoxPGS::SolverBoxPGS(RigidBodySystem* _rigidBodySystem) : Solver(_rigidBodySystem)
{

}

void SolverBoxPGS::solve(float h)
{
    std::vector<Contact*>& contacts = m_rigidBodySystem->getContacts();
    const int numContacts = contacts.size();

    // Build array of 3x3 diagonal matrices, one for each contact.
    //
    std::vector<Eigen::Matrix3f> Acontactii;
    if( numContacts > 0 )
    {
        // Build diagonal matrices
        Acontactii.resize(numContacts);
        for(int i = 0; i < numContacts; ++i)
        {
            Contact* c = contacts[i];
            const float eps = 1.0f / (h * h * c->k + h * c->b);    // constraint force mixing

            // Compute the diagonal term : Aii = J0*Minv0*J0^T + J1*Minv1*J1^T
            //
            Acontactii[i].setZero(3,3);
            Acontactii[i](0,0) += eps;

            if( !c->body0->fixed )
            {
                Acontactii[i] += c->J0Minv * c->J0.transpose();
            }
            if( !c->body1->fixed )
            {
                Acontactii[i] += c->J1Minv * c->J1.transpose();
            }
        }

        std::vector<Eigen::VectorXf> b;
        b.resize(numContacts);

        // Compute the right-hand side vector :
        //      b = -gamma*phi/h - J*vel - dt*JMinvJT*force
        //
        for(int i = 0; i < numContacts; ++i)
        {
            Contact* c = contacts[i];
            buildRHS(c, h, b[i]);
            c->lambda.setZero();
        }

        // PGS main loop.
        // There is no convergence test here.
        // Stop after @a maxIter iterations.
        //
        for(int iter = 0; iter < m_maxIter; ++iter)
        {
            // For each contact, compute an updated value of contacts[i]->lambda
            //      using matrix-free pseudo-code provided in the course notes.
            //
            for(int i = 0; i < numContacts; ++i)
            {
                Contact* c = contacts[i];

                // Initialize current solution as x = b[i]
                Eigen::VectorXf x = b[i];

                accumulateCoupledContacts(c, c->J0Minv, c->body0, x);
                accumulateCoupledContacts(c, c->J1Minv, c->body1, x);
                solveContact(Acontactii[i], x, c->lambda, c->mu);
            }
        }
    }
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/core.hpp>
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/airy.hpp>

EVE_TEST_TYPES( "Check return types of airy_ai"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::airy_ai(T(0)), T);
  TTS_EXPR_IS(eve::airy_ai(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of airy_ai on wide"
         , eve::test::simd::ieee_reals
         , eve::test::generate(eve::test::randoms(-20.0, 0.0),
                               eve::test::randoms(0.0, 20.0)
                              )
         )
   <typename T>(T a0, T a1)
{
  using v_t = eve::element_type_t<T>;
  v_t abstol = 1000*eve::eps(eve::as<v_t>());
  auto eve__airy_ai =  [](auto x) { return eve::airy_ai(x); };
  auto std__airy_ai =  [](auto x)->v_t { return boost::math::airy_ai(x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__airy_ai(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1500)), std__airy_ai(v_t(1500)), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(500)), std__airy_ai(v_t(500)), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(10)),  std__airy_ai(v_t(10))  , 13.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(5)),  std__airy_ai(v_t(5))   , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(2)),  std__airy_ai(v_t(2))   , 35.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1.5)),std__airy_ai(v_t(1.5)) , 11.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(0.5)),std__airy_ai(v_t(0.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1)),  std__airy_ai(v_t(1))   , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(0)),  v_t(0.355028053887817), 0.5);

  TTS_ULP_EQUAL(eve__airy_ai( T(1500)), T(std__airy_ai(v_t(1500))), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(500)),  T(std__airy_ai(v_t(500)) ), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(10)) ,  T(std__airy_ai( v_t(10)) ), 13.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(5))  ,  T(std__airy_ai( v_t(5))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(2))  ,  T(std__airy_ai( v_t(2))  ), 37.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(1.5)),  T(std__airy_ai( v_t(1.5))), 11.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(0.5)),  T(std__airy_ai( v_t(0.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(1))  ,  T(std__airy_ai( v_t(1))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(0))  ,  T(0.355028053887817), 0.5);

  TTS_ABSOLUTE_EQUAL(eve__airy_ai(v_t(-1500)), std__airy_ai(v_t(-1500)), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai(v_t(-500)), std__airy_ai(v_t(-500)), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai(v_t(-10)),  std__airy_ai(v_t(-10)), abstol);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(-5)),  std__airy_ai(v_t(-5))   , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(-2)),  std__airy_ai(v_t(-2))   , 35.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(-1.5)),std__airy_ai(v_t(-1.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(-0.5)),std__airy_ai(v_t(-0.5)) , 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(-1)),  std__airy_ai(v_t(-1))   , 10.0);

  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-1500)), T(std__airy_ai(v_t(-1500))), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-500)),  T(std__airy_ai(v_t(-500)) ), abstol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-10)) ,  T(std__airy_ai( v_t(-10)) ), abstol);
  TTS_ULP_EQUAL(eve__airy_ai( T(-5))  ,  T(std__airy_ai( v_t(-5))  ), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(-2))  ,  T(std__airy_ai( v_t(-2))  ), 36.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(-1.5)),  T(std__airy_ai( v_t(-1.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(-0.5)),  T(std__airy_ai( v_t(-0.5))), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(-1))  ,  T(std__airy_ai( v_t(-1))  ), 10.0);

   TTS_ABSOLUTE_EQUAL(eve__airy_ai(a0), map(std__airy_ai, a0), 0.0001);
   TTS_RELATIVE_EQUAL(eve__airy_ai(a1), map(std__airy_ai, a1), 0.0001);

};

EVE_TEST( "Check behavior of diff(airy_ai) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-20.0, 0.0),
                              eve::test::randoms(0.0, 20.0)
                             )
        )
  <typename T>(T a0, T a1)
{
  using v_t = eve::element_type_t<T>;
  v_t reltol = 50000000*eve::eps(eve::as<v_t>());
  auto eve__airy_ai =  [](auto x) { return eve::diff(eve::airy_ai)(x); };
  auto std__airy_ai =  [](auto x)->v_t { return boost::math::airy_ai_prime(x); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__airy_ai(eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::minf(eve::as< T>())), eve::nan(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::inf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__airy_ai(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1500)), std__airy_ai(v_t(1500)), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(500)), std__airy_ai(v_t(500)), 10.0);

  TTS_ULP_EQUAL(eve__airy_ai(v_t(10)),  std__airy_ai(v_t(10)) , 50.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(5)),  std__airy_ai(v_t(5))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(2)),  std__airy_ai(v_t(2))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1.5)),std__airy_ai(v_t(1.5)) , 50.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(0.5)),std__airy_ai(v_t(0.5)) , 51.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(1)),  std__airy_ai(v_t(1))   , 50.0);
  TTS_ULP_EQUAL(eve__airy_ai(v_t(0)),  v_t(-0.258819403792807), 0.5);

  TTS_ULP_EQUAL(eve__airy_ai( T(1500)), T(std__airy_ai(v_t(1500))), 10.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(500)),  T(std__airy_ai(v_t(500)) ), 10.0);

  TTS_ULP_EQUAL(eve__airy_ai( T(10)) ,  T(std__airy_ai( v_t(10)) ),  50.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(5))  ,  T(std__airy_ai( v_t(5))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(2))  ,  T(std__airy_ai( v_t(2))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(1.5)),  T(std__airy_ai( v_t(1.5))),  50.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(0.5)),  T(std__airy_ai( v_t(0.5))),  51.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(1))  ,  T(std__airy_ai( v_t(1))  ),  50.0);
  TTS_ULP_EQUAL(eve__airy_ai( T(0))  ,  T(-0.258819403792807), 0.5);

  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-1500)), std__airy_ai(v_t(-1500)), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-500)), std__airy_ai(v_t(-500)), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-10)),  std__airy_ai(v_t(-10)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-5)),  std__airy_ai(v_t(-5))   , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-2)),  std__airy_ai(v_t(-2))   , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-1.5)),std__airy_ai(v_t(-1.5)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-0.5)),std__airy_ai(v_t(-0.5)) , reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai(v_t(-1)),  std__airy_ai(v_t(-1))   , reltol);

  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-1500)), T(std__airy_ai(v_t(-1500))), reltol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-500)),  T(std__airy_ai(v_t(-500)) ), reltol);
  TTS_ABSOLUTE_EQUAL(eve__airy_ai( T(-10)) ,  T(std__airy_ai( v_t(-10)) ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai( T(-5))  ,  T(std__airy_ai( v_t(-5))  ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai( T(-2))  ,  T(std__airy_ai( v_t(-2))  ), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai( T(-1.5)),  T(std__airy_ai( v_t(-1.5))), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai( T(-0.5)),  T(std__airy_ai( v_t(-0.5))), reltol);
  TTS_RELATIVE_EQUAL(eve__airy_ai( T(-1))  ,  T(std__airy_ai( v_t(-1))  ), reltol);

  TTS_ABSOLUTE_EQUAL(eve__airy_ai(a0), map(std__airy_ai, a0), 0.0001);
  TTS_RELATIVE_EQUAL(eve__airy_ai(a1), map(std__airy_ai, a1), 0.0001);
};


/* +---------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)               |
|                          http://www.mrpt.org/                             |
|                                                                           |
| Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
| See: http://www.mrpt.org/Authors - All rights reserved.                   |
| Released under BSD License. See details in http://www.mrpt.org/License    |
+---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers
#include <iostream>
#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "posit.h"

mrpt::vision::pnp::posit::posit(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd camera_intrinsic_, int n0)
{
		obj_pts=obj_pts_;
		img_pts=img_pts_.block(0,0,n0,2);
		cam_intrinsic=camera_intrinsic_;
		R=Eigen::MatrixXd::Identity(3,3);
		t=Eigen::VectorXd::Zero(3);
		f=(cam_intrinsic(0,0)+cam_intrinsic(1,1))/2;

		obj_matrix=(obj_pts.transpose() * obj_pts).inverse() * obj_pts.transpose();

		n=n0;

		obj_vecs=Eigen::MatrixXd::Zero(n0,3);

		for(int i=0;i<n;i++)
			obj_vecs.row(i)=obj_pts.row(i)-obj_pts.row(0);

		img_vecs = Eigen::MatrixXd::Zero(n0,2);
		img_vecs_old=img_vecs;

		epsilons=Eigen::VectorXd::Zero(n);
}

void mrpt::vision::pnp::posit::POS()
{
	Eigen::Vector3d I0, J0 , r1, r2, r3;
	double I0_norm, J0_norm;

	int i;
	double scale;

	for(i=0;i<3;i++)
	{
		I0(i)=obj_matrix.row(i).dot(img_vecs.col(0));
		J0(i)=obj_matrix.row(i).dot(img_vecs.col(1));
	}

	I0_norm=I0.norm();
	J0_norm=J0.norm();

	scale=(I0_norm + J0_norm)/2;

	/*Computing TRANSLATION */
	t(0)=img_pts(0,0)/scale;
	t(1)=img_pts(0,1)/scale;
	t(2)=f/scale;

	/* Computing ROTATION */
	r1=I0/I0_norm;
	r2=J0/J0_norm;
	r3=r1.cross(r2);

	R.row(0)=r1;
	R.row(1)=r2;
	R.row(2)=r3;
}

/**
Iterate over results obtained by the POS function;
see paper "Model-Based Object Pose in 25 Lines of Code", IJCV 15, pp. 123-141, 1995.
*/
bool mrpt::vision::pnp::posit::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
	Eigen::FullPivLU<Eigen::MatrixXd> lu(obj_pts);
	if(lu.rank()<3)
		return false;

	int i, iCount;
	long imageDiff=1000;

	for(iCount=0; iCount<LOOP_MAX_COUNT;iCount++)
	{
		if(iCount==0)
		{
			for(i=0;i<img_vecs.rows();i++)
				img_vecs.row(i)=img_pts.row(i)-img_pts.row(0);
		}

		else
		{
			// Compute new image vectors
			epsilons.setZero();
			for(i=0; i<n; i++)
			{
				epsilons(i)+=obj_vecs.row(i).dot(R.row(2));
			}
			epsilons/=t(2);

			// Corrected image vectors
			for(i=0; i<n; i++)
			{
				img_vecs.row(i)= img_pts.row(i) * (1+epsilons(i)) -img_pts.row(0);
			}

			imageDiff=this->get_img_diff();

		}

		img_vecs_old=img_vecs;

		this->POS();

		if(iCount>0 && imageDiff==0)
			break;

		if(iCount==LOOP_MAX_COUNT)
		{
			std::cout<<"Solution Not converged"<<std::endl<<std::endl;
			break;
		}

	}
	R_=R;
	t_=t;

	return true;
}

long mrpt::vision::pnp::posit::get_img_diff()
{
	int i, j;
	long sumOfDiffs = 0;

	for (i=0;i<n;i++){
		for (j=0;j<2;j++){
			sumOfDiffs += std::abs(floor(0.5+img_vecs(i,j))-floor(0.5+img_vecs_old(i,j)));
		}
	}
	return sumOfDiffs;

}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include <boost/math/distributions/gamma.hpp>

#include "aslam/calibration/statistics/Randomizer.h"
#include "aslam/calibration/functions/LogGammaFunction.h"
#include "aslam/calibration/functions/DigammaFunction.h"
#include "aslam/calibration/functions/IncompleteGammaPFunction.h"
#include "aslam/calibration/exceptions/BadArgumentException.h"
#include "aslam/calibration/exceptions/InvalidOperationException.h"

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    template <typename T>
    GammaDistribution<T>::GammaDistribution(const T& shape, double invScale) {
      setShape(shape);
      setInvScale(invScale);
    }

    template <typename T>
    GammaDistribution<T>::GammaDistribution(const GammaDistribution& other) :
        mShape(other.mShape),
        mInvScale(other.mInvScale),
        mNormalizer(other.mNormalizer) {
    }

    template <typename T>
    GammaDistribution<T>& GammaDistribution<T>::operator =
        (const GammaDistribution& other) {
      if (this != &other) {
        mShape = other.mShape;
        mInvScale = other.mInvScale;
        mNormalizer = other.mNormalizer;
      }
      return *this;
    }

    template <typename T>
    GammaDistribution<T>::~GammaDistribution() {
    }

/******************************************************************************/
/* Stream operations                                                          */
/******************************************************************************/

    template <typename T>
    void GammaDistribution<T>::read(std::istream& stream) {
    }

    template <typename T>
    void GammaDistribution<T>::write(std::ostream& stream) const {
      stream << "shape: " << mShape << std::endl
        << "inverse scale: " << mInvScale;
    }

    template <typename T>
    void GammaDistribution<T>::read(std::ifstream& stream) {
    }

    template <typename T>
    void GammaDistribution<T>::write(std::ofstream& stream) const {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    template <typename T>
    void GammaDistribution<T>::setShape(const T& shape) {
      if (shape <= 0)
        throw BadArgumentException<T>(shape,
          "GammaDistribution::setShape(): shape must be strictly positive",
          __FILE__, __LINE__);
      mShape = shape;
      computeNormalizer();
    }

    template <typename T>
    const T& GammaDistribution<T>::getShape() const {
      return mShape;
    }

    template <typename T>
    void GammaDistribution<T>::setInvScale(double invScale) {
      if (invScale <= 0)
        throw BadArgumentException<double>(invScale,
          "GammaDistribution::setScale(): inverse scale must be "
          "strictly positive",
          __FILE__, __LINE__);
      mInvScale = invScale;
      computeNormalizer();
    }

    template <typename T>
    double GammaDistribution<T>::getInvScale() const {
      return mInvScale;
    }

    template <typename T>
    void GammaDistribution<T>::computeNormalizer() {
      LogGammaFunction<T> logGammaFunction;
      mNormalizer = logGammaFunction(mShape) - mShape * log(mInvScale);
    }

    template <typename T>
    double GammaDistribution<T>::getNormalizer() const {
      return mNormalizer;
    }

    template <typename T>
    double GammaDistribution<T>::pdf(const RandomVariable& value) const {
      if (value < 0)
        return 0.0;
      else
        return exp(logpdf(value));
    }

    template <typename T>
    double GammaDistribution<T>::logpdf(const RandomVariable& value) const {
      if (value == 0 && mShape == T(1))
        return -mNormalizer;
      else
        return (mShape - 1) * log(value) - value * mInvScale - mNormalizer;
    }

    template <typename T>
    double GammaDistribution<T>::cdf(const RandomVariable& value) const {
      const IncompleteGammaPFunction incGammaPFunction(mShape);
      if (value <= 0)
        return 0.0;
      else
        return incGammaPFunction(value * mInvScale);
    }

    template <typename T>
    typename GammaDistribution<T>::RandomVariable
        GammaDistribution<T>::invcdf(double probability) const {
      if (probability < 0 || probability > 1)
        throw BadArgumentException<double>(probability,
          "GammaDistribution::invcdf(): probability must lie in [0, 1]",
          __FILE__, __LINE__);
        return boost::math::quantile(boost::math::gamma_distribution<>(
          mShape, 1.0 / mInvScale), probability);
    }

    template <typename T>
    typename GammaDistribution<T>::RandomVariable
        GammaDistribution<T>::getSample() const {
      const static Randomizer<double> randomizer;
      return randomizer.sampleGamma(mShape, mInvScale);
    }

    template <typename T>
    typename GammaDistribution<T>::Mean GammaDistribution<T>::getMean() const {
      return mShape / mInvScale;
    }

    template <typename T>
    typename GammaDistribution<T>::Mode GammaDistribution<T>::getMode() const {
      if (mShape >= 1)
        return (mShape - 1) / mInvScale;
      else
        throw InvalidOperationException("GammaDistribution<T>::getMode(): "
          "shape must be bigger or equal than 1");
    }

    template <typename T>
    typename GammaDistribution<T>::Variance GammaDistribution<T>::getVariance()
        const {
      return mShape / (mInvScale * mInvScale);
    }

    template <typename T>
    double GammaDistribution<T>::KLDivergence(const GammaDistribution<T>& other)
        const {
      LogGammaFunction<T> logGammaFunction;
      const DigammaFunction<T> digammaFunction;
      return (mShape - 1) * digammaFunction(mShape) -
        (other.mShape - 1) * digammaFunction(other.mShape) -
        logGammaFunction(mShape) +
        logGammaFunction(other.mShape) + other.mShape *
        (log(mInvScale) - log(other.mInvScale)) +
        mShape * (1.0 / mInvScale - 1.0 / other.mInvScale) * other.mInvScale;
    }

  }
}


/*
 * MODULE : bayesFilter - Bayesian Filtering
 * Provides a MuPAD module interface to a filtering class
 */

// Shut up VC++, it has its own problems!
#if defined(_MSC_VER) && _MSC_VER <= 1200
#pragma warning(disable:4786)	// indentifier more 255 chars
#endif

#include <typeinfo.h>
#include <string>
#include <map>
								// MF macro is define by MuPAD module system
#undef MF
#include "BayesFilter/allFlt.h" // Include all of Bayesian Filtering library
#include <boost/random.hpp>		// Fast and good random numbers
#include "TNear.h"				// Nearest neighbour
#include "MuPadConvert.h"

MMG( info = "Module: Bayesian Filtering" )

namespace MuC = MuPAD_convert;
namespace FM = Bayesian_filter_matrix;

class MuFilter
/*
 * MuPAD Runtime abstract filter
 *  Polymorphic filter created at Runtime
 *  By forceing all filter classes used at runtime we avoid the need for RTTI
 *  support in filtering library
 */
{
public:
	virtual ~MuFilter() = 0 {};
	void checkdef(const Bayesian_filter::Bayes_filter_base* derived) const;
};

void MuFilter::checkdef(const Bayesian_filter::Bayes_filter_base* derived) const
/*
 * Check that derived is defined. If it isn't generate MFerror based on base name
 */
{
	static std::string error;
	if (!derived) {
		error = std::string("Operation undefine on filter type: ") + typeid(*this).name();
		MFerror( error.c_str() );
	}
}

struct Mu_SIR : public MuFilter, public Bayesian_filter::SIR_filter
{
	Mu_SIR(unsigned x, unsigned s, Bayesian_filter::SIR_random& r) :
		SIR_filter(x,s,r) {}
};

struct Mu_SIR_kalman : public MuFilter, public Bayesian_filter::SIR_kalman_filter
{
	Mu_SIR_kalman(unsigned x, unsigned s, Bayesian_filter::SIR_random& r) :
		SIR_kalman_filter(x,s,r) {}
};

struct Mu_Unscented : public MuFilter, public Bayesian_filter::Unscented_filter
{
	Mu_Unscented(unsigned x) : Unscented_filter(x) {}
};

struct Mu_Cov : public MuFilter, public Bayesian_filter::Covariance_filter
{
	Mu_Cov(unsigned x) : Covariance_filter(x) {}
};

struct Mu_Inf : public MuFilter, public Bayesian_filter::Information_filter
{
	Mu_Inf(unsigned x) : Information_filter(x) {}
};

struct Mu_InfJo : public MuFilter, public Bayesian_filter::Information_joseph_filter
{
	Mu_InfJo(unsigned x) : Information_joseph_filter(x) {}
};

struct Mu_SRIF : public MuFilter, public Bayesian_filter::Information_root_filter
{
	Mu_SRIF(unsigned x) : Information_root_filter(x) {}
};

struct Mu_UD : public MuFilter, public Bayesian_filter::UD_filter
{
	Mu_UD(unsigned x) : UD_filter(x, x) {}
};

class BFilter_handler
/*
 * MuPAD interface for Bayesian Filter object
 * Allow them to be accessed by integer handles
 *  Handles start at 1 and increment with each make
 * TODO: Balance the map by using random handles
 */
{
public:
	BFilter_handler();
	int make (MuFilter*);
	MuFilter* remove (MTcell arg);
	MuFilter* get_filter(MTcell arg);
private:
	typedef std::map<int,MuFilter*> Flt_map;
	Flt_map fltmap;
	unsigned handle;
};

BFilter_handler::BFilter_handler()
{
	handle = 0;
}

int BFilter_handler::make (MuFilter* f)
/*
 * Make a handle for a filter
 * Handle overflow by warning and return 0 if handle already in use;
 */
{
	++handle;
	if (handle == 0)
	{
		MFputs( "Filter handle overflow: may prevent creation of more filters" );
		++handle;
	}

	std::pair<Flt_map::iterator, bool> i = fltmap.insert(std::make_pair(handle, f));
	if (i.second)
		return handle;
	else
	{	// Not inserted
		MFputs( "Could create unique handle for filter: no filter created" );
		return 0;
	}
};

MuFilter*  BFilter_handler::remove (MTcell arg)
/*
 * Remove a handle for a filter and return filter pointer if valid
 */
{
	if (!MFisInt(arg))
		MFerror( "Filter handle not an Int" );
	const int h = MFint(arg);
	if (h <=0) {
		MFerror( "Filter handle incorrect" );
		return 0;	// Never reached
	}
	Flt_map::iterator e = fltmap.find(h);
	if (e ==fltmap.end()) {
		MFerror( "Filter handle incorrect" );
		return 0;	// Never reached
	}

	MuFilter* f = (*e).second;
	fltmap.erase(e);

	return f;
}

MuFilter* BFilter_handler::get_filter (MTcell arg)
/*
 * Convert a handle into a filter pointer
 */
{
	if (!MFisInt(arg))
		MFerror( "Filter handle not an Int" );
	const int h = MFint(arg);
	if (h <=0) {
		MFerror( "Filter handle incorrect" );
		return 0;	// Never reached
	}
	Flt_map::iterator e = fltmap.find(h);
	if (e ==fltmap.end()) {
		MFerror( "Filter handle incorrect" );
		return 0;	// Never reached
	}

	return (*e).second;
}

// Maintain state of handles functions innovations
BFilter_handler Handles;

class Boost_random
/*
 * Random number distributions
 */
{
public:
	Boost_random() : gen_normal(rng), gen_uniform(rng)
	{
	}
	double normal(const double mean, const double sigma)
	{
		boost::normal_distribution<boost::mt19937> gen(rng, mean, sigma);
		return gen();
	}
	void normal(FM::Vec& v)
	{
		std::generate (v.begin(), v.end(), gen_normal);
	}
	void uniform_01(FM::Vec& v)
	{
		std::generate (v.begin(), v.end(), gen_uniform);
	}
private:
	boost::mt19937 rng;
	boost::normal_distribution<boost::mt19937> gen_normal;
	boost::uniform_01<boost::mt19937> gen_uniform;
};

// Maintain state of random numbers between function innovations
static Boost_random Random;

class MuPAD_function_model : public Bayesian_filter::Function_model
/*
 * Function model rapper for a MuPad function
 */
{
public:
	MuPAD_function_model(const MTcell ff);
	virtual const FM::Vec& fx(const FM::Vec& x) const;
	// Note: Reference return value as a speed optimisation, MUST be copied by caller.
private:
	const MTcell Mu_fn;
	mutable FM::Vec rfx;
};

MuPAD_function_model::MuPAD_function_model(const MTcell ff) :
	Mu_fn(ff), rfx(FM::Empty)
{}

const FM::Vec& MuPAD_function_model::fx(const FM::Vec& x) const
{
	MTcell xarray = MuC::Array(x);
	MTcell fxarray = MFcall( MFcopy(Mu_fn), 1, xarray );

	if (MFisExpr(fxarray))
		MFerror ("prediction function not found");	// ISSUE: MFerror may leak arrays

	rfx = MuC::Vector(fxarray);
	MFfree(xarray);
	MFfree(fxarray);
	return rfx;
};

class Likelihood_observe_MuPAD : public Bayesian_filter::Likelihood_observe_model
/*
 * Likelihood observe model using a MuPad expression
 */
{
public:
	Likelihood_observe_MuPAD(unsigned z_size, const MTcell f);
	~Likelihood_observe_MuPAD();
	Float L(const FM::Vec& x) const;
	void Lz(const FM::Vec& z);
private:
	const MTcell Mu_fn;
	MTcell Mu_z;
};

Likelihood_observe_MuPAD::Likelihood_observe_MuPAD(unsigned z_size, const MTcell f) :
	Likelihood_observe_model(z_size),
	Mu_fn(f)
{
	Mu_z = 0;
}

Likelihood_observe_MuPAD::~Likelihood_observe_MuPAD()
{
	if (Mu_z)
		MFfree(Mu_z);
}

Likelihood_observe_MuPAD::Float Likelihood_observe_MuPAD::L(const FM::Vec& x) const
{
	MTcell Mu_x = MuC::Array(x);
	MTcell Mu_L = MFcall( MFcopy(Mu_fn), 2, MFcopy(Mu_z), Mu_x );

	if (MFisExpr(Mu_L))
		MFerror ("Likelihood observe function not found");	// ISSUE: MFerror may leak arrays

	Float L = MFdouble(Mu_L);		// ISSUE: Assume Float is double
	MFfree(Mu_x);
	MFfree(Mu_L);
	return L;
};

void Likelihood_observe_MuPAD::Lz(const FM::Vec& z)
{
	if (Mu_z)
		MFfree(Mu_z);
	Mu_z = MuC::Array(z);
};

class Boost_SIR_random_helper : public Bayesian_filter::SIR_filter::Random
/*
 * Random number generator for SIR_filter
 * Uses global Random
 */
{
public:
	Boost_SIR_random_helper()
	{}
	void normal(FM::Vec& v)
	{
		::Random.normal(v);
	}
	void uniform_01(FM::Vec& v)
	{
		::Random.uniform_01(v);
	}
};

class Boost_Predict_random_helper : public Bayesian_filter::General_LiAd_predict_model::Random
/*
 * Random number generator for General_LiAd_predict_model
 * Uses global Random
 */
{
public:
	void normal(FM::Vec& v)
	{
		::Random.normal(v);
	}
};

class Filter_maker
/*
 * Make instances of Bayes filters by name
 */
{
public:
	Filter_maker();
	typedef MuFilter* Filter;
	Filter make (MTcell args);
private:
	MTcell MVargs;		// Same name as in MuPAD function so macros work!
	unsigned state_size();
	Boost_SIR_random_helper randomHelper;

	typedef Filter (Filter_maker::* pMake_mem)();
	typedef std::map<std::string, pMake_mem> Make_map;
	Make_map filterMakers;

	// Filter makers
	Filter make_SIR();
	Filter make_SIR_Kalman();
	Filter make_Unscented();
	Filter make_Covariance();
	Filter make_Information();
	Filter make_Information_joseph();
	Filter make_SRIF();
	Filter make_UD();
};

Filter_maker::Filter_maker()
/*
 * Construct name-> make mapping
 */
{
	filterMakers["SIR"] = &Filter_maker::make_SIR;
	filterMakers["SIR_Kalman"] = &Filter_maker::make_SIR_Kalman;
	filterMakers["Unscented"] = &Filter_maker::make_Unscented;
	filterMakers["Covariance"] = &Filter_maker::make_Covariance;
	filterMakers["Information"] = &Filter_maker::make_Information;
	filterMakers["InformationJoseph"] = &Filter_maker::make_Information_joseph;
	filterMakers["SRIF"] = &Filter_maker::make_SRIF;
	filterMakers["UD"] = &Filter_maker::make_UD;
}

Filter_maker::Filter
 Filter_maker::make (MTcell args)
/*
 * Use arg to Make instance of a filter
 */
{
	MVargs = args;					// Allow MFarg macros to work in instance
	if (MVnargs < 1)
		MFerror("No filter name argument");
	MFargCheck (1, DOM_STRING);

	std::string filterTypeName(MFstring(MFarg(1)));
								// Find the entry in filterMakers map
	Make_map::iterator i = filterMakers.find (filterTypeName);

	Filter f = 0;
	if (i != filterMakers.end())
	{	// Call the make member function found to create a filter
		pMake_mem pmake = i->second;
		f = (this->*pmake)();
	}
	return f;
};

unsigned Filter_maker::state_size()
{
	MFargCheck (2, DOM_INT);
	int ss = MFint(MFarg(2));
	if (!(ss > 0))
		MFerror ( "state_size must > 0" );
	return unsigned(ss);
}

Filter_maker::Filter
 Filter_maker::make_SIR()
{
	MFnargsCheckRange (4,6);

				// Initialise Sample S with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));
				// Defaultable parameters
	int sample_size = 1000;
	double rougheningK = -1;
	if (MVnargs >=5)
	{
		MFargCheck (5, DOM_INT);
		sample_size = MFint(MFarg(5));
		if (!(sample_size > 0))
			MFerror ( "sample_size must > 0" );
	}
	if (MVnargs >=6)
	{
		MFargCheck (6, DOM_FLOAT);
		rougheningK = MFdouble(MFarg(6));
		if (!(rougheningK >= 0))
			MFerror ( "rougheningK must >= 0" );
	}

	Mu_SIR* f = new Mu_SIR(state_size(), sample_size, randomHelper);

	// Use a temporary filter to create samples
	Bayesian_filter::SIR_kalman_filter tempS(f->S.size1(), f->S.size2(), randomHelper);
	tempS.init_kalman (init_x,init_X);
	f->init(tempS.S);
				// Overide filter default
	if (rougheningK != -1.)
		f->rougheningK = rougheningK;

	return f;
}

Filter_maker::Filter
 Filter_maker::make_SIR_Kalman()
{
	MFnargsCheckRange (4,6);

				// Initialise Sample S with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));
				// Defaultable parameters
	int sample_size = 1000;
	double rougheningK = -1;
	if (MVnargs >=5)
	{
		MFargCheck (5, DOM_INT);
		sample_size = MFint(MFarg(5));
		if (!(sample_size > 0))
			MFerror ( "sample_size must > 0" );
	}
	if (MVnargs >=6)
	{
		MFargCheck (6, DOM_FLOAT);
		rougheningK = MFdouble(MFarg(6));
		if (!(rougheningK >= 0))
			MFerror ( "rougheningK must >= 0" );
	}

	Mu_SIR_kalman* f = new Mu_SIR_kalman(state_size(), sample_size, randomHelper);

	f->init_kalman (init_x,init_X);
				// Overide filter default
	if (rougheningK != -1.)
		f->rougheningK = rougheningK;

	return f;
}

Filter_maker::Filter
 Filter_maker::make_Unscented()
{
	MFnargsCheck (4);

	// Initialise with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_Unscented* f = new Mu_Unscented(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

Filter_maker::Filter
 Filter_maker::make_Covariance()
{
	MFnargsCheck (4);

	// Initialise with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_Cov* f = new Mu_Cov(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

Filter_maker::Filter
 Filter_maker::make_Information()
{
	MFnargsCheck (4);

	// Initialise with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_Inf* f = new Mu_Inf(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

Filter_maker::Filter
 Filter_maker::make_Information_joseph()
{
	MFnargsCheck (4);

	// Initialise with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_InfJo* f = new Mu_InfJo(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

Filter_maker::Filter
 Filter_maker::make_SRIF()
{
	MFnargsCheck (4);

	// Initialise with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_SRIF* f = new Mu_SRIF(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

Filter_maker::Filter
 Filter_maker::make_UD()
{
	MFnargsCheck (4);

	// Initialise Sample S with state and covariance
	FM::Vec init_x = MuC::Vector(MFarg(3));
	FM::Matrix init_X = MuC::Matrix(MFarg(4));

	Mu_UD* f = new Mu_UD(state_size());
	f->init_kalman (init_x,init_X);
	return f;
}

// Maintain a set of default filters to make
Filter_maker DefaultFilters;

//
// Module functions definitions
//

MFUNC( bfilter, MCnop )
/*
 * DOM_INT bfilter(String filterTypeName, DOM_INT nstate, DOM_INT nsamples)
 * Create a filter from its names type and return its handle (Int)
 */
{
	// Construct Filter  - Must catch exceptions for MuPAD kernel
	MuFilter* filter;
	try
	{
		filter = DefaultFilters.make (MVargs);
		if (!filter)
			MFerror( "Unknown filter type name" );
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
	catch (...)
	{
		MFerror( "Caused a system exception" );
	}

	// Make a handle to return.
	const int h = Handles.make (filter);
	if (!h)		// Not Created
		delete filter;
	MFreturn (MFint(h));
} MFEND

MFUNC( free, MCnop )
/*
 * DOM_NULL free(DOM_INT hFtiler)
 * free the resources associated with a filter handle
 * The handle thus invalidated
 */
{
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.remove(MFarg(1));

	if (filter)
	{
		delete filter;
	}

	MFreturn (MFcopy(MVnull));
} MFEND

MFUNC( predict_functional, MCnop )
/*
 * DOM_NULL predict_functional (DOM_INT hFilter, DOM_PROC f)
 *  Predict filter with zero noise through function f(x)
 */
{
	MFnargsCheck (2);
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Must be a Functional filter
	Bayesian_filter::Functional_filter* f = dynamic_cast<Bayesian_filter::Functional_filter*>(filter);
	filter->checkdef(f);

	class FF : public Bayesian_filter::Functional_predict_model
	{	// Functional form
		Bayesian_filter::Function_model& ff;
	public:
		FF (Bayesian_filter::Function_model& f_init) :
			ff(f_init)
		{}
		virtual const FM::Vec& fx(const FM::Vec& x) const
		{	return ff.fx(x);
		}
	};

	try
	{
		// Predict filter using MuPAD expression
		MuPAD_function_model mu_f(MFarg(2));

		f->predict (FF(mu_f));
		MFreturn (MFcopy(MVnull));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( predict_additive, MCnop )
/*
 * DOM_REAL predict_additive (DOM_INT hFilter, DOM_PROC f, DOM_Array q, DOM_Array G)
 *  Predict filter with additive noise Gq
 */
{
	MFnargsCheck (4);
	MuFilter* filter = Handles.get_filter(MFarg(1));
	FM::Vec    q = MuC::Vector(MFarg(3));
	FM::Matrix G = MuC::Matrix(MFarg(4));

	// Check matrix conformance
	{
		Bayesian_filter::State_filter* f = dynamic_cast<Bayesian_filter::State_filter*>(filter);
		filter->checkdef(f);

		if (f->x.size() != G.size1())
			MFerror("Mismatch in x and G size");
		if (q.size() != G.size2())
			MFerror("Mismatch in q and G size");
	}

	bool bFilterOp = false;			// Flag operation complete to avoid MFerror in try block
	double rcond;
	try
	{
		// Predict filter using MuPAD expression
		MuPAD_function_model mu_f(MFarg(2));
		Bayesian_filter::Simple_additive_predict_model model(mu_f, G, q);
		MFerror ("Not implemented");

#ifdef UNIMPLEMENTED
// TODO Requires General_LzAd_predict_model
		Boost_Predict_random_helper predict_random;
		Bayesian_filter::General_LzAd_predict_model Lmodel(G.size1(), q.size(), predict_random);
		// Use mu_f to construct Lmodel
		FM::copy (model.q, Lmodel.q);
		FM::copy (model.G, Lmodel.G);

		// Must be a Sample filter
		if (Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter)) {
			bFilterOp = true;
			f->predict (Lmodel);
			rcond = 0.;
		}
#endif
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}

	if (bFilterOp) {
		MFreturn (MFdouble(rcond));
	}
	else {
		filter->checkdef(0);
	}
} MFEND

MFUNC( predict_linear, MCnop )
/*
 * DOM_REAL predict_linear (DOM_INT hFilter, DOM_Array F, DOM_Array G, DOM_Array q)
 *  Predict filter with additive noise Gq
 */
{
	MFnargsCheck (4);
	MuFilter* filter = Handles.get_filter(MFarg(1));
	FM::Matrix Fx = MuC::Matrix(MFarg(2));
	FM::Matrix G = MuC::Matrix(MFarg(3));
	FM::Vec    q = MuC::Vector(MFarg(4));

	// Check matrix conformance
	{
		Bayesian_filter::State_filter* f = dynamic_cast<Bayesian_filter::State_filter*>(filter);
		filter->checkdef(f);

		if (f->x.size() != Fx.size1())
			MFerror("Mismatch in x and Fx matrix");
		if (Fx.size1() != Fx.size2())
			MFerror("Mismatch Fx not square");
		if (f->x.size() != G.size1())
			MFerror("Mismatch in x and G size");
		if (q.size() != G.size2())
			MFerror("Mismatch in q and G size");
	}

	bool bFilterOp = false;			// Flag operation complete to avoid MFerror in try block
	double rcond;
	try
	{
		// Predict filter
		Boost_Predict_random_helper random;
		Bayesian_filter::General_LiAd_predict_model model(Fx.size1(), q.size(), random);
		model.Fx = Fx;
		model.q = q;
		model.G = G;

		// Must be a Likelihood, or Sample filter
		if (Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter)) {
			bFilterOp = true;
			f->predict (model);
			rcond = 0.;
		}
		else
		if (Bayesian_filter::Linrz_filter* f = dynamic_cast<Bayesian_filter::Linrz_filter*>(filter)) {
			bFilterOp = true;
			rcond = f->predict (model);
		}
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}

	if (bFilterOp) {
		MFreturn (MFdouble(rcond));
	}
	else {
		filter->checkdef(0);
	}
} MFEND

MFUNC( observe_likelihood, MCnop )
/*
 * DOM_REAL observe_likelihood (DOM_INT hFilter, DOM_PROC L, DOM_ARRAY z)
 *  Observe filter with Likelihood L(z,x)
 */
{
	MFnargsCheck (3);
	MuFilter* filter = Handles.get_filter(MFarg(1));
	FM::Vec    z = MuC::Vector(MFarg(3));

	// Must be a Sample filter
	Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter);
	filter->checkdef(f);

	try
	{
		// Observe filter using MuPAD expression
		Likelihood_observe_MuPAD model(z.size(), MFarg(2));
		f->observe(model, z);
		double rcond;
		f->update (rcond);
		MFreturn (MFdouble(rcond));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( observe_linear_uncorrelated, MCnop )
/*
 * DOM_REAL observe_linear_uncorrelated (DOM_INT hFilter, DOM_ARRAY H, DOM_ARRAY z, DOM_ARRAY Zd)
 *  Observe z with noise Zv through linear H
 */
{
	MFnargsCheck (4);
	MuFilter* filter = Handles.get_filter(MFarg(1));
	FM::Matrix H = MuC::Matrix(MFarg(2));
	FM::Vec    z = MuC::Vector(MFarg(3));
	FM::Vec	  Zv = MuC::Vector(MFarg(4));

	{
		const Bayesian_filter::State_filter* f = dynamic_cast<const Bayesian_filter::State_filter*>(filter);
		filter->checkdef(f);

		// Check matrix conformance
		if (f->x.size() != H.size2())
			MFerror("Mismatch in x and H matrix");
		if (z.size() != H.size1())
			MFerror("Mismatch in z and H matrix");
		if (Zv.size() != z.size())
			MFerror("Mismatch in z and Zv size");
	}

	bool bFilterOp = false;			// Flag operation to avoid MFerror in try block
	double rcond;
	try
	{
		// Observe filter
		Bayesian_filter::General_LiUnAd_observe_model model(H.size2(), z.size());
		model.Hx = H;
		model.Zv = Zv;

		// Must be a Sample, Linrz filter
		if (Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter)) {
			bFilterOp = true;
			f->observe (model, z);
			double rcond;
			f->update (rcond);
		}
		else
		if (Bayesian_filter::Linrz_filter* f = dynamic_cast<Bayesian_filter::Linrz_filter*>(filter)) {
			bFilterOp = true;
			rcond = f->observe (model, z);
		}
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}

	if (bFilterOp) {
		MFreturn (MFdouble(rcond));
	}
	else {
		filter->checkdef(0);
	}
} MFEND

MFUNC( observe_linear_correlated, MCnop )
/*
 * DOM_REAL observe_linear_correlated (DOM_INT hFilter, DOM_ARRAY H, DOM_ARRAY z, DOM_ARRAY Z)
 *  Observe z with noise Zv through linear H
 */
{
	MFnargsCheck (4);
	MuFilter* filter = Handles.get_filter(MFarg(1));
	FM::Matrix H = MuC::Matrix(MFarg(2));
	FM::Vec    z = MuC::Vector(MFarg(3));
	FM::Matrix Z = MuC::Matrix(MFarg(4));

	{
		const Bayesian_filter::State_filter* f = dynamic_cast<const Bayesian_filter::State_filter*>(filter);
		filter->checkdef(f);

		// Check matrix conformance
		if (f->x.size() != H.size2())
			MFerror("Mismatch in x and H matrix");
		if (z.size() != H.size1())
			MFerror("Mismatch in z and H matrix");
		if (Z.size1() != z.size())
			MFerror("Mismatch in z and Z size");
		if (Z.size1() != Z.size2())
			MFerror("Mismatch Z not square");
	}

	bool bFilterOp = false;			// Flag operation to avoid MFerror in try block
	double rcond;
	try
	{
		// Observe filter
		Bayesian_filter::General_LiCoAd_observe_model model(H.size2(), z.size());
		model.Hx = H;
		model.Z = Z;
		// Must be a Sample, Linrz filter
		if (Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter)) {
			bFilterOp = true;
			f->observe (model, z);
			f->update (rcond);
		}
		else
		if (Bayesian_filter::Linrz_filter* f = dynamic_cast<Bayesian_filter::Linrz_filter*>(filter)) {
			bFilterOp = true;
			rcond = f->observe (model, z);
		}
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}

	if (bFilterOp) {
		MFreturn (MFdouble(rcond));
	}
	else {
		filter->checkdef(0);
	}
} MFEND

MFUNC( mean, MCnop )
/*
 * DOM_ARRAY mean(DOM_INT hFilter)
 *  mean of filter state
 */
{
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Must be a State filter
	Bayesian_filter::State_filter* f = dynamic_cast<Bayesian_filter::State_filter*>(filter);
	filter->checkdef(f);

	// Process Filter  - Must catch exceptions for MuPAD kernel
	try
	{
		f->update();
		MFreturn(MuC::Array(f->x));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( covariance, MCnop )
/*
 * DOM_ARRAY covariance (DOM_INT hFilter)
 *  covariance of filter state
 */
{
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Must be a Kalman filter
	Bayesian_filter::Kalman_filter* f = dynamic_cast<Bayesian_filter::Kalman_filter*>(filter);
	filter->checkdef(f);

	// Process Filter  - Must catch exceptions for MuPAD kernel
	try
	{
		f->update();
		MFreturn(MuC::Array(f->X));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( unique_samples, MCnop )
/*
 * DOM_INT unique_samples (DOM_INT hFilter)
 *  no of unique (different value) samples in Filter
 */
{
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Must be a Sample filter
	Bayesian_filter::Sample_filter* f = dynamic_cast<Bayesian_filter::Sample_filter*>(filter);
	filter->checkdef(f);

	// Process Filter  - Must catch exceptions for MuPAD kernel
	try
	{
		unsigned nsample = f->unique_samples();
		MFreturn(MFint(nsample));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( stochastic_samples, MCnop )
/*
 * DOM_INT stochastic_samples (DOM_INT hFilter)
 *  no of unique (stochastic history) samples in Filter
 */
{
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Must be a SIR filter
	Bayesian_filter::SIR_filter* f = dynamic_cast<Bayesian_filter::SIR_filter*>(filter);
	filter->checkdef(f);

	// Process Filter  - Must catch exceptions for MuPAD kernel
	try
	{
		unsigned nsample = f->stochastic_samples;
		MFreturn(MFint(nsample));
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( sample, MCnop )
/*
 * DOM_LIST sample (DOM_INT hFilter)
 *  Return samples (list of lists) reresenting filters state probability distribution.
 *  If the filter is represented by samples these direcly used
 *  otherwise a set of samples are generated to represent it
 */
{
	const unsigned GenerateSamples = 1000;
	MFnargsCheck( 1 );
	MuFilter* filter = Handles.get_filter(MFarg(1));

	// Process filter by type - Must catch exceptions for MuPAD kernel
	try
	{
		// Sample filter is easy
		{
			Bayesian_filter::Sample_filter* sf = dynamic_cast<Bayesian_filter::Sample_filter*>(filter);
			if (sf) {
				MFreturn(MuC::ListTranspose(sf->S));
			}
		}

		// Kalman filter requres more work. Create Samples from a mean and covariance of Kalman filter
		{
			Bayesian_filter::Kalman_filter* kf = dynamic_cast<Bayesian_filter::Kalman_filter*>(filter);
			if (kf) {
				kf->update();
				Boost_SIR_random_helper randomHelper;
				Bayesian_filter::SIR_kalman_filter tempS(kf->x.size(), GenerateSamples, randomHelper);
				tempS.init_kalman (kf->x, kf->X);
				MFreturn(MuC::ListTranspose(tempS.S));
			}
		}

		// Fall through if type unknown
		filter->checkdef(0);
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

#error This is wrong V must be a pointer like object
class V
{	// Vector proxy and difference for Neariest Neighbour
public:
	typedef FM::ColMatrix::const_Column NNVec;

	double distance (const V& o) const
	{
		double d = 0.;
		NNVec::const_iterator oi = (o.dp).begin();
		for (NNVec::const_iterator vi = (dp).begin(); vi != (dp).end(); ++vi, ++oi) {
			const double diff = *vi - *oi;
			d += diff*diff;
		}
		return sqrt(d);
	}
	V()	// Empty for return value
	{
	}
	V( const FM::ColMatrix& colmat, std::size_t i) : dp(colmat,i)
	{
	}
	NNVec dp;	// Vec data pointer
};

FM::Vec find_nearest(const FM::ColMatrix& S, double radius, const FM::Vec& loc)
/*
 * Find the nearest neighbour to loc in S
 */
{
	// Construct NN tree of samples
	CNearTree<V> sampleTree;
	for (std::size_t si = 0; si != S.size2(); ++si) {
		sampleTree.m_fnInsert( V(S,si) );
	}
	// Create a location as same type as a sample
	FM::ColMatrix cmloc(S.size1(),1);
	FM::column(cmloc,0) = loc;

	// Find NN
	V nn;
	if (sampleTree.m_bfnNearestNeighbor(radius, nn, V(cmloc,0)) )
	{
		FM::Vec rnn(S.size1());
		rnn = nn.dp;
		return rnn;
	}
	else
	{	// No NN
		FM::Vec rnn(FM::Empty);
		return rnn;

	}
}

MFUNC( nearest_sample, MCnop )
/*
 * Find nearest neighbour in a sample
 */
{
	MFnargsCheck( 3 );
	MuFilter* filter = Handles.get_filter(MFarg(1));
	double	  radius = MFdouble(MFarg(2));
	FM::Vec   loc = MuC::Vector(MFarg(3));

	// Process filter by type - Must catch exceptions for MuPAD kernel
	try
	{
		// Sample filter is easy
		{
			Bayesian_filter::Sample_filter* sf = dynamic_cast<Bayesian_filter::Sample_filter*>(filter);
			if (sf) {
				MFreturn( MuC::List(find_nearest(sf->S, radius, loc)) );
			}
		}

		// Fall through if type unknown
		filter->checkdef(0);
	}
	catch (Bayesian_filter::Bayes_filter_exception fe)
	{
		MFerror( fe.what() );
	}
} MFEND

MFUNC( _test, MCnop )
/*
 * testing
 */
{
	MFnargsCheck( 0 );

	MFreturn (MFcopy(MVnull));

	//FM::Vec v = MuC::Vector(MFarg(2));
	//MFreturn (MuC::List(v));

} MFEND