#include "HCUBE_Defines.h"

#include "Experiments/HCUBE_AtariIntrinsicExperiment.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>

using namespace NEAT;

namespace HCUBE
{
    SarsaLambda::SarsaLambda(int numFeatures, int numActions,
                             float gamma, float alpha, float epsilon, float lambda):
        numFeatures(numFeatures), numActions(numActions), gamma(gamma), alpha(alpha), epsilon(epsilon), lambda(lambda)
    {
        // Initialize the SARSA(Lambda) vectors
        for (int i=0; i<numFeatures*numActions; i++) {
            w.push_back(0);
            e.push_back(0);
        }
    }

    void SarsaLambda::reset() {
        // Reset eligibility vector
        for (int i=0; i<numFeatures*numActions; i++)
            e[i] = 0;
        reward = 0;
    }

    int SarsaLambda::act(std::vector<bool>& currState, double lastActionReward) {
        assert(currState.size() == numFeatures);

        vector<double> qVals;
        for (int a=0; a<numActions; a++) {
            double Q_a = 0;
            for (int i=0; i<numFeatures; i++)
                if (currState[i])
                    Q_a += w[a*numFeatures+i];
            qVals.push_back(Q_a);
        }

        int action = selectAction(qVals);
        double Q = qVals[action];

        // Compute bellman error
        double delta = lastActionReward + (gamma * Q) - oldQ;

        // Update the weights
        for (int i=0; i<numFeatures*numActions; i++) {
            w[i] += alpha * delta * e[i];
        }

        // Decay the eligibility traces
        for (int i=0; i<numFeatures*numActions; i++) {
            e[i] *= gamma * lambda;
        }

        // Set the active features' eligibility traces to 1
        for (int i=0; i<numFeatures; i++) {
            if (currState[i])
                e[numFeatures*action + i] = 1;
        }

        oldQ = Q;
        return action;
    }

    int SarsaLambda::selectAction(vector<double>& qVals) {
        if (double(rand())/RAND_MAX < epsilon) {
            return rand() % numActions;
        } else {
            vector<int> max_inds;
            double max_val = -1e37;
            for (int i=0; i < numActions; i++) {
                if (qVals[i] == max_val)
                    max_inds.push_back(i);
                else if (qVals[i] > max_val) {
                    max_inds.clear();
                    max_inds.push_back(i);
                    max_val = qVals[i];
                }
            }
            return max_inds[rand() % max_inds.size()];
        }
    }

    void SarsaLambda::printinfo() {
        printf("Gamma %f alpha %f epsilon %f lambda %f\n",gamma,alpha,epsilon,lambda);
        cout << "Weight Vec: ";
        for (int i=0; i<numFeatures*numActions; i++) {
            printf("%1.2f ",w[i]);
            if (i%numFeatures == 0)
                printf("\n");
        }
        cout << endl;
    }

    AtariIntrinsicExperiment::AtariIntrinsicExperiment(string _experimentName,int _threadID):
        Experiment(_experimentName,_threadID), visProc(NULL), rom_file(""),
        numActions(0), numObjClasses(0), display_active(false), agent(NULL)
    {
    }

    void AtariIntrinsicExperiment::initializeExperiment(string _rom_file) {
        rom_file = _rom_file;

        substrate_width = 8;
        substrate_height = 10;

        // Check that rom exists and is readable
        ifstream file(rom_file.c_str());
        if (!file.good()) {
            cerr << "Unable to find or open rom file: \"" << rom_file << "\"" << endl;
            exit(-1);
        }

        // Initialize Atari Stuff
        if (!ale.loadROM(rom_file.c_str(), display_active, true)) {
            cerr << "Ale had problem loading rom..." << endl;
            exit(-1);
        }
        numActions = ale.legal_actions.size();
        numFeatures = substrate_width * substrate_height * (numObjClasses + 1);

        for (int i=0; i<numFeatures; i++)
            phi.push_back(0);

        if (agent) delete agent;
        agent = new SarsaLambda(numFeatures, numActions);

        // Load the visual processing framework
        visProc = ale.visProc;
        numObjClasses = visProc->manual_obj_classes.size();
        if (numObjClasses <= 0) {
            cerr << "No object classes found. Make sure there is an images directory containg class images." << endl;
            exit(-1);
        }

        // One input layer for each object class, plus an extra one for the self object
        for (int i=0; i<=numObjClasses; ++i) {
            for (int y=0; y<substrate_height; y++) {
                for (int x=0; x<substrate_width; x++) {
                    int xmod = substrate_width*i+x;
                    Node node(xmod, y, 0);
                    nameLookup[node] = (toString(xmod) + string("/") + toString(y) + string("/") + toString("0"));
                }
            }
        }

        // Single output node computes the reward
        Node node(0,0,2);
        nameLookup[node] = string("0/0/2");
    }

    NEAT::GeneticPopulation* AtariIntrinsicExperiment::createInitialPopulation(int populationSize) {
        GeneticPopulation *population = new GeneticPopulation();
        vector<GeneticNodeGene> genes;

        // One input layer for each object class and an extra for the self object
        for (int i=0; i<=numObjClasses; ++i) {
            for (int y=0; y<substrate_height; y++) {
                for (int x=0; x<substrate_width; x++) {
                    int xmod = substrate_width*i+x;
                    Node node(xmod, y, 0);
                    genes.push_back(GeneticNodeGene(nameLookup[node],"NetworkSensor",0,false));
                }
            }
        }

        // Output Layer
        Node node(0,0,2);
        genes.push_back(GeneticNodeGene(nameLookup[node],"NetworkOutputNode",1,false, ACTIVATION_FUNCTION_SIGMOID));

        // Bias node
        genes.push_back(GeneticNodeGene("Bias","NetworkSensor",0,false));

        for (int a=0; a<populationSize; a++) {
            shared_ptr<GeneticIndividual> individual(new GeneticIndividual(genes,true,1.0));
            for (int b=0;b<10;b++) {
                individual->testMutate();
            }
            population->addIndividual(individual);
        }

        cout << "Finished creating population\n";
        return population;
    }

    void AtariIntrinsicExperiment::processGroup(shared_ptr<NEAT::GeneticGeneration> generation)
    {
        shared_ptr<NEAT::GeneticIndividual> individual = group.front();
        individual->setFitness(0);

        substrate = individual->spawnFastPhenotypeStack<double>();

        runAtariEpisode(individual);
    }

    void AtariIntrinsicExperiment::runAtariEpisode(shared_ptr<NEAT::GeneticIndividual> individual) {
        agent->printinfo();
        int numEpisodes = 10000;
        double totalScore = 0;
        for (int episode = 0; episode < numEpisodes; episode++) {
            double reward = 0;
            ale.reset_game();
            agent->reset();

            while (!ale.game_over()) {
                for (int i=0; i<numFeatures; i++)
                    phi[i] = false;

                // Set value of all nodes to zero
                substrate.reinitialize();
                substrate.dummyActivation();

                // Set substrate value for all objects (of a certain size)
                setSubstrateObjectValues(*visProc);

                // Set substrate value for self
                setSubstrateSelfValue(*visProc);

                // Propagate values through the ANN
                substrate.update();

                // cout << "Phi: ";
                // for (int i=0; i<numFeatures; i++)
                //     cout << phi[i] << " ";
                // cout << endl;
                // printLayerInfo();

                int action_indx = agent->act(phi, reward);
                Action action = ale.legal_actions[action_indx];
                reward = ale.act(action);
            }
            cout << "Episode " << episode << " ended in " << ale.frame << " frames with score " << ale.game_score << endl;
            totalScore += ale.game_score;
        }

        // Give the reward to the agent
        float avgScore = totalScore / float(numEpisodes);
        individual->reward(avgScore);
    }

    void AtariIntrinsicExperiment::setSubstrateObjectValues(VisualProcessor& visProc) {
        for (int i=0; i<visProc.manual_obj_classes.size(); i++) {
            Prototype& proto = visProc.manual_obj_classes[i];
            paintSubstrate(visProc, proto, i);
        }
    }

    void AtariIntrinsicExperiment::paintSubstrate(VisualProcessor& visProc, Prototype& proto, int substrateIndx) {
        // Assign values to each of the objects
        float assigned_value = 1.0;
        for (set<long>::iterator it=proto.obj_ids.begin(); it!=proto.obj_ids.end(); it++) {
            long obj_id = *it;
            assert(visProc.composite_objs.find(obj_id) != visProc.composite_objs.end());
            point obj_centroid = visProc.composite_objs[obj_id].get_centroid();
            int adj_x = obj_centroid.x * substrate_width / visProc.screen_width;
            int adj_y = obj_centroid.y * substrate_height / visProc.screen_height;
            substrate.setValue(nameLookup[Node(substrate_width*substrateIndx+adj_x,adj_y,0)], assigned_value);
            // Set the phi-feature to true
            phi[(substrate_width*substrate_height*substrateIndx) + (substrate_width*adj_y) + adj_x] = true;
        }
    }

    void AtariIntrinsicExperiment::setSubstrateSelfValue(VisualProcessor& visProc) {
        if (!visProc.found_self())
            return;
        paintSubstrate(visProc, visProc.manual_self, numObjClasses);
    }

    int AtariIntrinsicExperiment::selectAction(vector<double>& qVals) {
        // if (NEAT::Globals::getSingleton()->getRandom().getRandomDouble() <= epsilon) {
        //     cout << "Selecting Random Action" << endl;
        //     return NEAT::Globals::getSingleton()->getRandom().getRandomInt(numActions);
        // } else {
        //     cout << "Q-Vals: ";
        //     vector<int> max_inds;
        //     double max_val = -1e37;
        //     for (int i=0; i < numActions; i++) {
        //         cout << qVals[i] << " ";
        //         if (qVals[i] == max_val)
        //             max_inds.push_back(i);
        //         else if (qVals[i] > max_val) {
        //             max_inds.clear();
        //             max_inds.push_back(i);
        //             max_val = qVals[i];
        //         }
        //     }
        //     cout << endl;
        //     int max_indx = NEAT::Globals::getSingleton()->getRandom().getRandomInt(max_inds.size());
        //     int action_indx = max_inds[max_indx]; //ale.legal_actions[max_inds[action_indx]];
        //     return action_indx;
        // }
    }

    double AtariIntrinsicExperiment::gauss2D(double x, double y, double A, double mu_x, double mu_y,
                                    double sigma_x, double sigma_y)
    {
        return A * exp(-1.0 * ((x-mu_x) * (x-mu_x) / 2.0 * sigma_x * sigma_x + (y-mu_y) * (y-mu_y) / 2.0 * sigma_y * sigma_y));
    }

    void AtariIntrinsicExperiment::printLayerInfo() {
        for (int i=0; i<=numObjClasses; i++) {
            printf("Obj Class %d Substrate:\n",i);
            for (int y=0; y<substrate_height; y++) {
                for (int x=0; x<substrate_width; x++) {
                    float output = substrate.getValue(nameLookup[Node(i*substrate_width+x,y,0)]);
                    printf("%1.1f ",output);
                }
                printf("\n");
            }
            printf("\n");
        }
    }

    Experiment* AtariIntrinsicExperiment::clone()
    {
        AtariIntrinsicExperiment* experiment = new AtariIntrinsicExperiment(*this);
        return experiment;
    }
}


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


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_GENERIC_REALLOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_GENERIC_REALLOG_HPP_INCLUDED

#include <nt2/exponential/functions/reallog.hpp>
#include <nt2/include/functions/simd/log.hpp>
#include <nt2/include/functions/simd/is_nltz.hpp>
#include <nt2/include/functions/simd/all.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/assert.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::reallog_, tag::cpu_
                            , (A0)
                            , (generic_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    NT2_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG(nt2::all(nt2::is_nltz(a0)), "reallog cannot produce complex result.");
      return nt2::log(nt2::tofloat(a0));
    }
  };
} }

#endif


#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>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> reciprocal(std::complex<T> const &val)
    {
      return static_cast<T>(1.) / val;
    }
    template <class T>
    auto reciprocal(T const &val) -> decltype(boost::simd::rec(val))
    {
      return boost::simd::rec(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME reciprocal
#define NUMPY_NARY_FUNC_SYM wrapper::reciprocal
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <Eigen/Dense>
#include <iostream>
#include "PowerSpectrum.h"
#include "MFCC/MFCC.h"

PowerSpectrum::PowerSpectrum(Analyser * analyser, AnalyserCanvas * canvas)
    : maxNfft(512),
      analyser(analyser), canvas(canvas)
{
    setObjectName("PowerSpectrum");

    holdLength = 25;

    rpm::vector<double> initialValues(holdLength);
    std::fill(initialValues.begin(), initialValues.end(), 1e-12);

    holdQues.resize(maxNfft, rpm::deque<double>(initialValues.begin(), initialValues.end()));
    holdSets.resize(maxNfft, rpm::multiset<double, std::greater<double>>(initialValues.begin(), initialValues.end()));
}

void PowerSpectrum::renderSpectrum(const int nframe, const int nNew, const double maximumFrequency, rpm::deque<SpecFrame>::const_iterator begin, rpm::deque<SpecFrame>::const_iterator end)
{
    using Eigen::ArrayXd;

    std::lock_guard<std::mutex> guard(imageLock);

    int minGain = canvas->getMinGainSpectrum();
    int maxGain = canvas->getMaxGainSpectrum();

    QPainter painter(&spectrum);
    painter.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing | QPainter::SmoothPixmapTransform);

    const double fs = begin->fs;

    // Advance the hold buffer.
    for (auto it = begin; it != end; ++it) {
        advanceHold(*it);
    }

    const int nfft = maxNfft;

    rpm::vector<double> spec(nfft, 0.0);

    for (int i = 0; i < nfft; ++i) {
        spec[i] = *(holdSets[i].begin());
    }

    const double delta = fs / (2 * nfft);

    QPainterPath path;

    for (int i = 0; i < nfft; ++i) {
        double freq = i * delta;

        if (freq > maximumFrequency) {
            break;
        }

        double gain = std::clamp<double>(20 * std::log10(spec[i]), minGain - 20, maxGain + 20);

        int x = targetWidth - (targetWidth * (maxGain - gain)) / (maxGain - minGain);
        int y = canvas->yFromFrequency(freq, maximumFrequency);

        if (i == 0) {
            path.moveTo(x, y);
        }
        else {
            path.lineTo(x, y);
        }
    }

    painter.setPen(QColor(0xFFA500));
    painter.drawPath(path);
}

void PowerSpectrum::advanceHold(const SpecFrame& frame)
{
    // The frame needs to be resampled first.
    const int nfft = maxNfft;

    if (nfft >= frame.nfft) {
        int gap = nfft / frame.nfft;

        for (int i = 0; i < frame.nfft; ++i) {
            for (int j = gap * i; j < gap * (i + 1); ++j) {
                const double val = abs(frame.spec(i));

                const double back = holdQues[j].back();
                holdQues[j].pop_back();
                holdQues[j].push_front(val);

                holdSets[j].erase(holdSets[j].find(back));
                holdSets[j].emplace(val);
            }
        }
    }
    else {
        int gap = frame.nfft / nfft;

        for (int i = 0; i < nfft; ++i) {
            double val = 0;
            for (int j = gap * i; j < gap * (i + 1); ++j) {
                val += abs(frame.spec(j));
            }
            val /= (double) gap;

            const double back = holdQues[i].back();
            holdQues[i].pop_back();
            holdQues[i].push_front(val);

            holdSets[i].erase(holdSets[i].find(back));
            holdSets[i].emplace(val);
        }
    }
}

void PowerSpectrum::renderLpc(double maximumFrequency, SpecFrame lpcSpectrum)
{
    std::lock_guard<std::mutex> guard(imageLock);

    const double fs = lpcSpectrum.fs;
    const int nfft = lpcSpectrum.nfft;
    const auto& spec = lpcSpectrum.spec;

    const double delta = fs / (2.0 * nfft);

    int minGain = canvas->getMinGainSpectrum();
    int maxGain = canvas->getMaxGainSpectrum();

    QPainter painter(&lpcIm);
    painter.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing | QPainter::SmoothPixmapTransform);

    QPainterPath path;

    for (int i = 0; i < nfft; ++i) {
        double freq = i * delta;

        if (freq > maximumFrequency) {
            break;
        }

        double gain = std::clamp<double>(20 * std::log10(abs(spec(i))) - 15, minGain - 20, maxGain + 20);

        int x = targetWidth - (targetWidth * (maxGain - gain)) / (maxGain - minGain);
        int y = canvas->yFromFrequency(freq, maximumFrequency);

        if (i == 0) {
            path.moveTo(x, y);
        }
        else {
            path.lineTo(x, y);
        }
    }

    painter.setPen(QPen(QColor(0xC0C0C0), 2));
    painter.drawPath(path);
}

void PowerSpectrum::paintEvent(QPaintEvent * event)
{
    targetWidth = width();
    targetHeight = height();

    painter.begin(this);

    painter.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing | QPainter::SmoothPixmapTransform);
    painter.fillRect(0, 0, targetWidth, targetHeight, Qt::black);

    imageLock.lock();
    painter.drawPicture(0, 0, lpcIm);
    painter.drawPicture(0, 0, spectrum);
    imageLock.unlock();

    const double maxFreq = analyser->getMaximumFrequency();
    const double freq = canvas->getSelectedFrequency();
    const int y = canvas->yFromFrequency(freq, maxFreq);

    painter.setPen(QPen(QColor(0x7F7F7F), 1));
    painter.drawLine(0, y, targetWidth, y);

    painter.end();
}


#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <limits>

int main(int argc, char *argv[])
{
	using type = boost::multiprecision::cpp_dec_float_100;
	type var;

	var = 1;
	for(int i = 0; i < 20; i++)
		var *= 100;

	std::cout << var << std::endl;
	std::cout << boost::multiprecision::log(var) << std::endl;
	std::cout << std::numeric_limits<type>::max() << std::endl;

	return 0;
}


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

This implements two histogram (character frequency) based filters. The histogram of a
word is an array that gives the number of times each possible letter in the alphabet
occurs inside a certain word. For the word "food", the array would be

a b c d e f g h ... o ... z
0 0 0 1 0 1 0 0 ... 2 ... 0

We define the delta d between two such histogram arrays as the sum of all absolute
differences of the counts of each pair of characters. Shortened example, see deltaSlow()
for the algorithm:

vec a 0 0 1 3 2 1
vec b 0 1 0 3 2 0
delta 0+1+1+0+0+1

The filter can be used for exact matches, hamming distance and edit distance although
it is only beneficial for edit distnace as matching exact words and hamming distance if
comparatively cheap anyway. For two words to be within edit distance t, their frequency
delta must be <= 2*t. This can be improved by looking at the possible edit operations. If
the length of the two words differs, that can only be caused by insert or delete operations.
These operations change the delta by 1, not 2 like substitution. Therefore, the criterion can
be improved to <= 2*t-lengthdiff where lengthdiff is the absolute length difference
between the two words.

While not overly costly, the naive way of executing the filter does not yield a sufficient
result. We therefore use SSE instructions to impove the filter by about a factor of 3. Also,
we have a shortened version of the filter which combines some characters. Its pruning
capability is not as good as the full version of the filter but it can be executed on one
sse register instead of two.
*/

#pragma once

#include <boost/utility/string_ref.hpp>
#include <cassert>
#include <x86intrin.h>

namespace campers {

/// The full 26 character frequency filter
struct Frequency {
    /// Maps each letter in the alphabet to a slot inside two sse 16 byte registers. A good mapping for english words
    /// was obtained by randomized search while monitoring the prune rate using a script such as:
    /// #!/bin/bash
    /// while true; do
    ///     seq 0 31 | sort -R | head -n 26 | xargs | tr ' ' ',' > include/mapping.hpp
    ///     make -j >/dev/null 2>&1 && ./driver test_data/inter* | grep prune | awk '{ print $2 }' | tr '\n' ' ' | cat - include/mapping.hpp
    /// done
    ///
    /// Alternatively, a good but not ideal mapping is simply mapping a-m to the first register and n-z to the second.
    static constexpr unsigned char charMap[26]={ 24,26,22,2,13,20,14,1,30,27,23,16,5,6,8,4,3,9,25,7,29,28,31,17,21,15 };
    /// Union trick. We use two sse 128 bit registers to represent 32 char values for 26 characters
    union { uint64_t l[4]; unsigned char f[32]; __m128i p[2]; };

    /// Comparison for adding Frequency to maps
    bool operator<(const Frequency& other) const {
        return std::lexicographical_compare(l,l+4,other.l,other.l+4);
    }

    /// Comparison for adding Frequency to hash maps
    bool operator==(const Frequency& other) const {
        return l[0]==other.l[0]&&l[1]==other.l[1]&&l[2]==other.l[2]&&l[3]==other.l[3];
    }

    /// Constructor
    Frequency() {}

    /// Constructor which builds the frequency vector for a given word
    explicit Frequency(const boost::string_ref& word) {
        for (unsigned index=0;index!=32;++index) f[index]=0;
        for (unsigned index=0;index<word.length();++index)
            ++f[charMap[word[index]-'a']];
    }

    /// Delta computation using the naive mechanism adding the absolute of each character count difference
    unsigned deltaSlow(const Frequency& f2) const {
        unsigned result=0; for (unsigned index=0;index!=26;++index)
        if (f[index]<f2.f[index])
            result+=f2.f[index]-f[index];
        else
            result+=f[index]-f2.f[index];
        return result;
    }

    /// Get coefficient
    unsigned getCoefficient() const {
        auto sum1=_mm_sad_epu8(p[0],_mm_setzero_si128());
        auto fsum1=_mm_extract_epi64(_mm_add_epi8(sum1,_mm_srli_si128(sum1,8)),0);
        auto sum2=_mm_sad_epu8(p[1],_mm_setzero_si128());
        auto fsum2=_mm_extract_epi64(_mm_add_epi8(sum2,_mm_srli_si128(sum2,8)),0);
        return 32+fsum1-fsum2;
    }

    /// Delta computation using sse, the sse instruction compute the same result as the naive approach but
    /// in less cycles.
    unsigned delta(const Frequency& f2) const {
        __m128i half=_mm_add_epi8(_mm_sad_epu8(p[0],f2.p[0]),_mm_sad_epu8(p[1],f2.p[1]));
        return _mm_extract_epi64(_mm_add_epi8(half,_mm_srli_si128(half,8)),0);
    }
};

/// The frequency filter which maps 26 characters on 16 slots so that it can be evaluated on one sse
/// register instead of two. The mapping is constructed from a dictionary such that letters which do
/// not occur together frequently are mapped to the same slot.
struct FrequencyFast {
    /// A mapping compacting the 26 characters of the alphabet into 16 slots of one sse register. The mapping
    /// was extracted similarly to that of the regular Frequency filter.
    static constexpr unsigned char charMap[26]={13,0,1,14,9,2,3,4,5,5,6,0,7,15,2,8,9,12,11,6,10,11,12,13,14,15};
    /// Union trick. We use two sse 128 bit registers to represent 32 char values for 26 characters
    union { uint64_t l[2]; unsigned char f[16]; __m128i p; };

    /// Comparison for adding Frequency to maps
    bool operator<(const FrequencyFast& other) const {
        return _mm_extract_epi64(p,1) < _mm_extract_epi64(other.p,1) ||
        (_mm_extract_epi64(p,1) == _mm_extract_epi64(other.p,1) && _mm_extract_epi64(p,0) < _mm_extract_epi64(other.p,0));
    }

    /// Comparison for adding Frequency to hash maps
    bool operator==(const FrequencyFast& other) const {
        return l[0]==other.l[0]&&l[1]==other.l[1];
    }

    /// Constructor
    FrequencyFast() {}

    /// Constructor which builds the frequency vector for a given word
    explicit FrequencyFast(const boost::string_ref& word) {
        for (unsigned index=0;index!=16.;++index) f[index]=0;
        for (unsigned index=0;index<word.length();++index)
            ++f[charMap[word[index]-'a']];
    }

    /// Delta computation using the naive mechanism adding the absolute of each character count difference
    unsigned deltaSlow(const FrequencyFast& f2) const {
        unsigned result=0; for (unsigned index=0;index!=16;++index)
        if (f[index]<f2.f[index])
            result+=f2.f[index]-f[index];
        else
            result+=f[index]-f2.f[index];
        return result;
    }

    /// Delta computation using sse, the sse instruction compute the same result as the naive approach but
    /// in less cycles.
    unsigned delta(const FrequencyFast& f2) const {
        __m128i half=_mm_sad_epu8(p,f2.p);
        return _mm_extract_epi64(_mm_add_epi8(half,_mm_srli_si128(half,8)),0);
    }
};

}

namespace std {

/// Hash for FrequencyFast crc
template<>
struct hash<campers::FrequencyFast> {
    size_t operator()(const campers::FrequencyFast& f) const {
        return _mm_crc32_u64(_mm_crc32_u64(0,reinterpret_cast<const uint64_t*>(&f)[0]),reinterpret_cast<const uint64_t*>(&f)[1]);
    }
};

/// Hash for Frequency using crc
template<>
struct hash<campers::Frequency> {
    size_t operator()(const campers::Frequency& f) const {
        return _mm_crc32_u64(_mm_crc32_u64(_mm_crc32_u64(_mm_crc32_u64(0,reinterpret_cast<const uint64_t*>(&f)[0]),
            reinterpret_cast<const uint64_t*>(&f)[1]),reinterpret_cast<const uint64_t*>(&f)[2]),(reinterpret_cast<const uint64_t*>(&f)[3]));
    }
};
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/weighted_histogram.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(
  scitbx::weighted_histogram<>)

namespace scitbx { namespace af { namespace boost_python { namespace {

  struct weighted_histogram_wrappers
  {
    typedef weighted_histogram<> w_t;
    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("weighted_histogram", no_init)
        .def(init<
          af::const_ref<double> const&,
          std::size_t>((
            arg("data"),
            arg("n_slots")=1000)))
        .def(init<
          af::const_ref<double> const&,
          af::const_ref<double> const&,
          std::size_t>((
            arg("data"),
            arg("weights"),
            arg("n_slots")=1000)))
        .def(init<
          af::const_ref<double> const&,
          double const&,
          double const&,
          std::size_t,
          double const&>((
            arg("data"),
            arg("data_min"),
            arg("data_max"),
            arg("n_slots")=1000,
            arg("relative_tolerance")=1e-4)))
        .def(init<
          af::const_ref<double> const&,
          af::const_ref<double> const&,
          double const&,
          double const&,
          std::size_t,
          double const&>((
            arg("data"),
            arg("weights"),
            arg("data_min"),
            arg("data_max"),
            arg("n_slots")=1000,
            arg("relative_tolerance")=1e-4)))
        .def(init<
          w_t const&,
          af::const_ref<double> const&,
          double const&>((
            arg("other"),
            arg("data"),
            arg("relative_tolerance")=1e-4)))
        .def(init<
          w_t const&,
          af::const_ref<double> const&,
          af::const_ref<double> const&,
          double const&>((
            arg("other"),
            arg("data"),
            arg("weights"),
            arg("relative_tolerance")=1e-4)))
        .enable_pickling()
        .def(init<
          double const&,
          double const&,
          double const&,
          af::shared<double> const&,
          std::size_t>()) // intentionally no arg
        .def("update",
          (void(w_t::*)(double const&, double const&)) &w_t::update, (
          arg("data_value"),
          arg("relative_tolerance")=1e-4))
        .def("update",
             (void(w_t::*)(double const&, double const&, double const&)) &w_t::update, (
          arg("data_value"),
          arg("weight"),
          arg("relative_tolerance")=1e-4))
        .def("update",
          (void(w_t::*)(w_t const&)) &w_t::update, (
          arg("other")))
        .def("update",
          (void(w_t::*)(af::shared<double> const&)) &w_t::update, (
          arg("from_ar")))

        .def("data_min", &w_t::data_min)
        .def("data_max", &w_t::data_max)
        .def("slot_width", &w_t::slot_width)
        .def("slot_centers", &w_t::slot_centers)
        .def("slots", &w_t::slots)
        .def("get_i_slot", &w_t::get_i_slot)
        .def("n_out_of_slot_range", &w_t::n_out_of_slot_range)
        .def("get_cutoff", &w_t::get_cutoff, (
          arg("max_points"),
          arg("relative_tolerance")=1e-4))
      ;
    }
  };

} // namespace <anonymous>

  void wrap_flex_weighted_histogram()
  {
    weighted_histogram_wrappers::wrap();
  }

}}} // namespace scitbx::af::boost_python


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_CONSTRAINT_INTERFACE_H_
#define TURI_CONSTRAINT_INTERFACE_H_

#include <string>
#include <flexible_type/flexible_type.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// Optimizaiton
#include <optimization/optimization_interface.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup optimization_constraints Optimization Constraints
 * \{
 */

/**
 *
 * Interface for constraints for Gradient Projection solvers. See chapter 12
 * of (1) for an intro to constrained optimization.
 *
 * Some implementations are based on Section 16.7 of (1).
 *
 *
 * Background: Gradient Projection Methods
 * --------------------------------------------------------------------------
 *
 * Gradient project methods are methods for solving bound constrained
 * optimization problems.
 *
 * Traditionally, in unconstrained optimization, we solve the problem
 *  \min_x f(x) using a gradient descent method as follows:
 *
 *              x_{k+1} = x_{k} - \alpha_k \grad f(x_k)             (G)
 *
 *   where \alpha_k is a step size.
 *
 *  The gradient-projection framework performs solves the problem:
 *  \min_{x \in C} f(x) using a slight modification to the gradient step in (G)
 *
 *          x_{k+1} =  P_C(x_{k} - \alpha_k \grad f(x_k))       (PG)
 *
 *   where P_C is the projection of a point on to the convex set.
 *
 *       P_C(z) = \min_{x \in z} ||x-z||^2
 *
 *   which works out to be the closest point to the set in C.
 *
 * Comparison of gradient projection with other methods
 * -----------------------------------------------------------
 *
 * In solving bound constrained optimization problems, active set methods face
 * criticism because the working set changes slowly; at each iteration, at most
 * one constraint is added to or dropped from the working set. If there are k0
 * constraints active at the initial W0, but kθ constraints active at the
 * solution, then at least |kθ−k0| iterations are required for convergence.
 * This property can be a serious disadvantage in large problems if the working
 * set at the starting point is vastly different from the active set at the
 * solution.
 *
 *
 * The gradient-projection method is guaranteed to identify the active set at
 * a solution in a finite number of iterations.  After it has identified the
 * correct active set, the gradient-projection algorithm reduces to the
 * steepest-descent algorithm on the subspace of free variables.
 *
 * References:
 *
 * (1) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999. (Chapter 12)
 *
 *
*/
class constraint_interface {

  public:

  /**
   * Default desctuctor.
   */
  virtual ~constraint_interface() {};

  /**
   * Project a dense point into the constraint space.
   * \param[in,out]  point   Point which we are using to project the point.
   *
   * Given a convex set X, the projection operator is given by
   *
   *     P(y) = \min_{x \in X} || x - y ||^2
   *
   */
  virtual void project(DenseVector &point) const = 0;

  /**
   * Project a dense a block of co-ordinates point into the constraint space.
   *
   * \param[in,out]  point        A block project the point.
   * \param[in]      block_start  Start index of the block
   * \param[in]      block_size   Size of the block
   *
   * Given a convex set X, the projection operator is given by
   *
   *     P(y) = \min_{x \in X} || x - y ||^2
   *
   */
  virtual void project_block(DenseVector &point, const size_t block_start,
      const size_t block_size) const = 0;

  /**
   * Boolean function to determine if a dense point is present in a constraint
   * space.
   * \param[in]  point   Point which we are querying.
   *
   */
  virtual bool is_satisfied(const DenseVector &point) const = 0;

  /**
   * A measure of the first order optimality conditions.
   *
   * \param[in]  point    Point which we are querying.
   * \param[in]  gradient Gradient at that point for a given function
   *
   *  If you don't know what to do, then use
   *                ||P_C(x - grad) - x||
   * where x is the point, grad is the gradient and P_C is the projection to
   * the set in consideration.
   *
   */
  virtual double first_order_optimality_conditions(const DenseVector &point,
                                        const DenseVector& gradient) const = 0;

};

/// \}
} // Optimization
} // Turi

#endif


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/boundary_operator.hpp"
#include "assembly/context.hpp"
#include "assembly/modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "assembly/modified_helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"

#include "common/boost_make_shared_fwd.hpp"

#include "fiber/basis_transformation_functor_wrappers.hpp"
#include "fiber/default_collection_of_basis_transformations.hpp"
#include "fiber/geometrical_data.hpp"
#include "fiber/piecewise_linear_continuous_scalar_basis.hpp"
#include "fiber/surface_curl_3d_functor.hpp"

#include "grid/grid_factory.hpp"
#include "grid/grid.hpp"
#include "grid/grid_view.hpp"
#include "grid/entity_iterator.hpp"
#include "grid/entity.hpp"
#include "grid/geometry.hpp"

#include "space/piecewise_linear_continuous_scalar_space.hpp"
#include "space/piecewise_constant_scalar_space.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

// Tests

using namespace Bempp;

BOOST_AUTO_TEST_SUITE(ModifiedHelmholtz3dHypersingularBoundaryOperator)

BOOST_AUTO_TEST_CASE_TEMPLATE(works_for_complex_kernel, BasisFunctionType, basis_function_types)
{
    typedef BasisFunctionType BFT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType RT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType KT;
    typedef typename Fiber::ScalarTraits<BFT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "meshes/two_disjoint_triangles.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setAbsoluteQuadratureOrder(5);
    accuracyOptions.doubleSingular.setAbsoluteQuadratureOrder(5);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const KT waveNumber(1.23, 0.31);

    BoundaryOperator<BFT, RT> slpOpConstants =
            Bempp::modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseConstants),
                waveNumber);
    BoundaryOperator<BFT, RT> slpOpLinears =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber);
    BoundaryOperator<BFT, RT> hypOp =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber);

    // Get the matrix repr. of the hypersingular operator
    arma::Mat<RT> hypMat = hypOp.weakForm()->asMatrix();

    // Construct the expected hypersingular operator matrix. For this, we need:

    // * the surface curls of all basis functions (which are constant)
    typedef Fiber::SurfaceCurl3dElementaryFunctor<CT> ElementaryFunctor;
    typedef Fiber::ElementaryBasisTransformationFunctorWrapper<ElementaryFunctor> Functor;
    Functor functor;
    size_t basisDeps = 0, geomDeps = 0;
    functor.addDependencies(basisDeps, geomDeps);

    arma::Mat<CT> points(2, 1);
    points.fill(0.);

    typedef Fiber::PiecewiseLinearContinuousScalarBasis<3, BFT> Basis;
    Basis basis;
    Fiber::BasisData<BFT> basisData;
    basis.evaluate(basisDeps, points, Fiber::ALL_DOFS, basisData);

    Fiber::GeometricalData<CT> geomData[2];
    std::auto_ptr<GridView> view = grid->leafView();
    std::auto_ptr<EntityIterator<0> > it = view->entityIterator<0>();
    it->entity().geometry().getData(geomDeps, points, geomData[0]);
    it->next();
    it->entity().geometry().getData(geomDeps, points, geomData[1]);

    Fiber::DefaultCollectionOfBasisTransformations<Functor> transformations(functor);

    Fiber::CollectionOf3dArrays<BFT> surfaceCurl[2];
    transformations.evaluate(basisData, geomData[0], surfaceCurl[0]);
    transformations.evaluate(basisData, geomData[1], surfaceCurl[1]);

    // * the single-layer potential matrix for constant basis functions
    arma::Mat<RT> slpMatConstants = slpOpConstants.weakForm()->asMatrix();
    // * the single-layer potential matrix for linear basis functions
    arma::Mat<RT> slpMatLinears = slpOpLinears.weakForm()->asMatrix();

    arma::Mat<RT> expectedHypMat(6, 6);
    for (size_t testElement = 0; testElement < 2; ++testElement)
        for (size_t trialElement = 0; trialElement < 2; ++trialElement)
            for (size_t r = 0; r < 3; ++r)
                for (size_t c = 0; c < 3; ++c) {
                    RT curlMultiplier = 0.;
                    for (size_t dim = 0; dim < 3; ++dim)
                        curlMultiplier += surfaceCurl[testElement][0](dim, r, 0) *
                                surfaceCurl[trialElement][0](dim, c, 0);
                    RT valueMultiplier = 0.;
                    for (size_t dim = 0; dim < 3; ++dim)
                        valueMultiplier += geomData[testElement].normals(dim, 0) *
                            geomData[trialElement].normals(dim, 0);
                    expectedHypMat(3 * testElement + r, 3 * trialElement + c) =
                            curlMultiplier * slpMatConstants(testElement, trialElement) +
                            waveNumber * waveNumber * valueMultiplier *
                            slpMatLinears(3 * testElement + r, 3 * trialElement + c);
                }

    BOOST_CHECK(check_arrays_are_close<RT>(expectedHypMat, hypMat, 1e-6));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(works_for_real_kernel, BasisFunctionType, basis_function_types)
{
    typedef BasisFunctionType BFT;
    typedef BasisFunctionType RT;
    typedef typename Fiber::ScalarTraits<BFT>::RealType KT;
    typedef typename Fiber::ScalarTraits<BFT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "meshes/two_disjoint_triangles.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setAbsoluteQuadratureOrder(5);
    accuracyOptions.doubleSingular.setAbsoluteQuadratureOrder(5);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const KT waveNumber = 1.23;

    BoundaryOperator<BFT, RT> slpOpConstants =
            Bempp::modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseConstants),
                make_shared_from_ref(pwiseConstants),
                waveNumber);
    BoundaryOperator<BFT, RT> slpOpLinears =
            modifiedHelmholtz3dSingleLayerBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber);
    BoundaryOperator<BFT, RT> hypOp =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber);

    // Get the matrix repr. of the hypersingular operator
    arma::Mat<RT> hypMat = hypOp.weakForm()->asMatrix();

    // Construct the expected hypersingular operator matrix. For this, we need:

    // * the surface curls of all basis functions (which are constant)
    typedef Fiber::SurfaceCurl3dElementaryFunctor<CT> ElementaryFunctor;
    typedef Fiber::ElementaryBasisTransformationFunctorWrapper<ElementaryFunctor> Functor;
    Functor functor;
    size_t basisDeps = 0, geomDeps = 0;
    functor.addDependencies(basisDeps, geomDeps);

    arma::Mat<CT> points(2, 1);
    points.fill(0.);

    typedef Fiber::PiecewiseLinearContinuousScalarBasis<3, BFT> Basis;
    Basis basis;
    Fiber::BasisData<BFT> basisData;
    basis.evaluate(basisDeps, points, Fiber::ALL_DOFS, basisData);

    Fiber::GeometricalData<CT> geomData[2];
    std::auto_ptr<GridView> view = grid->leafView();
    std::auto_ptr<EntityIterator<0> > it = view->entityIterator<0>();
    it->entity().geometry().getData(geomDeps, points, geomData[0]);
    it->next();
    it->entity().geometry().getData(geomDeps, points, geomData[1]);

    Fiber::DefaultCollectionOfBasisTransformations<Functor> transformations(functor);

    Fiber::CollectionOf3dArrays<BFT> surfaceCurl[2];
    transformations.evaluate(basisData, geomData[0], surfaceCurl[0]);
    transformations.evaluate(basisData, geomData[1], surfaceCurl[1]);

    // * the single-layer potential matrix for constant basis functions
    arma::Mat<RT> slpMatConstants = slpOpConstants.weakForm()->asMatrix();
    // * the single-layer potential matrix for linear basis functions
    arma::Mat<RT> slpMatLinears = slpOpLinears.weakForm()->asMatrix();

    arma::Mat<RT> expectedHypMat(6, 6);
    for (size_t testElement = 0; testElement < 2; ++testElement)
        for (size_t trialElement = 0; trialElement < 2; ++trialElement)
            for (size_t r = 0; r < 3; ++r)
                for (size_t c = 0; c < 3; ++c) {
                    RT curlMultiplier = 0.;
                    for (size_t dim = 0; dim < 3; ++dim)
                        curlMultiplier += surfaceCurl[testElement][0](dim, r, 0) *
                                surfaceCurl[trialElement][0](dim, c, 0);
                    RT valueMultiplier = 0.;
                    for (size_t dim = 0; dim < 3; ++dim)
                        valueMultiplier += geomData[testElement].normals(dim, 0) *
                            geomData[trialElement].normals(dim, 0);
                    expectedHypMat(3 * testElement + r, 3 * trialElement + c) =
                            curlMultiplier * slpMatConstants(testElement, trialElement) +
                            waveNumber * waveNumber * valueMultiplier *
                            slpMatLinears(3 * testElement + r, 3 * trialElement + c);
                }

    BOOST_CHECK(check_arrays_are_close<RT>(expectedHypMat, hypMat, 1e-6));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(interpolated_matches_noniterpolated,
                              BasisFunctionType, basis_function_types)
{
    typedef BasisFunctionType BFT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType RT;
    typedef typename Fiber::ScalarTraits<BFT>::ComplexType KT;
    typedef typename Fiber::ScalarTraits<BFT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
                params, "meshes/two_disjoint_triangles.msh",
                false /* verbose */);

    PiecewiseLinearContinuousScalarSpace<BFT> pwiseLinears(grid);
    PiecewiseConstantScalarSpace<BFT> pwiseConstants(grid);

    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setAbsoluteQuadratureOrder(5);
    accuracyOptions.doubleSingular.setAbsoluteQuadratureOrder(5);
    NumericalQuadratureStrategy<BFT, RT> quadStrategy(accuracyOptions);

    Context<BFT, RT> context(make_shared_from_ref(quadStrategy), assemblyOptions);

    const KT waveNumber(3.23, 0.31);

    BoundaryOperator<BFT, RT> opNoninterpolated =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", NO_SYMMETRY,
                false);
    BoundaryOperator<BFT, RT> opInterpolated =
            modifiedHelmholtz3dHypersingularBoundaryOperator<BFT, KT, RT>(
                make_shared_from_ref(context),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                make_shared_from_ref(pwiseLinears),
                waveNumber,
                "", NO_SYMMETRY,
                true);

    arma::Mat<RT> matNoninterpolated = opNoninterpolated.weakForm()->asMatrix();
    arma::Mat<RT> matInterpolated = opInterpolated.weakForm()->asMatrix();

    const CT eps = std::numeric_limits<CT>::epsilon();
    BOOST_CHECK(check_arrays_are_close<RT>(
                    matNoninterpolated, matInterpolated, 100 * eps));
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [tenpower]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 4>;
using pack_it = bs::pack <std::int32_t,4>;

int main()
{
  pack_it pi = {-1,    2,    -3,    4   };

  std::cout
    << "---- simd" << '\n'
    << "<- pi =               " << pi << '\n'
    << "-> bs::tenpower(pi) = " << bs::tenpower(pi) << '\n';

  std::int32_t xi =  4;

  std::cout
    << "---- scalar"  << '\n'
    << "<- xi =               " << xi << '\n'
    << "-> bs::tenpower(xi) = " << bs::tenpower(xi) << '\n';
  return 0;
}
//! [tenpower]


#include <Eigen/Dense>
#include <aslam/Exceptions.hpp>
#include <aslam/cameras/OmniCameraGeometry.hpp>

namespace aslam {
namespace cameras {

OmniCameraGeometry::OmniCameraGeometry() { *this = createTestGeometry(); }

OmniCameraGeometry::OmniCameraGeometry(double xi, double k1, double k2, double p1, double p2, double gamma1,
                                       double gamma2, double u0, double v0, int width, int height)
    : _xi(xi),
      _k1(k1),
      _k2(k2),
      _p1(p1),
      _p2(p2),
      _gamma1(gamma1),
      _gamma2(gamma2),
      _u0(u0),
      _v0(v0),
      _width(width),
      _height(height) {
    updateTemporaries();
}

OmniCameraGeometry::~OmniCameraGeometry() {}

// This updates the intrinsic parameters with a small step: i <-- i + di
// The Jacobians above are with respect to this update function.
void OmniCameraGeometry::updateIntrinsicsOplus(double* di) {
    _xi += di[0];
    _k1 += di[1];
    _k2 += di[2];
    _p1 += di[3];
    _p2 += di[4];
    _gamma1 += di[5];
    _gamma2 += di[6];
    _u0 += di[7];
    _v0 += di[8];
    updateTemporaries();
}

// The amount of time elapsed between the start of the image and the
// keypoint. For a global shutter camera, this can return Duration(0).
Duration OmniCameraGeometry::temporalOffset(const keypoint_t& keypoint) const { return Duration(0); }

OmniCameraGeometry::keypoint_t OmniCameraGeometry::maxKeypoint() const { return keypoint_t(_width, _height); }

OmniCameraGeometry::keypoint_t OmniCameraGeometry::minKeypoint() const { return keypoint_t(0, 0); }

std::string OmniCameraGeometry::typeName() const { return "OmniCamera"; }

OmniCameraGeometry OmniCameraGeometry::createTestGeometry() {
    return OmniCameraGeometry(1.2080, -2.103030975849235e-01, 1.511327079893021e-02, 6.035288960688402e-04,
                              -5.277839371727245e-04, 5.671146836756682e+02, 5.665554204210736e+02,
                              6.196542975873804e+02, 5.106649931478771e+02, 1224, 1024);
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param u u image coordinate
 * \param v v image coordinate
 * \param X X coordinate of the point on the sphere
 * \param Y Y coordinate of the point on the sphere
 * \param Z Z coordinate of the point on the sphere
 */
void OmniCameraGeometry::lift_sphere(double u, double v, double* X, double* Y, double* Z) const {
    double mx_d, my_d, mx_u, my_u;
    double lambda;

    // Lift points to normalised plane
    // Matlab points start at 1 (calibration)
    mx_d = _inv_K11 * (u) + _inv_K13;
    my_d = _inv_K22 * (v) + _inv_K23;

    // Recursive distortion model
    // int n = 6;
    // double dx_u, dy_u;
    // distortion(mx_d,my_d,&dx_u,&dy_u);
    // // Approximate value
    // mx_u = mx_d-dx_u;
    // my_u = my_d-dy_u;

    // for(int i=1;i<n;i++) {
    // 	distortion(mx_u,my_u,&dx_u,&dy_u);
    // 	mx_u = mx_d-dx_u;
    // 	my_u = my_d-dy_u;
    // }
    // PTF: March 31, 2012. The above was not that accurate.
    //      Substitute Gauss-Newton.
    undistortGN(mx_d, my_d, &mx_u, &my_u);

    // Lift normalised points to the sphere (inv_hslash)
    lambda = (_xi + sqrt(1 + (1 - _xi * _xi) * (mx_u * mx_u + my_u * my_u))) / (1 + mx_u * mx_u + my_u * my_u);
    *X = lambda * mx_u;
    *Y = lambda * my_u;
    *Z = lambda - _xi;
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param u u image coordinate
 * \param v v image coordinate
 * \param X X coordinate of the projective ray
 * \param Y Y coordinate of the projective ray
 * \param Z Z coordinate of the projective ray
 */
void OmniCameraGeometry::lift_projective(double u, double v, double* X, double* Y, double* Z) const {
    double mx_d, my_d, mx_u, my_u;
    double rho2_d;

    // Lift points to normalised plane
    // Matlab points start at 1 (calibration)
    mx_d = _inv_K11 * (u) + _inv_K13;
    my_d = _inv_K22 * (v) + _inv_K23;

    // Recursive distortion model
    // int n = 8;
    // double dx_u, dy_u;
    // distortion(mx_d,my_d,&dx_u,&dy_u);
    // // Approximate value
    // mx_u = mx_d-dx_u;
    // my_u = my_d-dy_u;

    // for(int i=1;i<n;i++) {
    // 	distortion(mx_u,my_u,&dx_u,&dy_u);
    // 	mx_u = mx_d-dx_u;
    // 	my_u = my_d-dy_u;
    // }
    // PTF: March 31, 2012. The above was not that accurate.
    //      Substitute Gauss-Newton.
    undistortGN(mx_d, my_d, &mx_u, &my_u);

    // std::cout << "lift projective: u: " << mx_u << ", v: " << my_u << std::endl;

    // Obtain a projective ray
    // Reuse variable
    rho2_d = mx_u * mx_u + my_u * my_u;
    *X = mx_u;
    *Y = my_u;
    *Z = 1 - _xi * (rho2_d + 1) / (_xi + sqrt(1 + (1 - _xi * _xi) * rho2_d));

    // std::cout << "lift projective: p: " << *X << ", " << *Y << ", " << *Z << std::endl;
}

/**
 * \brief Project a 3D points (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param x 3D point x coordinate
 * \param y 3D point y coordinate
 * \param z 3D point z coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::space2plane(double x, double y, double z, double* u, double* v) const {
    double mx_u, my_u, mx_d, my_d;

    // Project points to the normalised plane
    z = z + _xi * sqrt(x * x + y * y + z * z);
    mx_u = x / z;
    my_u = y / z;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Project a 3D points (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *        and calculate jacobian
 *
 * \param x 3D point x coordinate
 * \param y 3D point y coordinate
 * \param z 3D point z coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::space2plane(double x, double y, double z, double* u, double* v, double* dudx, double* dvdx,
                                     double* dudy, double* dvdy, double* dudz, double* dvdz) const {
    double mx_u, my_u, mx_d, my_d;
    double norm, inv_denom;
    double dxdmx, dydmx, dxdmy, dydmy;

    norm = sqrt(x * x + y * y + z * z);
    // Project points to the normalised plane
    inv_denom = 1 / (z + _xi * norm);
    mx_u = inv_denom * x;
    my_u = inv_denom * y;

    // Calculate jacobian
    inv_denom = inv_denom * inv_denom / norm;
    *dudx = inv_denom * (norm * z + _xi * (y * y + z * z));
    *dvdx = -inv_denom * _xi * x * y;
    *dudy = *dvdx;
    *dvdy = inv_denom * (norm * z + _xi * (x * x + z * z));
    inv_denom = inv_denom * (-_xi * z - norm);  // reuse variable
    *dudz = x * inv_denom;
    *dvdz = y * inv_denom;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u, &dxdmx, &dydmx, &dxdmy, &dydmy);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Make the product of the jacobians
    // and add projection matrix jacobian
    inv_denom = _gamma1 * (*dudx * dxdmx + *dvdx * dxdmy);  // reuse
    *dvdx = _gamma2 * (*dudx * dydmx + *dvdx * dydmy);
    *dudx = inv_denom;

    inv_denom = _gamma1 * (*dudy * dxdmx + *dvdy * dxdmy);  // reuse
    *dvdy = _gamma2 * (*dudy * dydmx + *dvdy * dydmy);
    *dudy = inv_denom;

    inv_denom = _gamma1 * (*dudz * dxdmx + *dvdz * dxdmy);  // reuse
    *dvdz = _gamma2 * (*dudz * dydmx + *dvdz * dydmy);
    *dudz = inv_denom;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Projects an undistorted 2D point (\a mx_u,\a my_u) to the image plane in (\a u,\a v)
 *
 * \param mx_u 2D point x coordinate
 * \param my_u 3D point y coordinate
 * \param u return value, contains the image point u coordinate
 * \param v return value, contains the image point v coordinate
 */
void OmniCameraGeometry::undist2plane(double mx_u, double my_u, double* u, double* v) const {
    double mx_d, my_d;

    // Apply distortion
    double dx_u, dy_u;
    distortion(mx_u, my_u, &dx_u, &dy_u);
    mx_d = mx_u + dx_u;
    my_d = my_u + dy_u;

    // Apply generalised projection matrix
    // Matlab points start at 1
    *u = _gamma1 * mx_d + _u0;
    *v = _gamma2 * my_d + _v0;
}

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *
 * \param mx_u undistorted x coordinate of point on the normalised plane
 * \param my_u undistorted y coordinate of point on the normalised plane
 * \param dx return value, to obtain the distorted point : mx_d = mx_u+dx_u
 * \param dy return value, to obtain the distorted point : my_d = my_u+dy_u
 */
void OmniCameraGeometry::distortion(double mx_u, double my_u, double* dx_u, double* dy_u) const {
    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = mx_u * mx_u;
    my2_u = my_u * my_u;
    mxy_u = mx_u * my_u;
    rho2_u = mx2_u + my2_u;
    rad_dist_u = _k1 * rho2_u + _k2 * rho2_u * rho2_u;
    *dx_u = mx_u * rad_dist_u + 2 * _p1 * mxy_u + _p2 * (rho2_u + 2 * mx2_u);
    *dy_u = my_u * rad_dist_u + 2 * _p2 * mxy_u + _p1 * (rho2_u + 2 * my2_u);
}

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *        and calculate jacobian
 *
 * \param mx_u undistorted x coordinate of point on the normalised plane
 * \param my_u undistorted y coordinate of point on the normalised plane
 * \param dx return value, to obtain the distorted point : mx_d = mx_u+dx_u
 * \param dy return value, to obtain the distorted point : my_d = my_u+dy_u
 */
void OmniCameraGeometry::distortion(double mx_u, double my_u, double* dx_u, double* dy_u, double* dxdmx, double* dydmx,
                                    double* dxdmy, double* dydmy) const {
    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = mx_u * mx_u;
    my2_u = my_u * my_u;
    mxy_u = mx_u * my_u;
    rho2_u = mx2_u + my2_u;
    rad_dist_u = _k1 * rho2_u + _k2 * rho2_u * rho2_u;
    *dx_u = mx_u * rad_dist_u + 2 * _p1 * mxy_u + _p2 * (rho2_u + 2 * mx2_u);
    *dy_u = my_u * rad_dist_u + 2 * _p2 * mxy_u + _p1 * (rho2_u + 2 * my2_u);

    *dxdmx = 1 + rad_dist_u + _k1 * 2 * mx2_u + _k2 * rho2_u * 4 * mx2_u + 2 * _p1 * my_u + 6 * _p2 * mx_u;
    *dydmx = _k1 * 2 * mx_u * my_u + _k2 * 4 * rho2_u * mx_u * my_u + _p1 * 2 * mx_u + 2 * _p2 * my_u;
    *dxdmy = *dydmx;
    *dydmy = 1 + rad_dist_u + _k1 * 2 * my2_u + _k2 * rho2_u * 4 * my2_u + 6 * _p1 * my_u + 2 * _p2 * mx_u;
}

void OmniCameraGeometry::updateTemporaries() {
    // Inverse camera projection matrix parameters
    _inv_K11 = 1.0 / _gamma1;
    _inv_K13 = -_u0 / _gamma1;
    _inv_K22 = 1.0 / _gamma2;
    _inv_K23 = -_v0 / _gamma2;
    _one_over_xixi_m_1 = 1.0 / (_xi * _xi - 1.0);
}

void OmniCameraGeometry::setIntrinsicsVectorImplementation(const Eigen::VectorXd& V) {
    _xi = V[0];
    _k1 = V[1];
    _k2 = V[2];
    _p1 = V[3];
    _p2 = V[4];
    _gamma1 = V[5];
    _gamma2 = V[6];
    _u0 = V[7];
    _v0 = V[8];
    updateTemporaries();
}

Eigen::VectorXd OmniCameraGeometry::getIntrinsicsVectorImplementation() const {
    Eigen::VectorXd V(9);
    V[0] = _xi;
    V[1] = _k1;
    V[2] = _k2;
    V[3] = _p1;
    V[4] = _p2;
    V[5] = _gamma1;
    V[6] = _gamma2;
    V[7] = _u0;
    V[8] = _v0;
    return V;
}

Eigen::Matrix3d OmniCameraGeometry::getCameraMatrix() const {
    Eigen::Matrix3d K;
    K << _gamma1, 0, _u0, 0, _gamma2, _v0, 0, 0, 1;
    return K;
}

Eigen::VectorXd OmniCameraGeometry::createRandomKeypoint() const {
    // This is tricky...The camera model defines a circle on the normalized image
    // plane and the projection equations don't work outside of it.
    // With some manipulation, we can see that, on the normalized image plane,
    // the edge of this circle is at u^2 + v^2 = 1/(xi^2 - 1)
    // So: this function creates keypoints inside this boundary.

    // Create a point on the normalized image plane inside the boundary.
    // This is not efficient, but it should be correct.

    Eigen::Vector2d u(width() + 1, height() + 1);

    while (u[0] < 0 || u[0] > width() - 1 || u[1] < 0 || u[1] > height() - 1) {
        u.setRandom();
        u = u - Eigen::Vector2d(0.5, 0.5);
        u /= u.norm();
        u *= ((double)rand() / (double)RAND_MAX) * _one_over_xixi_m_1;

        // Now we run the point through distortion and projection.
        // Apply distortion
        double dx_u, dy_u;
        distortion(u[0], u[1], &dx_u, &dy_u);
        double mx_d = u[0] + dx_u;
        double my_d = u[1] + dy_u;

        // Apply generalised projection matrix
        // Matlab points start at 1
        u[0] = _gamma1 * mx_d + _u0;
        u[1] = _gamma2 * my_d + _v0;
    }

    // I would like to do this below but it is way, way worse than above.
    // // The output
    // Eigen::Vector2d u;
    // // The output projected to the normalized image plane.
    // Eigen::Vector2d u_norm;

    // // The singularity radius minus an epsilon
    // double nRadius = 1.0/_one_over_xixi_m_1;

    // do
    // 	{
    // 	  // Create a random keypoint in the image.
    // 	  //u.setRandom();
    // 	  u[0] = ( (double)rand() / (double)RAND_MAX)*_width;
    // 	  u[1] = ( (double)rand() / (double)RAND_MAX)*_height;

    // 	  // Lift points to normalised plane
    // 	  // Matlab points start at 1 (calibration)
    // 	  double u0_d = _inv_K11*(u[0])+_inv_K13;
    // 	  double u1_d = _inv_K22*(u[1])+_inv_K23;
    // 	  undistortGN(u0_d, u1_d, &u_norm[0], &u_norm[1]);

    // 	}
    // while( u_norm.dot(u_norm) >= nRadius );

    return u;
}

// Use Gauss-Newton to undistort.
void OmniCameraGeometry::undistortGN(double u_d, double v_d, double* u, double* v) const {
    *u = u_d;
    *v = v_d;

    double ubar = u_d;
    double vbar = v_d;
    const int n = 5;
    Eigen::Matrix2d F;

    double hat_u_d;
    double hat_v_d;

    // void OmniCameraGeometry::distortion(double mx_u, double my_u,
    // 					  double *dx_u, double *dy_u,
    // 					  double *dxdmx, double *dydmx,
    // 					  double *dxdmy, double *dydmy) const
    for (int i = 0; i < n; i++) {
        distortion(ubar, vbar, &hat_u_d, &hat_v_d, &F(0, 0), &F(1, 0), &F(0, 1), &F(1, 1));

        Eigen::Vector2d e(u_d - ubar - hat_u_d, v_d - vbar - hat_v_d);
        Eigen::Vector2d du = (F.transpose() * F).inverse() * F.transpose() * e;

        ubar += du[0];
        vbar += du[1];

        if (e.dot(e) < 1e-15) break;
    }
    *u = ubar;
    *v = vbar;
}

/// \brief initialize the intrinsics based on one view of a gridded calibration target
/// \return true on success
///
/// These functions were developed with the help of Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
bool OmniProjection::initializeIntrinsics(const GridCalibrationTargetObservation& obs) {
    if (!obs.target()) {
        return false;
    }

    double square(double x) { return x * x; }
    float square(float x) { return x * x; }
    double hypot(double a, double b) { return sqrt(square(a) + square(b)); }

    // First, initialize the image center at the center of the image.
    _xi = 1.0;
    _cu = obs.imCols() / 2.0;
    _cv = obs.imRows() / 2.0;
    _ru = obs.imCols();
    _rv = obs.imRows();

    _distortion.clear();

    // Grab a reference to the target for easy access.
    const GridCalibrationTarget& target = *obs.target();

    /// Initialize some temporaries needed.
    double gamma0 = 0.0;
    double minReprojErr = std::numeric_limits<double>::max();

    // Now we try to find a non-radial line to initialize the focal length
    bool success = false;
    for (size_t r = 0; r < target.rows(); ++r) {
        // Grab all the valid corner points for this checkerboard observation
        cv::Mat P(target.cols(); 4, CV_64F);
        size_t count = 0;
        for (size_t c = 0; c < target.cols(); ++c) {
            Eigen::Vector2d imagePoint;
            Eigen::Vector3d gridPoint;
            if (obs.imageGridPoint(r, c, imagePoint)) {
                double u = imagePoint[0] - _cu;
                double v = imagePoint[1] - _cv;
                P.at<double>(count, 0) = u;
                P.at<double>(count, 1) = v;
                P.at<double>(count, 2) = 0.5;
                P.at<double>(count, 3) = -0.5 * (square(u) + square(v));
                ++count;
            }
        }

        const int MIN_CORNERS = 8;
        // MIN_CORNERS is an arbitrary threshold for the number of corners
        if (count > MIN_CORNERS) {
            // Resize P to fit with the count of valid points.
            cv::Mat C;
            cv::SVD::solveZ(P.colRange(0, count), C);

            double t = square(C.at<double>(0)) + square(C.at<double>(1)) + C.at<double>(2) * C.at<double>(3);
            if (t < 0) {
                continue;
            }

            // check that line image is not radial
            double d = sqrt(1.0 / t);
            double nx = C.at<double>(0) * d;
            double ny = C.at<double>(1) * d;
            if (hypot(nx, ny) > 0.95) {
                continue;
            }

            double nz = sqrt(1.0 - square(nx) - square(ny));
            double gamma = fabs(C.at<double>(2) * d / nz);

            _fu = gamma;
            _fv = gamma;
            sm::kinematics::Transformation T_target_camera;
            if (!estimateTransformation(obs, T_target_camera)) {
                continue;
            }

            double reprojErr = 0.0;
            size_t numReprojected = computeReprojectionError(obs, T_target_camera, reprojErr);

            if (numReprojected > MIN_CORNERS) {
                double avgReprojErr = reprojErr / numReprojected;

                if (avgReprojErr < minReprojErr) {
                    minReprojErr = avgReprojErr;
                    gamma0 = gamma;
                    success = true;
                }
            }

        }  // If this observation has enough valid corners
    }      // For each row in the image.

    _fu = gamma0;
    _fv = gamma0;

    return success;

}  // initializeIntrinsics()

bool OmniProjection::computeReprojectionError(const GridCalibrationTargetObservation& obs,
                                              const sm::kinematics::Transformation& T_target_camera,
                                              double& outErr) const {
    outErr = 0.0;
    size_t count = 0;
    sm::kinematics::Transformation T_camera_target = T_target_camera.inverse();

    for (size_t i = 0; i < obs.size(); ++i) {
        Eigen::Vector2d y, yhat;
        if (obs.imagePoint(i, y) && euclideanToKeypoint(T_camera_target * obs.target()->point(i), yhat)) {
            outErr += (y - yhat).norm();
            ++count;
        }
    }

    return count;
}

/// \brief estimate the transformation of the camera with respect to the calibration target
///        On success out_T_t_c is filled in with the transformation that takes points from
///        the camera frame to the target frame
/// \return true on success
///
/// These functions were developed with the help of Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
bool OmniProjection::estimateTransformation(const GridCalibrationTargetObservation& obs,
                                            sm::kinematics::Transformation& out_T_t_c) const {
    // Convert all chessboard corners to a fakey pinhole view.
    // Call the OpenCV pnp function.
}

}  // namespace cameras

}  // namespace aslam


/*
 * patSpeedDistributions.cc
 *
 *  Created on: Mar 26, 2012
 *      Author: jchen
 */

#include "patSpeedDistributions.h"
#include "patDisplay.h"
#include "patNBParameters.h"
#include "patErrMiscError.h"

#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/lognormal.hpp> // for normal_distribution
#include <boost/math/distributions/exponential.hpp> // for exponential distribution
#include "patDisplay.h"
#include "patError.h"
#include <sstream>
#include <vector>
#include <cstdlib>
using boost::math::normal;
using boost::math::lognormal;
using boost::math::exponential_distribution;
 map<TransportMode, TrafficModelParam> patSpeedDistributions::tm_params;
patSpeedDistributions::patSpeedDistributions() {

}

patSpeedDistributions::~patSpeedDistributions() {
}

patSpeedDistributions* patSpeedDistributions::ins = NULL;

patSpeedDistributions* patSpeedDistributions::the() {
	if (patSpeedDistributions::ins == NULL) {
		patSpeedDistributions::ins = new patSpeedDistributions;
	}
	return patSpeedDistributions::ins;
}
double patSpeedDistributions::pdf(double v, TransportMode mode) {
	TrafficModelParam tm_param = patSpeedDistributions::tm_params[mode];
	double rtn = 0.0;
	if (v < 0.0) {
//		WARNING("wrong speed" << v);
		rtn = 0.0;
	} else if (v == 0.0) {
		rtn = tm_param.w * tm_param.lambda;
	} else {

		if (tm_param.mode == TransportMode(WALK)) {
			boost::math::exponential e(tm_param.lambda);
			boost::math::normal n(tm_param.mu,tm_param.sigma);

			rtn = tm_param.w * boost::math::pdf(e,v )  + (1.0-tm_param.w) *boost::math::pdf(n,v ) ;
			// rtn = tm_param.w * tm_param.lambda * exp(-tm_param.lambda * v)
			// 		+ (1.0 - tm_param.w)
			// 				* exp(-(v - tm_param.mu) * (v - tm_param.mu) /(2.0 * tm_param.sigma * tm_param.sigma))
			// 				/ (tm_param.sigma * sqrt(2.0 * 	M_PI) );
		} else {
			boost::math::exponential e(tm_param.lambda );
			boost::math::lognormal n(tm_param.mu,tm_param.sigma);
			 rtn = tm_param.w * boost::math::pdf(e,v )  + (1.0-tm_param.w) *boost::math::pdf(n,v ) ;
			// rtn = tm_param.w * tm_param.lambda * exp(-tm_param.lambda * v)
			// 		+ (1.0 - tm_param.w)
			// 				* exp( -(log(v) - tm_param.mu) * (log(v) - tm_param.mu) /(2.0 * tm_param.sigma * tm_param.sigma))
			// 				/ (v * tm_param.sigma * sqrt(2.0 * M_PI));
		}
	}
	if (rtn >= 1.0 || rtn < 0.0) {
		WARNING("WRONG");
	}
	return rtn;
}
void patSpeedDistributions::readParams(patError*& err) {

	vector<TransportMode> modes;
	modes.push_back(TransportMode(CAR));
	modes.push_back(TransportMode(BUS));
	modes.push_back(TransportMode(METRO));
	modes.push_back(TransportMode(TRAIN));
	modes.push_back(TransportMode(WALK));
	modes.push_back(TransportMode(BIKE));
	for (int i = 0; i < modes.size(); ++i) {
		TrafficModelParam params;
		params.mode = modes[i];
		string file_name = patNBParameters::the()->paramFolder + "speed/"
				+ getTransportModeString(modes[i]);
		//patAccelMeasurementModel::m_params[modes[i]];
		ifstream file_stream_handler;
		file_stream_handler.open(file_name.c_str(), ios::in);
		if (!file_stream_handler) {
			stringstream str;
			str << "Error while parsing " << file_name;
			err = new patErrMiscError(str.str());
			WARNING(err->describe());
			return;
		}
//		DEBUG_MESSAGE("Read file:" << file_name);
		int components = 0;
		string line;
		if (getline(file_stream_handler, line)) {

			istringstream linestream(line);
			string item;

			getline(linestream, item, ',');
			//	DEBUG_MESSAGE(item);
			params.w = atof(item.c_str());

			getline(linestream, item, ',');
			//DEBUG_MESSAGE(item);
			params.lambda = atof(item.c_str());
			//		DEBUG_MESSAGE(item);

			getline(linestream, item, ',');
			params.mu = atof(item.c_str());
//			DEBUG_MESSAGE(item);

			getline(linestream, item, ',');
			params.sigma = atof(item.c_str());

//			DEBUG_MESSAGE(
//					params.w << "," << params.lambda << "," << params.mu << ","
//							<< params.sigma)
			patSpeedDistributions::tm_params[modes[i]] = params;
		} else {
			stringstream str;
			str << "Speed distribution for  " << modes[i] << "is wrong";
			err = new patErrMiscError(str.str());
			WARNING(err->describe());
			return;

		}

	}
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cotangent of the input in radian: \f$\cos(x)/\sin(x)\f$.

    @par Header <boost/simd/function/cot.hpp>

    @par Note

      As most other trigonometric function cot can be called
      with a second optional parameter  which is a tag on speed and accuracy
      (see @ref cos for further details)

    @see cos, sin, tan, cotd, cotpi

    @par Example:

      @snippet cot.cpp cot

    @par Possible output:

      @snippet cot.txt cot

  **/
  IEEEValue cot(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/cot.hpp>
#include <boost/simd/function/simd/cot.hpp>

#endif


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include "EigenPolyhedra.h"
#include <Eigen/Eigenvalues>

namespace abstract{

/// Constructs an empty buffer
template <class scalar>
EigenPolyhedra<scalar>::EigenPolyhedra(const std::string name,const int dimension) :
    m_name(name),
    m_dimension(dimension),
    m_polyhedra(dimension),
    m_eigenPolyhedra(dimension),
    m_singularPolyhedra(dimension),
    m_pEigenVectors(&Polyhedra<scalar>::ms_emptyMatrix),
    m_pInverseEigenVectors(&Polyhedra<scalar>::ms_emptyMatrix)
{
  m_polyhedra.setName(name);
  m_eigenPolyhedra.setName(name+"-eigen");
  m_singularPolyhedra.setName(name+"-singular");
}

/// Constructs an empty buffer
template <class scalar>
EigenPolyhedra<scalar>::EigenPolyhedra(const std::string name,const int dimension,AbstractPolyhedra<scalar> &source,MatrixS &transform,MatrixS &inverse) :
    m_name(name),
    m_dimension(dimension),
    m_polyhedra(source,transform,inverse),
    m_eigenPolyhedra(dimension),
    m_singularPolyhedra(dimension),
    m_pEigenVectors(&transform),
    m_pInverseEigenVectors(&inverse)
{
  m_polyhedra.setName(name);
  m_eigenPolyhedra.setName(name+"-eigen");
  m_singularPolyhedra.setName(name+"-singular");
  if (transform.rows()>0) getEigenPolyhedra();
}

template <class scalar>
EigenPolyhedra<scalar>::~EigenPolyhedra()
{
}

template <class scalar>
void EigenPolyhedra<scalar>::setName(const std::string name)
{
  m_name=name;
  m_polyhedra.setName(name);
  m_eigenPolyhedra.setName(name+"-eigen");
  m_singularPolyhedra.setName(name+"-singular");
}

/// Changes the dimensionality of the polyhedra
template <class scalar>
void EigenPolyhedra<scalar>::changeDimension(const int dimension)
{
  m_polyhedra.changeDimension(dimension);
  m_eigenPolyhedra.changeDimension(dimension);
  m_singularPolyhedra.changeDimension(dimension);
}

/// Loads a full dynamic description
template <class scalar>
int EigenPolyhedra<scalar>::load(const std::string &data,size_t pos,const bool vertices)
{
  int result=m_polyhedra.loadData(data,pos,vertices);
  m_eigenPolyhedra.clear();
  m_singularPolyhedra.clear();
  if (result>0) getEigenPolyhedra();
  return result;
}

/// Loads a known polyhedra
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::load(AbstractPolyhedra<scalar> &source,const MatrixS &transform,const MatrixS &inverse,space_t space)
{
  if (space==eNormalSpace) {
    source.getTransformedPolyhedra(m_polyhedra,transform,inverse);
    m_polyhedra.getTransformedPolyhedra(m_eigenPolyhedra,*m_pInverseEigenVectors,*m_pEigenVectors);
    m_singularPolyhedra.clear();
  }
  else {
    source.getTransformedPolyhedra(m_eigenPolyhedra,transform,inverse);
    m_eigenPolyhedra.getTransformedPolyhedra(m_polyhedra,*m_pEigenVectors,*m_pInverseEigenVectors);
    return m_eigenPolyhedra;
  }
  return m_polyhedra;
}

/// Loads a system description from file
template <class scalar>
bool EigenPolyhedra<scalar>::loadFromFile(const std::string &fileName)
{
  if (!m_polyhedra.loadFromFile(fileName)) return false;
  m_eigenPolyhedra.clear();
  m_singularPolyhedra.clear();
  getEigenPolyhedra();
  return true;
}

/// Loads a system description from file
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::load(MatrixS &faces,MatrixS &supports,space_t space)
{
  m_singularPolyhedra.clear();
  if (space==eNormalSpace) {
    m_polyhedra.load(faces,supports);
    getEigenPolyhedra();
    return m_polyhedra;
  }
  else {
    m_eigenPolyhedra.load(faces,supports);
    m_eigenPolyhedra.getTransformedPolyhedra(m_polyhedra,*m_pEigenVectors,*m_pInverseEigenVectors);
  }
  return getPolyhedra(space);
}

/// Loads a system description from file
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::mergeLoad(AbstractPolyhedra<scalar> &source,MatrixS &faces,MatrixS &supports,space_t space)
{
  m_singularPolyhedra.clear();
  if (space==eNormalSpace) {
    m_polyhedra.load(faces,supports);
    m_polyhedra.merge(source,false);
    getEigenPolyhedra();
    return m_polyhedra;
  }
  else {
    m_eigenPolyhedra.load(faces,supports);
    m_eigenPolyhedra.merge(source,false);
    m_eigenPolyhedra.getTransformedPolyhedra(m_polyhedra,*m_pEigenVectors,*m_pInverseEigenVectors);
  }
  return getPolyhedra(space);
}

template <class scalar>
void EigenPolyhedra<scalar>::clear()
{
  m_polyhedra.clear();
  m_eigenPolyhedra.clear();
  m_singularPolyhedra.clear();
}

/// Retrieves a copy of this polyhedra transformed by the given matrix
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::getPolyhedra(space_t space,bool force)
{
  if (space==eNormalSpace) return m_polyhedra;
  return (space==eEigenSpace) ? getEigenPolyhedra(force) : m_singularPolyhedra;
}

template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::getPolyhedra(space_t space,const std::vector<int> &rotations,const std::vector<int> &dilations)
{
  if (space==eNormalSpace) return m_polyhedra;
  return (space==eEigenSpace) ? getEigenPolyhedra() : getSingularPolyhedra(rotations,dilations);
}

/// Retrieves a copy of this polyhedra transformed by the given matrix
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::getTransformedPolyhedra(Polyhedra<scalar>& polyhedra,space_t space,const MatrixS& transform,const MatrixS& inverse,const MatrixS &templates)
{
  return getPolyhedra(space).getTransformedPolyhedra(polyhedra,transform,inverse,templates);
}

/// Sets the eigenvector transformation matrix
template <class scalar>
void EigenPolyhedra<scalar>::setEigenSpace(MatrixS &transform,MatrixS &inverse)
{
  m_pEigenVectors=&transform;
  m_pInverseEigenVectors=&inverse;
  m_eigenPolyhedra.clear();
  m_polyhedra.getTransformedPolyhedra(m_eigenPolyhedra,*m_pInverseEigenVectors,*m_pEigenVectors);
  m_singularPolyhedra.clear();
}

/// Centralizes the polyhedra and reflects the change in the converse space
template <class scalar>
void EigenPolyhedra<scalar>::centralize(const bool inEigenSpace)
{
  if (inEigenSpace) {
    getEigenPolyhedra();
    m_eigenPolyhedra.decentralize();
    m_eigenPolyhedra.centralize();
    m_eigenCentre=m_eigenPolyhedra.getCentre();
    m_centre=m_eigenCentre*m_pInverseEigenVectors->transpose();
    m_polyhedra.translate(m_centre);
  }
  else {
    m_polyhedra.decentralize();
    m_polyhedra.centralize();
    m_centre=m_polyhedra.getCentre();
    m_eigenCentre=m_eigenCentre*m_pEigenVectors->transpose();
    m_eigenPolyhedra.translate(m_eigenCentre);
  }
}

/// Retrieve the centre of the polyhedra
template <class scalar>
typename Tableau<scalar>::MatrixS EigenPolyhedra<scalar>::getCentre(const bool fromEigenSpace,const bool inEigenSpace)
{
  centralize(fromEigenSpace);
  return inEigenSpace ? m_eigenCentre : m_centre;
}

/// Creates m_pEigenPolyhedra if not existent
template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::getEigenPolyhedra(bool force)
{
  if (force || m_eigenPolyhedra.isEmpty()) {
    m_polyhedra.getTransformedPolyhedra(m_eigenPolyhedra,*m_pInverseEigenVectors,*m_pEigenVectors);
  }
  return m_eigenPolyhedra;
}

template <class scalar>
AbstractPolyhedra<scalar>& EigenPolyhedra<scalar>::getSingularPolyhedra(const std::vector<int> &rotations,const std::vector<int> &dilations)
{
  if (m_singularPolyhedra.isEmpty()) {
    AbstractPolyhedra<scalar> source(getEigenPolyhedra());
    source.getRounded(rotations,dilations,m_singularPolyhedra);
  }
  return m_singularPolyhedra;
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class EigenPolyhedra<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class EigenPolyhedra<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class EigenPolyhedra<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class EigenPolyhedra<mpinterval>;
  #endif
#endif

}


/// @file model.hpp Various log-PDF evaluators for parts of the Bayesian model

#ifndef BIGGLES_MODEL_HPP__
#define BIGGLES_MODEL_HPP__

#include <vector>
#include <boost/tuple/tuple.hpp>
#include <Eigen/Dense>

#include "partition.hpp"
#include "types.hpp"

namespace biggles
{

/// @brief Functions and types defining the Biggles dynamic model
namespace model
{

/// @name Access to the tracking control parameters
/// @{
inline const float& birth_rate(const parameters& p) { return p.birth_rate; }
inline float& birth_rate(parameters& p) { return p.birth_rate; }

inline const float& clutter_rate(const parameters& p) { return p.clutter_rate; }
inline float& clutter_rate(parameters& p) { return p.clutter_rate; }

inline const float& survival_probability(const parameters& p) { return p.survival_probability; }
inline float& survival_probability(parameters& p) { return p.survival_probability; }

inline const float& observation_probability(const parameters& p) { return p.observation_probability; }
inline float& observation_probability(parameters& p) { return p.observation_probability; }

inline const matrix4f& process_noise_covariance(const parameters& p) { return p.process_noise_covariance; }
inline matrix4f& process_noise_covariance(parameters& p) { return p.process_noise_covariance; }
/// @}

/// @brief Access the mean number of tracks per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& mean_new_tracks_per_frame(const parameters& p) { return p.birth_rate; }

/// @brief Access the mean number of tracks per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline float& mean_new_tracks_per_frame(parameters& p) { return p.birth_rate; }

/// @brief Access the mean number of spurious observations per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& mean_false_observations_per_frame(const parameters& p) { return p.clutter_rate; }

/// @brief Access the mean number of spurious observations per-frame parameter from a biggles::parameters tuple.
///
/// @param p
inline float& mean_false_observations_per_frame(parameters& p) { return p.clutter_rate; }

/// @brief Access the frame-to-frame survival probability parameter from a biggles::parameters tuple.
///
/// @param p
inline const float& frame_to_frame_survival_probability(const parameters& p) { return p.survival_probability; }

/// @brief Access the frame-to-frame survival probability parameter from a biggles::parameters tuple.
///
/// @param p
inline float& frame_to_frame_survival_probability(parameters& p) { return p.survival_probability; }

/// @brief Access the probability that a track will generate an observation from a biggles::parameters tuple.
///
/// @param p
inline const float& generate_observation_probability(const parameters& p) { return p.observation_probability; }

/// @brief Access the probability that a track will generate an observation from a biggles::parameters tuple.
///
/// @param p
inline float& generate_observation_probability(parameters& p) { return p.observation_probability; }

/// @brief Access the covariance matrix of the observation error from a biggles::parameters tuple.
///
/// @param p
inline const Eigen::Matrix2f& observation_error_covariance(const parameters& p) { return p.observation_error_covariance; }

/// @brief Access the covariance matrix of the observation error from a biggles::parameters tuple.
///
/// @param p
inline Eigen::Matrix2f& observation_error_covariance(parameters& p) { return p.observation_error_covariance; }

/// @brief Access the constraint radius from a biggles::parameters tuple.
///
/// @param p
inline const float& constraint_radius(const parameters& p) { return p.constraint_radius; }

/// @brief Access the constraint radius from a biggles::parameters tuple.
///
/// @param p
inline float& constraint_radius(parameters& p) { return p.constraint_radius; }

/// @brief Calculate the log-pdf of a partition given a set of model parameters, independent of observed data.
///
/// **The following needs to be reviewed. Binomial coefficients need to be included**
///
/// Calculate the value of \f$ \ell(T | \theta) \f$ from the partition and model parameters given. Since the model
/// parameters are independent of one another, we may factorise the pdf as follows:
///
/// \f[
/// P(T | \theta) = P(T | p_s) P(T | p_d) P(T | \lambda_b) P(T | \lambda_f).
/// \f]
///
/// Note that \f$ R \f$ does not appear here because it depends on data; these terms are all the data-independent parts
/// of the posterior on \f$ T \f$. Each of these terms may defined in terms of the following values:
///
/// - \f$ N_t^s \f$: the number of tracks which survive from \f$ t-1 \f$ to \f$ t \f$;
/// - \f$ N_t^d \f$: the number of tracks which did not survive from \f$ t-1 \f$ to \f$ t \f$;
/// - \f$ N_t^b \f$: the number of tracks which newly appeared at \f$ t \f$;
/// - \f$ N_t^o \f$: the number of observations assigned to a track at \f$ t \f$;
/// - \f$ N_t^f \f$: the number of observations deemed spurious at \f$ t \f$,
///
/// where \f$ N_0^s = N_o^b = 0 \f$ by convention. Each of the individual parameter pdf terms can be written down based
/// on the definition of the corresponding parameters:
///
/// - \f$ P(T | p_s) = \prod_{t=1}^K p_s^{N_t^s} (1-p_s)^{N_t^d}; \f$
/// - \f$ P(T | p_d) = \prod_{t=1}^K p_d^{N_t^o} (1-p_d)^{N_t^s + N_t^b - N_t^o}; \f$
/// - \f$ P(T | \lambda_b) = \prod_{t=1}^K \mathcal{P}(N_t^b ; \lambda_b); \f$
/// - \f$ P(T | \lambda_f) = \prod_{t=1}^K \mathcal{P}(N_t^f ; \lambda_f), \f$
///
/// where \f$ \mathcal{P}(x ; \lambda) \f$ is the Poisson pmf with mean \f$ \lambda \f$ evaluated at \f$ x \f$. The
/// corresponding log terms become:
///
/// - \f$ \ell(T | p_s) = \sum_{t=1}^K N_t^s \log(p_s) + N_t^d \log(1-p_s); \f$
/// - \f$ \ell(T | p_d) = \sum_{t=1}^K N_t^o \log(p_d) + (N_t^s + N_t^b - N_t^o) \log(1-p_d); \f$
/// - \f$ \ell(T | \lambda_b) = \sum_{t=1}^K N_t^b \log(\lambda_b) - \lambda_b - \log(\Gamma(1 + N_t^b)); \f$
/// - \f$ \ell(T | \lambda_f) = \sum_{t=1}^K N_t^f \log(\lambda_f) - \lambda_f - \log(\Gamma(1 + N_t^f)); \f$
///
/// @sa biggles::parameters
///
/// @param part A reference to the partition to consider.
/// @param parameters A reference to the model parameters.
///
/// @return The value of \f$ \ell(T | \theta) \f$.
float log_partition_given_parameters_density(const partition& part, const parameters& parameters);

float log_partition_given_observation_prob_density(const partition& part, const parameters& parameters);
float log_partition_given_survival_prob_density(const partition& part, const parameters& parameters);
float log_partition_given_birth_rate_density(const partition& part, const parameters& parameters);
float log_partition_given_clutter_rate_density(const partition& part, const parameters& parameters);

/// @brief Calculate the log-pdf for the track observations given the parameters.
///
/// This function works by using a biggles::kalman_filter to sample missing states for a track and then to calculate
/// the likelihood of the observations we've seen given the parameters.
///
/// Specifically, suppose we have an observation \f$ y \f$, a predicted state, \f$ \hat{x} \f$ and state estimation
/// error estimate, \f$ \hat{P} \f$. Then the total error in the predicted observation, \f$ \hat{y} = B \hat{x} \f$ is
/// given by \f$ \hat{\Sigma} = B \hat{P} B^T + R \f$. We therefore calculate the likelihood of \f$ y \f$ assuming a
/// Gaussian model:
///
/// \f[
/// P(y | \hat{x}, \hat{P}) = \mathcal{N}(y ; \hat{y}, \hat{\Sigma}).
/// \f]
///
/// We combine all these likelihoods for each observation in the track.
///
/// @sa biggles::kalman_filter
///
/// @param track_p
/// @param parameters
///
/// @return The value of \f$ \ell(d_i|t_i, \theta) \f$.
float log_track_given_parameters_density(const boost::shared_ptr<const track>& track_p,
                                         const parameters& parameters);

/// @brief Compute the log-prior on the model parameters.
///
/// The prior on the model parameters is as follows:
///
/// - \f$ \lambda_b \f$ and \f$ \lambda_s \f$ have improper uninformative priors on them being positive.
/// - \f$ p_s \f$ and \f$ p_d \f$ have uniform priors over [0, 1].
/// - \f$ R \f$ has an inverse Wishart prior with parameters \f$ \Phi = 2I \f$ and \f$ s = 5 \f$. These are the same
/// parameters as in sample_parameters_given_partition().
///
/// @sa sample_parameters_given_partition()
///
/// @param parameters The model parameters whose prior should be calculated.
///
/// @return The value of \f$ \ell(\theta) \f$.
float log_parameters_prior_density(const parameters& parameters);

/// @brief Calculate the log pdf of the clutter observations given the model parameters.
///
/// The clutter observations themselves are independent of the model parameter \f$ R \f$ and so their distribution is
/// very simple:
///
/// \f[
/// P(d_0 | t_0, \theta) = \prod_{t=1}^K \left( \frac{1}{V} \right)^{N_t^f}
/// \f]
///
/// where \f$ V \f$ is the total number of pixels in the image.
///
/// @note Currently it is assumed that \f$ V = 256^2 \f$. This is really a bug but for the moment, this is the only
/// place where the absolute image size matters.
///
/// @param clutter
/// @param parameters
///
/// @return The value of \f$ \ell(d_0 | t_0, \theta) \f$.
float log_clutter_given_parameters_density(const partition& part, const parameters& parameters);

/// @brief Calculate the log pdf of the track observations given the model parameters.
///
/// This uses the Kalman filter. track likelihood depends on the model parameter \f$ R \f$ only.
float log_tracks_given_parameters_density(const partition& part, const model::parameters& parameters);

/** \brief returns log( p(data| partition, parameters) )
 *
 *
 */
float log_likelihood(const partition& part_sample, const model::parameters& para_sample);

/// @brief Evaluate the full log pdf for a particular partition
///
/// In the documentation for biggles::partition_sampler, it was shown that
///
/// \f[
/// \ell(T | \theta, D) = \kappa + \ell(d_0|t_0, \theta) + \sum_{i=1}^K \ell(d_i|t_i, \theta) + \ell(T|\theta) +
/// \ell(\theta).
/// \f]
///
/// This function computes this value by calling other log density calculation functions.
///
/// @sa biggles::partition_sampler
/// @sa log_clutter_given_parameters_density()
/// @sa log_track_given_parameters_density()
/// @sa log_partition_given_parameters_density()
/// @sa log_parameters_prior_density()
///
/// @param part
/// @param parameters
///
/// @return The value of \f$ \ell(T | \theta, D) + \kappa \f$ where \f$ \kappa \f$ is some constant offset.
float log_partition_given_parameters_and_data_density(const partition& part, const parameters& parameters);

class log_factorial {
    typedef float VALUE_TYPE;
    std::deque< VALUE_TYPE > factorial_;
    log_factorial() {
        factorial_.push_back(0.f);
        factorial_.push_back(0.f);
    }
    log_factorial& operator=(log_factorial&);
    log_factorial(const log_factorial&);
public:
    /// \brief the call operator (non-const)
    VALUE_TYPE operator() (size_t n) {
        while (n >= factorial_.size()) {
            factorial_.push_back(factorial_.back() + logf(factorial_.size()));
        }
        return factorial_.at(n);
    }
    /// \brief calculate without creating a reference
    static VALUE_TYPE calc (size_t n) {
        static log_factorial fact;
        return fact(n);
    }
    /// \brief return an instance of the binomial coefficent
    static log_factorial& get() {
        static log_factorial fact;
        return fact;
    }
};

/// \brief calculates the binomial coefficent
///
/// Each value is calculated once and is stored in Pascal's triangle.
/// The calculation includes an overflow control.
/// It is a recursive procedure.
/// This is implemented as a singleton
class binomial_coefficient {
    typedef float VALUE_TYPE;
    /// \brief the row type; the size of each vector is fixed
    typedef std::vector<VALUE_TYPE> row_t;
    std::deque< row_t > triangle_; /// \brief Pascal's triangle
    size_t num_rows_; /// \brief the current number of rows of Pascal's triangle
    /// \brief The calculation routine
    void build_row(size_t n) {
        row_t row(n+1, VALUE_TYPE(1));
        for (size_t k = 1; k < n; ++k) {
            VALUE_TYPE s1 = operator()(n-1, k - 1);
            VALUE_TYPE s2 = operator()(n-1, k);
            if (s1 + s2 < s1) {
                std::stringstream errmsg;
                errmsg << "maximum reached n = " << n << ", k = " << k;
                throw std::overflow_error(errmsg.str());
            }
            row[k] = s1 + s2;
        }
        triangle_.push_back(row);
        num_rows_ = triangle_.size();
    }
    /// \brief The constructor initalises the Pascal's triangle with (0 choose 0)
    binomial_coefficient() {
        triangle_.push_back(row_t(1,VALUE_TYPE(1)));
        num_rows_ = triangle_.size();
    }
    binomial_coefficient& operator=(binomial_coefficient&);
    binomial_coefficient(const binomial_coefficient&);
public:
    /// \brief the call operator (non-const)
    VALUE_TYPE operator() (size_t n, size_t k) {
        if (num_rows_ <= n) build_row(n);
        return triangle_[n][k];
    }
    /// \brief return an instance of the binomial coefficent
    static binomial_coefficient& get() {
        static binomial_coefficient bc;
        return bc;
    }
};

}

}

#endif // BIGGLES_MODEL_HPP__