/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#include "vm.hpp"
#include <boost/assert.hpp>

#if defined(_MSC_VER)
# pragma warning(disable: 4800) // forcing value to bool 'true' or 'false'
                                // (performance warning)
#endif

namespace client
{
    int vmachine::execute(
        std::vector<int> const& code
      , std::vector<int>::const_iterator pc
      , std::vector<int>::iterator frame_ptr
    )
    {
        std::vector<int>::iterator stack_ptr = frame_ptr;

        while (pc != code.end())
        {
            BOOST_ASSERT(pc != code.end());

            switch (*pc++)
            {
                case op_neg:
                    stack_ptr[-1] = -stack_ptr[-1];
                    break;

                case op_not:
                    stack_ptr[-1] = !bool(stack_ptr[-1]);
                    break;

                case op_add:
                    --stack_ptr;
                    stack_ptr[-1] += stack_ptr[0];
                    break;

                case op_sub:
                    --stack_ptr;
                    stack_ptr[-1] -= stack_ptr[0];
                    break;

                case op_mul:
                    --stack_ptr;
                    stack_ptr[-1] *= stack_ptr[0];
                    break;

                case op_div:
                    --stack_ptr;
                    stack_ptr[-1] /= stack_ptr[0];
                    break;

                case op_eq:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] == stack_ptr[0]);
                    break;

                case op_neq:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] != stack_ptr[0]);
                    break;

                case op_lt:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] < stack_ptr[0]);
                    break;

                case op_lte:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] <= stack_ptr[0]);
                    break;

                case op_gt:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] > stack_ptr[0]);
                    break;

                case op_gte:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1] >= stack_ptr[0]);
                    break;

                case op_and:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1]) && bool(stack_ptr[0]);
                    break;

                case op_or:
                    --stack_ptr;
                    stack_ptr[-1] = bool(stack_ptr[-1]) || bool(stack_ptr[0]);
                    break;

                case op_load:
                    *stack_ptr++ = frame_ptr[*pc++];
                    break;

                case op_store:
                    --stack_ptr;
                    frame_ptr[*pc++] = stack_ptr[0];
                    break;

                case op_int:
                    *stack_ptr++ = *pc++;
                    break;

                case op_true:
                    *stack_ptr++ = true;
                    break;

                case op_false:
                    *stack_ptr++ = false;
                    break;

                case op_jump:
                    pc += *pc;
                    break;

                case op_jump_if:
                    if (!bool(stack_ptr[-1]))
                        pc += *pc;
                    else
                        ++pc;
                    --stack_ptr;
                    break;

                case op_stk_adj:
                    stack_ptr = stack.begin() + *pc++;
                    break;

                case op_call:
                    {
                        int nargs = *pc++;
                        int jump = *pc++;

                        // a function call is a recursive call to execute
                        int r = execute(
                            code
                          , code.begin() + jump
                          , stack_ptr - nargs
                        );

                        // cleanup after return from function
                        stack_ptr[-nargs] = r;      //  get return value
                        stack_ptr -= (nargs - 1);   //  the stack will now contain
                                                    //  the return value
                    }
                    break;

                case op_return:
                    return stack_ptr[-1];
            }
        }
    }
}


#ifndef __POLYNOMIAL_FILTERING_HPP
#define __POLYNOMIAL_FILTERING_HPP

//#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

typedef boost::numeric::ublas::vector<double> RealVector;
typedef boost::numeric::ublas::matrix<double> RealMatrix;

#endif // __POLYNOMIAL_


#include <Math/Vector3.hpp>
#include <Math/DiagMatrix3.hpp>
#include <Math/Float.hpp>

#include <HDF5/Matlab.hpp>
#include <HDF5/MatlabVector2.hpp>
#include <HDF5/MatlabVector3.hpp>
#include <HDF5/MatlabDiagMatrix3.hpp>
#include <HDF5/DelayedArray.hpp>
#include <HDF5/Array.hpp>

#include <boost/scoped_ptr.hpp>

#include <QVector>

template <typename T, bool delayLoading = false>
struct TypeCheckGen;

template <typename T, bool delayLoading>
struct TypeCheckGen {
    boost::optional<std::string> Type;

#define MEMBERS(m)   \
    m (Type)
    HDF5_MATLAB_DECLARE_TYPE (TypeCheckGen, MEMBERS)
#undef MEMBERS
};

template <typename T>
std::shared_ptr<TypeCheckGen<T,true> > loadTo (const TypeCheckGen<T, true>& self) {
    std::shared_ptr<TypeCheckGen<T,true> > data = std::make_shared<TypeCheckGen<T,true> > ();
    data->Type = self.Type;
    return data;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::inverse_pdf::is_pseudo_scalar_distribution.hpp //
//                                                                           //
//  (C) Copyright 2009 Erwann Rogard                                         //
//  Use, modification and distribution are subject to the                    //
//  Boost Software License, Version 1.0. (See accompanying file              //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_INVERSE_PDF_IS_PSEUDO_SCALAR_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_INVERSE_PDF_IS_PSEUDO_SCALAR_DISTRIBUTION_HPP_ER_2009
#include <boost/mpl/bool.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_pseudo_scalar_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/inverse_pdf/inverse_pdf.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

namespace meta{

    template<typename D>
    struct is_pseudo_scalar_distribution<
        inverse_pdf<D>
    > : is_pseudo_scalar_distribution<D> {};

}// meta
}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


/**
 * @file    testLoopyBelief.cpp
 * @brief
 * @author Duy-Nguyen Ta
 * @date    Oct 11, 2013
 */

#include <gtsam/inference/VariableIndex.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/range/adaptor/map.hpp>
#include <boost/assign/list_of.hpp>
#include <iostream>
#include <fstream>

using namespace std;
using namespace boost;
using namespace boost::assign;
using namespace gtsam;

/**
 * Loopy belief solver for graphs with only binary and unary factors
 */
class LoopyBelief {

  /** Star graph struct for each node, containing
   * - the star graph itself
   * - the product of original unary factors so we don't have to recompute it later, and
   * - the factor indices of the corrected belief factors of the neighboring nodes
   */
  typedef std::map<Key, size_t> CorrectedBeliefIndices;
  struct StarGraph {
    DiscreteFactorGraph::shared_ptr star;
    CorrectedBeliefIndices correctedBeliefIndices;
    DecisionTreeFactor::shared_ptr unary;
    VariableIndex varIndex_;
    StarGraph(const DiscreteFactorGraph::shared_ptr& _star,
        const CorrectedBeliefIndices& _beliefIndices,
        const DecisionTreeFactor::shared_ptr& _unary) :
        star(_star), correctedBeliefIndices(_beliefIndices), unary(_unary), varIndex_(
            *_star) {
    }

    void print(const std::string& s = "") const {
      cout << s << ":" << endl;
      star->print("Star graph: ");
      for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) {
        cout << "Belief factor index for " << key << ": "
            << correctedBeliefIndices.at(key) << endl;
      }
      if (unary)
        unary->print("Unary: ");
    }
  };

  typedef std::map<Key, StarGraph> StarGraphs;
  StarGraphs starGraphs_; ///< star graph at each variable

public:
  /** Constructor
   * Need all discrete keys to access node's cardinality for creating belief factors
   * TODO: so troublesome!!
   */
  LoopyBelief(const DiscreteFactorGraph& graph,
      const std::map<Key, DiscreteKey>& allDiscreteKeys) :
      starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) {
  }

  /// print
  void print(const std::string& s = "") const {
    cout << s << ":" << endl;
    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
      starGraphs_.at(key).print((boost::format("Node %d:") % key).str());
    }
  }

  /// One step of belief propagation
  DiscreteFactorGraph::shared_ptr iterate(
      const std::map<Key, DiscreteKey>& allDiscreteKeys) {
    static const bool debug = false;
    static DiscreteConditional::shared_ptr dummyCond; // unused by-product of elimination
    DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph());
    std::map<Key, std::map<Key, DiscreteFactor::shared_ptr> > allMessages;
    // Eliminate each star graph
    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
//      cout << "***** Node " << key << "*****" << endl;
      // initialize belief to the unary factor from the original graph
      DecisionTreeFactor::shared_ptr beliefAtKey;

      // keep intermediate messages to divide later
      std::map<Key, DiscreteFactor::shared_ptr> messages;

      // eliminate each neighbor in this star graph one by one
      for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
        DiscreteFactorGraph subGraph;
        for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) {
          subGraph.push_back(starGraphs_.at(key).star->at(factor));
        }
        if (debug) subGraph.print("------- Subgraph:");
        DiscreteFactor::shared_ptr message;
        boost::tie(dummyCond, message) = EliminateDiscrete(subGraph,
            Ordering(list_of(neighbor)));
        // store the new factor into messages
        messages.insert(make_pair(neighbor, message));
        if (debug) message->print("------- Message: ");

        // Belief is the product of all messages and the unary factor
        // Incorporate new the factor to belief
        if (!beliefAtKey)
          beliefAtKey = std::dynamic_pointer_cast<DecisionTreeFactor>(
              message);
        else
          beliefAtKey =
              std::make_shared<DecisionTreeFactor>(
                  (*beliefAtKey)
                      * (*std::dynamic_pointer_cast<DecisionTreeFactor>(
                          message)));
      }
      if (starGraphs_.at(key).unary)
        beliefAtKey = std::make_shared<DecisionTreeFactor>(
            (*beliefAtKey) * (*starGraphs_.at(key).unary));
      if (debug) beliefAtKey->print("New belief at key: ");
      // normalize belief
      double sum = 0.0;
      for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v) {
        DiscreteFactor::Values val;
        val[key] = v;
        sum += (*beliefAtKey)(val);
      }
      string sumFactorTable;
      for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v)
        sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str();
      DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable);
      if (debug) sumFactor.print("denomFactor: ");
      beliefAtKey = std::make_shared<DecisionTreeFactor>((*beliefAtKey) / sumFactor);
      if (debug) beliefAtKey->print("New belief at key normalized: ");
      beliefs->push_back(beliefAtKey);
      allMessages[key] = messages;
    }

    // Update corrected beliefs
    VariableIndex beliefFactors(*beliefs);
    for(Key key: starGraphs_ | boost::adaptors::map_keys) {
      std::map<Key, DiscreteFactor::shared_ptr> messages = allMessages[key];
      for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) {
        DecisionTreeFactor correctedBelief = (*std::dynamic_pointer_cast<
            DecisionTreeFactor>(beliefs->at(beliefFactors[key].front())))
            / (*std::dynamic_pointer_cast<DecisionTreeFactor>(
                messages.at(neighbor)));
        if (debug) correctedBelief.print("correctedBelief: ");
        size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at(
            key);
        starGraphs_.at(neighbor).star->replace(beliefIndex,
            std::make_shared<DecisionTreeFactor>(correctedBelief));
      }
    }

    if (debug) print("After update: ");

    return beliefs;
  }

private:
  /**
   * Build star graphs for each node.
   */
  StarGraphs buildStarGraphs(const DiscreteFactorGraph& graph,
      const std::map<Key, DiscreteKey>& allDiscreteKeys) const {
    StarGraphs starGraphs;
    VariableIndex varIndex(graph); ///< access to all factors of each node
    for(Key key: varIndex | boost::adaptors::map_keys) {
      // initialize to multiply with other unary factors later
      DecisionTreeFactor::shared_ptr prodOfUnaries;

      // collect all factors involving this key in the original graph
      DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
      for(size_t factorIndex: varIndex[key]) {
        star->push_back(graph.at(factorIndex));

        // accumulate unary factors
        if (graph.at(factorIndex)->size() == 1) {
          if (!prodOfUnaries)
            prodOfUnaries = std::dynamic_pointer_cast<DecisionTreeFactor>(
                graph.at(factorIndex));
          else
            prodOfUnaries = std::make_shared<DecisionTreeFactor>(
                *prodOfUnaries
                    * (*std::dynamic_pointer_cast<DecisionTreeFactor>(
                        graph.at(factorIndex))));
        }
      }

      // add the belief factor for each neighbor variable to this star graph
      // also record the factor index for later modification
      KeySet neighbors = star->keys();
      neighbors.erase(key);
      CorrectedBeliefIndices correctedBeliefIndices;
      for(Key neighbor: neighbors) {
        // TODO: default table for keys with more than 2 values?
        string initialBelief;
        for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) {
          initialBelief = initialBelief + "0.0 ";
        }
        initialBelief = initialBelief + "1.0";
        star->push_back(
            DecisionTreeFactor(allDiscreteKeys.at(neighbor), initialBelief));
        correctedBeliefIndices.insert(make_pair(neighbor, star->size() - 1));
      }
      starGraphs.insert(
          make_pair(key,
              StarGraph(star, correctedBeliefIndices, prodOfUnaries)));
    }
    return starGraphs;
  }
};

/* ************************************************************************* */

TEST_UNSAFE(LoopyBelief, construction) {
  // Variables: Cloudy, Sprinkler, Rain, Wet
  DiscreteKey C(0, 2), S(1, 2), R(2, 2), W(3, 2);

  // Map from key to DiscreteKey for building belief factor.
  // TODO: this is bad!
  std::map<Key, DiscreteKey> allKeys = map_list_of(0, C)(1, S)(2, R)(3, W);

  // Build graph
  DecisionTreeFactor pC(C, "0.5 0.5");
  DiscreteConditional pSC(S | C = "0.5/0.5 0.9/0.1");
  DiscreteConditional pRC(R | C = "0.8/0.2 0.2/0.8");
  DecisionTreeFactor pSR(S & R, "0.0 0.9 0.9 0.99");

  DiscreteFactorGraph graph;
  graph.push_back(pC);
  graph.push_back(pSC);
  graph.push_back(pRC);
  graph.push_back(pSR);

  graph.print("graph: ");

  LoopyBelief solver(graph, allKeys);
  solver.print("Loopy belief: ");

  // Main loop
  for (size_t iter = 0; iter < 20; ++iter) {
    cout << "==================================" << endl;
    cout << "iteration: " << iter << endl;
    DiscreteFactorGraph::shared_ptr beliefs = solver.iterate(allKeys);
    beliefs->print();
  }

}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <algorithm>
#include <Eigen/Geometry>
#include <stdexcept>

#ifndef VOXEL_GRID_HPP
#define VOXEL_GRID_HPP

namespace VoxelGrid
{
    struct GRID_INDEX
    {
        int64_t x;
        int64_t y;
        int64_t z;

        GRID_INDEX() : x(-1), y(-1), z(-1) {}

        GRID_INDEX(const int64_t in_x, const int64_t in_y, const int64_t in_z) : x(in_x), y(in_y), z(in_z) {}

        bool operator==(const GRID_INDEX& other) const
        {
            return (x == other.x && y == other.y && z == other.z);
        }
    };

    template<typename T, typename Allocator=std::allocator<T>>
    class VoxelGrid
    {
    protected:

        bool initialized_;
        Eigen::Affine3d origin_transform_;
        Eigen::Affine3d inverse_origin_transform_;
        std::vector<T, Allocator> data_;
        double cell_x_size_;
        double cell_y_size_;
        double cell_z_size_;
        double x_size_;
        double y_size_;
        double z_size_;
        int64_t stride1_;
        int64_t stride2_;
        int64_t num_x_cells_;
        int64_t num_y_cells_;
        int64_t num_z_cells_;
        T default_value_;
        T oob_value_;

        inline int64_t GetDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return (x_index * stride1_) + (y_index * stride2_) + z_index;
        }

        inline void SetContents(const T& value)
        {
            data_.clear();
            data_.resize(num_x_cells_ * num_y_cells_ * num_z_cells_, value);
        }

        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size,const double y_size, const double z_size) const
        {
            if (cell_x_size <= 0.0)
            {
                throw std::invalid_argument("cell_x_size must be positive and non-zero");
            }
            if (std::isnan(cell_x_size))
            {
                throw std::invalid_argument("cell_x_size must not be NaN");
            }
            if (std::isinf(cell_x_size) != 0)
            {
                throw std::invalid_argument("cell_x_size must not be INF");
            }
            if (cell_y_size <= 0.0)
            {
                throw std::invalid_argument("cell_y_size must be positive and non-zero");
            }
            if (std::isnan(cell_y_size))
            {
                throw std::invalid_argument("cell_y_size must not be NaN");
            }
            if (std::isinf(cell_y_size) != 0)
            {
                throw std::invalid_argument("cell_y_size must not be INF");
            }
            if (cell_z_size <= 0.0)
            {
                throw std::invalid_argument("cell_z_size must be positive and non-zero");
            }
            if (std::isnan(cell_z_size))
            {
                throw std::invalid_argument("cell_z_size must not be NaN");
            }
            if (std::isinf(cell_z_size) != 0)
            {
                throw std::invalid_argument("cell_z_size must not be INF");
            }
            if (x_size <= 0.0)
            {
                throw std::invalid_argument("x_size must be positive and non-zero");
            }
            if (y_size <= 0.0)
            {
                throw std::invalid_argument("y_size must be positive and non-zero");
            }
            if (z_size <= 0.0)
            {
                throw std::invalid_argument("z_size must be positive and non-zero");
            }
            if (std::isnan(x_size))
            {
                throw std::invalid_argument("x_size must not be NaN");
            }
            if (std::isnan(y_size))
            {
                throw std::invalid_argument("y_size must not be NaN");
            }
            if (std::isnan(z_size))
            {
                throw std::invalid_argument("z_size must not be NaN");
            }
            if (std::isinf(x_size) != 0)
            {
                throw std::invalid_argument("x_size must not be INF");
            }
            if (std::isinf(y_size) != 0)
            {
                throw std::invalid_argument("y_size must not be INF");
            }
            if (std::isinf(z_size) != 0)
            {
                throw std::invalid_argument("z_size must not be INF");
            }
        }

        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells,const int64_t num_y_cells, const int64_t num_z_cells) const
        {
            if (cell_x_size <= 0.0)
            {
                throw std::invalid_argument("cell_x_size must be positive and non-zero");
            }
            if (std::isnan(cell_x_size))
            {
                throw std::invalid_argument("cell_x_size must not be NaN");
            }
            if (std::isinf(cell_x_size) != 0)
            {
                throw std::invalid_argument("cell_x_size must not be INF");
            }
            if (cell_y_size <= 0.0)
            {
                throw std::invalid_argument("cell_y_size must be positive and non-zero");
            }
            if (std::isnan(cell_y_size))
            {
                throw std::invalid_argument("cell_y_size must not be NaN");
            }
            if (std::isinf(cell_y_size) != 0)
            {
                throw std::invalid_argument("cell_y_size must not be INF");
            }
            if (cell_z_size <= 0.0)
            {
                throw std::invalid_argument("cell_z_size must be positive and non-zero");
            }
            if (std::isnan(cell_z_size))
            {
                throw std::invalid_argument("cell_z_size must not be NaN");
            }
            if (std::isinf(cell_z_size) != 0)
            {
                throw std::invalid_argument("cell_z_size must not be INF");
            }
            if (num_x_cells <= 0)
            {
                throw std::invalid_argument("num_x_cells must be positive and non-zero");
            }
            if (num_y_cells <= 0)
            {
                throw std::invalid_argument("num_y_cells must be positive and non-zero");
            }
            if (num_z_cells <= 0)
            {
                throw std::invalid_argument("num_z_cells must be positive and non-zero");
            }
        }

        inline void CoreInitialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            cell_x_size_ = fabs(cell_x_size);
            cell_y_size_ = fabs(cell_y_size);
            cell_z_size_ = fabs(cell_z_size);
            num_x_cells_ = num_x_cells;
            num_y_cells_ = num_y_cells;
            num_z_cells_ = num_z_cells;
            x_size_ = (double)num_x_cells_ * cell_x_size_;
            y_size_ = (double)num_y_cells_ * cell_y_size_;
            z_size_ = (double)num_z_cells_ * cell_z_size_;
            default_value_ = default_value;
            oob_value_ = oob_value;
            stride1_ = num_y_cells_ * num_z_cells_;
            stride2_ = num_z_cells_;
            SetContents(default_value_);
        }

    public:

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_size, const double x_size, const double y_size, double const z_size, const T& default_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value)
        {
            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
        }

        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
        }

        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        VoxelGrid()
        {
            origin_transform_.setIdentity();
            inverse_origin_transform_ = origin_transform_.inverse();
            cell_x_size_ = 0.0;
            cell_y_size_ = 0.0;
            cell_z_size_ = 0.0;
            x_size_ = 0.0;
            y_size_ = 0.0;
            z_size_ = 0.0;
            num_x_cells_ = 0;
            num_y_cells_ = 0;
            num_z_cells_ = 0;
            stride1_ = num_y_cells_ * num_z_cells_;
            stride2_ = num_z_cells_;
            data_.clear();
            initialized_ = false;
        }

        inline void Initialize(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        inline void Initialize(const Eigen::Affine3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
            origin_transform_ = origin_transform;
            inverse_origin_transform_ = origin_transform_.inverse();
            initialized_ = true;
        }

        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
        }

        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
        {
            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
            Eigen::Translation3d origin_translation(-x_size_ * 0.5, -y_size_ * 0.5, -z_size_ * 0.5);
            Eigen::Quaterniond origin_rotation;
            origin_rotation.setIdentity();
            origin_transform_ = origin_translation * origin_rotation;
            inverse_origin_transform_ = origin_transform_.inverse();
            initialized_ = true;
        }

        inline bool IsInitialized() const
        {
            return initialized_;
        }

        inline void ResetWithDefault()
        {
            SetContents(default_value_);
        }

        inline void ResetWithNewValue(const T& new_value)
        {
            SetContents(new_value);
        }

        inline void ResetWithNewDefault(const T& new_default)
        {
            default_value_ = new_default;
            SetContents(default_value_);
        }

        inline bool IndexInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            if (x_index >= 0 && y_index >= 0 && z_index >= 0 && x_index < num_x_cells_ && y_index < num_y_cells_ && z_index < num_z_cells_)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline std::pair<const T&, bool> GetImmutable(const Eigen::Vector3d& location) const
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex(location);
            if (indices.size() == 3)
            {
                return GetImmutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<const T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<const T&, bool> GetImmutable(const double x, const double y, const double z) const
        {
            const Eigen::Vector3d location(x, y, z);
            return GetImmutable(location);
        }

        inline std::pair<const T&, bool> GetImmutable(const GRID_INDEX& index) const
        {
            return GetImmutable(index.x, index.y, index.z);
        }

        inline std::pair<const T&, bool> GetImmutable(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                return std::pair<const T&, bool>(data_[data_index], true);
            }
            else
            {
                return std::pair<const T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<T&, bool> GetMutable(const Eigen::Vector3d& location)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex(location);
            if (indices.size() == 3)
            {
                return GetMutable(indices[0], indices[1], indices[2]);
            }
            else
            {
                return std::pair<T&, bool>(oob_value_, false);
            }
        }

        inline std::pair<T&, bool> GetMutable(const double x, const double y, const double z)
        {
            const Eigen::Vector3d location(x, y, z);
            return GetMutable(location);
        }

        inline std::pair<T&, bool> GetMutable(const GRID_INDEX& index)
        {
            return GetMutable(index.x, index.y, index.z);
        }

        inline std::pair<T&, bool> GetMutable(const int64_t x_index, const int64_t y_index, const int64_t z_index)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                return std::pair<T&, bool>(data_[data_index], true);
            }
            else
            {
                return std::pair<T&, bool>(oob_value_, false);
            }
        }

        inline bool SetValue(const Eigen::Vector3d& location, const T& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue(const double x, const double y, const double z, const T& value)
        {
            const Eigen::Vector3d location(x, y, z);
            return SetValue(location, value);
        }

        inline bool  SetValue(const GRID_INDEX& index, const T& value)
        {
            return SetValue(index.x, index.y, index.z, value);
        }

        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, const T& value)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                data_[data_index] = value;
                return true;
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue(const Eigen::Vector3d& location, T&& value)
        {
            assert(initialized_);
            const std::vector<int64_t> indices = LocationToGridIndex(location);
            if (indices.size() == 3)
            {
                return SetValue(indices[0], indices[1], indices[2], value);
            }
            else
            {
                return false;
            }
        }

        inline bool SetValue(const double x, const double y, const double z, T&& value)
        {
            const Eigen::Vector3d location(x, y, z);
            return SetValue(location, value);
        }

        inline bool  SetValue(const GRID_INDEX& index, T&& value)
        {
            return SetValue(index.x, index.y, index.z, value);
        }

        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, T&& value)
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
                assert(data_index >= 0 && data_index < (int64_t)data_.size());
                data_[data_index] = value;
                return true;
            }
            else
            {
                return false;
            }
        }

        inline double GetXSize() const
        {
            return x_size_;
        }

        inline double GetYSize() const
        {
            return y_size_;
        }

        inline double GetZSize() const
        {
            return z_size_;
        }

        inline std::vector<double> GetCellSizes() const
        {
            return std::vector<double>{cell_x_size_, cell_y_size_, cell_z_size_};
        }

        inline T GetDefaultValue() const
        {
            return default_value_;
        }

        inline T GetOOBValue() const
        {
            return oob_value_;
        }

        inline void SetDefaultValue(const T& default_value)
        {
            default_value_ = default_value;
        }

        inline void SetOOBValue(const T& oob_value)
        {
            oob_value_ = oob_value;
        }

        inline int64_t GetNumXCells() const
        {
            return num_x_cells_;
        }

        inline int64_t GetNumYCells() const
        {
            return num_y_cells_;
        }

        inline int64_t GetNumZCells() const
        {
            return num_z_cells_;
        }

        inline Eigen::Affine3d GetOriginTransform() const
        {
            return origin_transform_;
        }

        inline std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const
        {
            const Eigen::Vector3d point(x, y, z);
            return LocationToGridIndex(point);
        }

        inline std::vector<int64_t> LocationToGridIndex(const Eigen::Vector3d& location) const
        {
            assert(initialized_);
            const Eigen::Vector3d point_in_grid_frame = inverse_origin_transform_ * location;
            const int64_t x_cell = (int64_t)(point_in_grid_frame.x() / cell_x_size_);
            const int64_t y_cell = (int64_t)(point_in_grid_frame.y() / cell_y_size_);
            const int64_t z_cell = (int64_t)(point_in_grid_frame.z() / cell_z_size_);
            if (IndexInBounds(x_cell, y_cell, z_cell))
            {
                return std::vector<int64_t>{x_cell, y_cell, z_cell};
            }
            else
            {
                return std::vector<int64_t>();
            }
        }

        inline std::vector<double> GridIndexToLocation(const GRID_INDEX& index) const
        {
            return GridIndexToLocation(index.x, index.y, index.z);
        }

        inline std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            assert(initialized_);
            if (IndexInBounds(x_index, y_index, z_index))
            {
                const Eigen::Vector3d point_in_grid_frame(cell_x_size_ * ((double)x_index + 0.5), cell_y_size_ * ((double)y_index + 0.5), cell_z_size_ * ((double)z_index + 0.5));
                const Eigen::Vector3d point = origin_transform_ * point_in_grid_frame;
                return std::vector<double>{point.x(), point.y(), point.z()};
            }
            else
            {
                return std::vector<double>();
            }
        }

        inline const std::vector<T>& GetRawData() const
        {
            return data_;
        }

        inline std::vector<T> CopyRawData() const
        {
            return data_;
        }

        inline bool SetRawData(std::vector<T>& data)
        {
            assert(initialized_);
            const int64_t expected_length = num_x_cells_ * num_y_cells_ * num_z_cells_;
            if ((int64_t)data.size() == expected_length)
            {
                data_ = data;
                return true;
            }
            else
            {
                std::cerr << "Failed to load internal data - expected " << expected_length << " got " << data.size() << std::endl;
                return false;
            }
        }

        inline uint64_t HashDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
        {
            return (x_index * stride1_) + (y_index * stride2_) + z_index;
        }
    };
}

namespace std
{
    template <>
    struct hash<VoxelGrid::GRID_INDEX>
    {
        std::size_t operator()(const VoxelGrid::GRID_INDEX& index) const
        {
            using std::size_t;
            using std::hash;
            return ((std::hash<int64_t>()(index.x) ^ (std::hash<int64_t>()(index.y) << 1) >> 1) ^ (std::hash<int64_t>()(index.z) << 1));
        }
    };
}

inline std::ostream& operator<<(std::ostream& strm, const VoxelGrid::GRID_INDEX& index)
{
    strm << "GridIndex: " << index.x << "," << index.y << "," << index.z;
    return strm;
}

#endif // VOXEL_GRID_HPP


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_MODULO_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MODULO_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing modulo capabilities

    return the elementwise modulo of the two parameters
    Infix notation can be used with operator '\%'.

    Does not work for floating point entries.

    @par Semantic:

    For every parameters of type T:

    @code
    T r = modulo(x,y);
    @endcode

    is similar to:

    @code
    T r = x % y;
    @endcode

    @see  rem, remainder, mod,

  **/
  const boost::dispatch::functor<tag::modulo_> modulo = {};
} }
#endif

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

#endif


#ifndef CAT2MAP_HPP
#define CAT2MAP_HPP

#include <fstream>
#include <iostream>
#include <exception>
#include <vector>
#include <cmath>
#include <cassert>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/log/trivial.hpp>
#include <boost/tokenizer.hpp>

#include <healpix_map.h>
#include <pointing.h>
#include <healpix_map_fitsio.h>
#include <datatypes.h>

/**
 * \class Cat2Map
 * \brief A class for converting catalogues to HEALPix maps
 *
 * This class reads a catalogue and convert the information to a
 * HEALPix map.
 */
class Cat2Map
{
public:

    /**
     * \typedef boost::property_tree::ptree propertyTreeType
     * \brief defines the property tree type
     */
    typedef boost::property_tree::ptree propertyTreeType;

    /**
     * \typedef Healpix_Map<double> mapType
     * \brief defines the HEALPix map type
     */
    typedef Healpix_Map<double> mapType;

    static const constexpr double deg2rad = M_PI/double(180);/**< factor for converting degree to radians */
    static const constexpr double rotPhi = double(0); /**< rotaion to phi required for convertion from ra */

    /**
     * \brief The default constructor
     *
     * \param iniFileName name of the input configuration file
     */
    explicit Cat2Map(std::string const& iniFileName)
    {
        BOOST_LOG_TRIVIAL(info) << std::string("Reading the ini file ") + std::string(iniFileName);
        boost::property_tree::ini_parser::read_ini(iniFileName,mPropTree);

        // set the output maps
        BOOST_LOG_TRIVIAL(info) << "Resulution of the output map is "<<mPropTree.get<std::string>("output.n_side");

        int nSide = mPropTree.get<int>("output.n_side");
        mMapN.SetNside(nSide,RING);
        mMapNNInv.SetNside(nSide,RING);
        mMapE1.SetNside(nSide,RING);
        mMapE2.SetNside(nSide,RING);
        mMapE1NInv.SetNside(nSide,RING);
        mMapE2NInv.SetNside(nSide,RING);

        mMapN.fill(double(0));
        mMapNNInv.fill(double(0));
        mMapE1.fill(double(0));
        mMapE1.fill(double(0));
        mMapE1NInv.fill(double(0));
        mMapE2NInv.fill(double(0));

        // read the mask
        read_Healpix_map_from_fits(mPropTree.get<std::string>("input.mask_file_name"),mMask);

        try
        {
            std::string testMapFileName = mPropTree.get<std::string>("test.map_file_name");

            BOOST_LOG_TRIVIAL(info) << "Test map file specified as "<< testMapFileName;

            read_Healpix_map_from_fits(testMapFileName,mTestE1,int(2),int(2));
            read_Healpix_map_from_fits(testMapFileName,mTestE2,int(3),int(2));

            mDoTest = true;
        }
        catch(std::exception)
        {
            BOOST_LOG_TRIVIAL(info) << "No test map file specified.";
            mDoTest = false;
        }

        try
        {
            std::string testMapFileName = mPropTree.get<std::string>("output.z_bounds");
            BOOST_LOG_TRIVIAL(info) << "z bounds specified as "<< testMapFileName;
            typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
            boost::char_separator<char> sep(",");
            tokenizer tokens(testMapFileName, sep);

            for(tokenizer::iterator tokIter = tokens.begin(); tokIter !=tokens.end(); ++tokIter)
            {
                mZBounds.push_back(boost::lexical_cast<double>(*tokIter));
            }
        }
        catch(std::exception)
        {
            BOOST_LOG_TRIVIAL(info) << "No z bounds specified. All objects will be accumulated.";
        }

        if(mZBounds.size() != size_t(2))
        {
            std::string msg("The z-bounds should consist of two values. No more, no less.");
            throw std::runtime_error(msg);
        }

        if(mZBounds[0] >= mZBounds[1])
        {
            std::string msg("The upper bound should be greater than the lower bound.");
            throw std::runtime_error(msg);
        }

        mMisMatchCountE1 = size_t(0);
        mMisMatchCountE2 = size_t(0);
        mNumObsPix = size_t(0);
    }

    /**
     * \brief A function that accumulates the objects in a catalogue
     */
    void accumulate()
    {
        // open file for reading
        std::ifstream inputCatFile;
        inputCatFile.open( mPropTree.get<std::string>("input.catlogue_file_name").c_str(),std::ios::in );

        if(inputCatFile.is_open())
        {

            BOOST_LOG_TRIVIAL(info) << "Accumulating objects";
            BOOST_LOG_TRIVIAL(info) << "Number of rows to be skipped = " << mPropTree.get<std::string>("input.skip_rows");
            BOOST_LOG_TRIVIAL(info) << "Delimiter for separation is "<< mPropTree.get<std::string>("input.delimiter");

            size_t col_ra =  mPropTree.get<size_t>("input.col_ra");
            size_t col_dec =  mPropTree.get<size_t>("input.col_dec");
            size_t col_z = mPropTree.get<size_t>("input.col_z");
            size_t col_ellip_1 = mPropTree.get<size_t>("input.col_ellip_1");
            size_t col_ellip_2 = mPropTree.get<size_t>("input.col_ellip_2");

            BOOST_LOG_TRIVIAL(info) << "Column of ra values = "<< col_ra;
            BOOST_LOG_TRIVIAL(info) << "Column of dec values = "<< col_dec;
            BOOST_LOG_TRIVIAL(info) << "Column of z values = "<< col_z;
            BOOST_LOG_TRIVIAL(info) << "Column of ellip_1 values = "<< col_ellip_1;
            BOOST_LOG_TRIVIAL(info) << "Column of ellip_2 values = "<< col_ellip_2;

            size_t line_id = 0;
            while(!inputCatFile.eof())
            {
                std::string line;

                std::getline(inputCatFile,line);

                ++line_id;

                if(line_id > mPropTree.get<size_t>("input.skip_rows"))
                {
                    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
                    boost::char_separator<char> sep(mPropTree.get<std::string>("input.delimiter").c_str());
                    tokenizer tokens(line, sep);

                    std::vector<double> ents;
                    for(tokenizer::iterator tokIter = tokens.begin(); tokIter !=tokens.end(); ++tokIter)
                    {
                        ents.push_back(boost::lexical_cast<double>(*tokIter));
                    }

                    if(ents.size()>0)
                    {
                        assert(col_ra<ents.size());
                        assert(col_dec<ents.size());
                        assert(col_ellip_1<ents.size());
                        assert(col_ellip_2<ents.size());

                        double ra = ents[col_ra];
                        double dec = ents[col_dec];
                        double z = ents[col_z];
                        double e1 = ents[col_ellip_1];
                        double e2 = ents[col_ellip_2];

                        double theta = -deg2rad*dec + M_PI*double(0.5);
                        double phi = deg2rad*(ra - rotPhi);

                        auto pix = mMapE1.ang2pix(pointing(theta,phi));

                        // check if the pixel falls in the masked region
                        if(mMask[pix]>0)
                        {
                            if(z >= mZBounds[0] and z < mZBounds[1])
                            {
                                if(mDoTest)
                                {
                                    // if testing accumulate the miss-match
                                    // between pixel values
                                    if(std::abs( (e1 - mTestE1[pix])/e1 ) >= 1e-5)
                                    {
                                        mMisMatchCountE1 += size_t(1);
                                    }

                                    if(std::abs( (e2 - mTestE2[pix])/e2 ) > 1e-5)
                                    {
                                        mMisMatchCountE2 += size_t(1);
                                    }
                                }

                                // do accumulation
                                mMapN[pix] += double(1);
                                mMapE1[pix] += e1;
                                mMapE2[pix] += e2;
                                mMapE1NInv[pix] += e1*e1;
                                mMapE2NInv[pix] += e2*e2;
                                //mNumObsPix += size_t(1); //TODO this is wrong
                            }
                        }

                    }

                }
                else
                {
                    BOOST_LOG_TRIVIAL(info) << "Skipping line " << line_id;
                }
            }

            // make it the average , also count the total objects
            double totObjs(0);
            for(auto pix = 0; pix<mMapN.Npix(); ++pix)
            {
                if(mMapN[pix]>0) // TODO how many gals we need to make an estimate
                {
                    mMapE1[pix] /= mMapN[pix];
                    mMapE2[pix] /= mMapN[pix];

                    // var = E(X^2) - E(X)^2
                    mMapE1NInv[pix] = ( mMapE1NInv[pix]/mMapN[pix] - mMapE1[pix]*mMapE1[pix]);
                    mMapE2NInv[pix] = ( mMapE2NInv[pix]/mMapN[pix] - mMapE2[pix]*mMapE2[pix]);

                    // nInv = 1/var
                    if(mMapE1NInv[pix] > double(0) and mMapE2NInv[pix] > double(0) )
                    {
                        mMapE1NInv[pix] = double(1)/mMapE1NInv[pix];
                        mMapE2NInv[pix] = double(1)/mMapE2NInv[pix];

                        // compute the observed number of pixels in the data
                        mNumObsPix += size_t(1);
                        totObjs += mMapN[pix];

                        //mMapNNInv[pix] = nbar; //TODO note that I may have to recalculate this again
                    }
                    else
                    {
                        // we don't have the variance properly defined
                        mMask[pix] = 0;
                        mMapE1NInv[pix] = 0.;
                        mMapE2NInv[pix] = 0;
                        mMapE1[pix] = 0;
                        mMapE2[pix] = 0;
                        mMapN[pix] = 0;
                        mMapNNInv[pix] = 0;
                    }
                }
                else
                {
                    // make the new mask
                    mMask[pix] = 0;
                    mMapE1NInv[pix] = 0.;
                    mMapE2NInv[pix] = 0;
                    mMapE1[pix] = 0;
                    mMapE2[pix] = 0;
                    mMapNNInv[pix] = 0;
                }
            }

            // output the sky fraction of the augmented mask
            size_t nPix = (size_t) mTestE1.Npix();
            double fKsy = (double)mNumObsPix / (double)nPix;
            BOOST_LOG_TRIVIAL(info) << "Sky fraction " << fKsy;

            BOOST_LOG_TRIVIAL(info) << "Toal objects in the map " << totObjs;

            // output the n_bar or the mean number of objects per pixel
            double nBar = totObjs/(double)mNumObsPix;
            BOOST_LOG_TRIVIAL(info) << "Mean objects per pixel " << nBar;

            assert(nBar > double(0));

            // assign nbar to nInv map for number density
            for(auto pix = 0; pix<mMapN.Npix(); ++pix)
            {
                if(mMapN[pix] > 0)
                {
                    mMapN[pix] = (mMapN[pix] - nBar)/nBar;
                    mMapNNInv[pix] = nBar;
                }
            }

            // if testing print the mismatch stats
            if(mDoTest)
            {

                double fracMissMatchE1 = (double)mMisMatchCountE1 / (double)mNumObsPix;
                double fracMissMatchE2 = (double)mMisMatchCountE2 / (double)mNumObsPix;

                BOOST_LOG_TRIVIAL(warning) << "Pixel value miss-match for e1 = "<<fracMissMatchE1;
                BOOST_LOG_TRIVIAL(warning) << "Pixel value miss-match for e2 = "<<fracMissMatchE2;
            }
        }
        else
        {
            std::string msg = std::string("Input catalogue file ")
                + std::string(mPropTree.get<std::string>("input.catlogue_file_name"))
                + std::string(" failed to open.");
            BOOST_LOG_TRIVIAL(error) << msg;
            throw std::runtime_error(msg);
        }

    }

    /**
     * \brief write the maps to fits files
     */
    void writeMaps()
    {
        BOOST_LOG_TRIVIAL(info) << "Output data map file name :  "
            << mPropTree.get<std::string>("output.data_map_file_name");

        write_Healpix_map_to_fits(std::string("!")+mPropTree.get<std::string>("output.data_map_file_name"),
            mMapN,mMapE1,mMapE2,planckType<double>());

        BOOST_LOG_TRIVIAL(info) << "Output nInv map file name :  "
            << mPropTree.get<std::string>("output.nInv_map_file_name");

        write_Healpix_map_to_fits(std::string("!")+mPropTree.get<std::string>("output.nInv_map_file_name"),
            mMapNNInv,mMapE1NInv,mMapE2NInv,planckType<double>());

        BOOST_LOG_TRIVIAL(info) << "Output augmented mask file name :  "
            << mPropTree.get<std::string>("output.augmented_mask_file_name");

        write_Healpix_map_to_fits(std::string("!")+mPropTree.get<std::string>("output.augmented_mask_file_name"),
            mMask,planckType<double>());
    }

private:
    propertyTreeType mPropTree; /**< property tree that stores the ini file information */
    mapType mMapN; /**< number density map */
    mapType mMapNNInv; /**< number density map */
    mapType mMapE1; /**<  ellipticity-1 map */
    mapType mMapE2; /**< ellipticity-2 map */
    mapType mMapE1NInv; /**<  ellipticity-1 nInv map */
    mapType mMapE2NInv; /**< ellipticity-2 nInv map */
    mapType mMask; /**< mask */
    std::vector<double> mZBounds; /**< z bounds */

    mapType mTestE1; /**<  ellipticity-1 test map*/
    mapType mTestE2; /**< ellipticity-2 test map */
    bool mDoTest; /**< a flag to to sanity test */
    size_t mMisMatchCountE1; /**< sanity check mismatch for ellipticity-1*/
    size_t mMisMatchCountE2; /**< sanity check mismatch for ellipticity-2*/
    size_t mNumObsPix; /**< number of observed pixels*/

};

#endif //CAT2MAP_HPP


/*
 * Copyright 2014 RWTH Aachen University. All rights reserved.
 *
 * Licensed under the RWTH LM License (the "License");
 * you may not use this file except in compliance with the License.
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <cassert>
#include <cmath>
#include <boost/filesystem/operations.hpp>
#include "gradienttest.h"

GradientTest::GradientTest(const uint32_t seed,
                           Trainer *const trainer,
                           const bool is_feedforward)
    : seed_(seed),
      significant_digits_(6),
      epsilon_(1e-5),
      trainer_(trainer) {
  if (is_feedforward) {
    for (auto &indices : trainer_->training_data_->data_) {
      assert(indices.size() >= 2);
      indices.resize(2);
    }
  }
  num_running_words_ = trainer_->training_data_->CountNumRunningWords();
}

// N.b. This test will fail for float (which has only six digits of precision)!
void GradientTest::Test() {
  std::cout << "Testing gradient implementation ...\n";
  for (auto f : trainer_->net_->functions_) {
    TableLookup *table_lookup = dynamic_cast<TableLookup *>(f.get());
    if (table_lookup != nullptr) {
      Test(table_lookup);
      continue;
    }
    Linear *linear = dynamic_cast<Linear *>(f.get());
    if (linear != nullptr) {
      Test(linear);
      continue;
    }
    LSTM *lstm = dynamic_cast<LSTM *>(f.get());
    if (lstm != nullptr) {
      Test(lstm);
      continue;
    }
    Output *output = dynamic_cast<Output *>(f.get());
    if (output != nullptr) {
      Test(output);
    }
  }
  std::cout << "\nGradient test SUCCEEDED!\n";
}

void GradientTest::Test(TableLookup *table_lookup) {
  std::cout << "\nTesting \"TableLookup\" ...\n";
  const int word_dimension = table_lookup->word_dimension_;
  TestWeights("weights",
              word_dimension * table_lookup->input_dimension(),
              table_lookup->weights_);
  if (table_lookup->bias_)
    TestWeights("bias", word_dimension, table_lookup->bias_);
  if (table_lookup->recurrency_) {
    const int output_dimension = table_lookup->output_dimension();
    TestWeights("recurrent weights",
                output_dimension * output_dimension,
                table_lookup->recurrency_->recurrent_weights_);
  }
}

void GradientTest::Test(Linear *linear) {
  std::cout << "\nTesting \"Linear\" ...\n";
  const int output_dimension = linear->output_dimension();
  TestWeights("weights",
              output_dimension * linear->input_dimension(),
              linear->weights_);
  if (linear->bias_) {
    TestWeights("bias",
                output_dimension,
                linear->bias_);
  }
  if (linear->recurrency_) {
    TestWeights("recurrent weights",
                output_dimension * output_dimension,
                linear->recurrency_->recurrent_weights_);
  }
}

void GradientTest::Test(LSTM *lstm) {
  std::cout << "\nTesting \"LSTM\" ...\n";
  int size = lstm->output_dimension() * lstm->input_dimension();
  TestWeights("weights", size, lstm->weights_);
  TestWeights("input gate weights", size, lstm->input_gate_weights_);
  TestWeights("forget gate weights", size, lstm->forget_gate_weights_);
  TestWeights("output gate weights", size, lstm->output_gate_weights_);
  size = lstm->output_dimension() * lstm->output_dimension();
  TestWeights("recurrent weights", size, lstm->recurrent_weights_);
  TestWeights("input gate recurrent weights",
              size,
              lstm->input_gate_recurrent_weights_);
  TestWeights("forget gate recurrent weights",
              size,
              lstm->forget_gate_recurrent_weights_);
  TestWeights("output gate recurrent weights",
              size,
              lstm->output_gate_recurrent_weights_);
  size = lstm->output_dimension();
  TestWeights("input gate peephole weights",
              size,
              lstm->input_gate_peephole_weights_);
  TestWeights("forget gate peephole weights",
              size,
              lstm->forget_gate_peephole_weights_);
  TestWeights("output gate peephole weights",
              size,
              lstm->output_gate_peephole_weights_);
  if (lstm->bias_) {
    TestWeights("bias",
                lstm->output_dimension(),
                lstm->bias_);
    TestWeights("input gate bias",
                lstm->output_dimension(),
                lstm->input_gate_bias_);
    TestWeights("forget gate bias",
                lstm->output_dimension(),
                lstm->forget_gate_bias_);
    TestWeights("output gate bias",
                lstm->output_dimension(),
                lstm->output_gate_bias_);
  }
}

void GradientTest::Test(Output *output) {
  std::cout << "\nTesting \"Output\" ...\n";
  const int num_classes = output->num_classes_,
            num_out_of_shortlist_words = output->num_out_of_shortlist_words_,
            input_dimension = output->input_dimension();
  TestWeights("class weights",
              num_classes * input_dimension,
              output->class_weights_);
  TestWeights("word weights",
              num_out_of_shortlist_words * input_dimension,
              output->word_weights_);
  if (output->class_bias_) {
    TestWeights("class bias",
                num_classes,
                output->class_bias_);
    TestWeights("word bias", num_out_of_shortlist_words, output->word_bias_);
  }
}

void GradientTest::ComputeDifferenceQuotient(const int size,
                                             Real weights[],
                                             Real quotient[]) {
  for (int i = 0; i < size; ++i) {
    weights[i] += epsilon_;
    const Real f1 = num_running_words_ * log(trainer_->ComputePerplexity(
        trainer_->training_data_));
    weights[i] -= 2. * epsilon_;
    const Real f0 = num_running_words_ * log(trainer_->ComputePerplexity(
        trainer_->training_data_));
    quotient[i] = (f1 - f0) / (2. * epsilon_);
    weights[i] += epsilon_;
  }
}

void GradientTest::ComputeDerivative(const int size,
                                     const Real weights[],
                                     Real derivative[]) {
  FastCopy(weights, size, derivative);
  trainer_->TrainEpoch();
  FastSub(derivative, size, weights, derivative);
  FastDivideByConstant(derivative,
                       size,
                       trainer_->net_->learning_rate(),
                       derivative);
  trainer_->random_->Reset(seed_);
  if (boost::filesystem::exists(trainer_->net_config_))
    trainer_->net_->Read(trainer_->net_config_);
  else
    trainer_->net_->RandomizeWeights(trainer_->random_);
}

void GradientTest::Compare(const std::string &name,
                           const int size,
                           const Real derivative[],
                           const Real quotient[]) const {
  std::cout << name << ":\n";
  for (int i = 0; i < size; ++i) {
    std::cout << "dF/dw:           " << std::setw(11) << std::fixed <<
                 std::setprecision(7) << derivative[i] <<
                 "\nDelta F/Delta w: " << std::setw(11) << quotient[i] <<
                 '\n';
    const Real threshold = pow(
        10.0,
        std::max(static_cast<Real>(0.0),
            static_cast<Real>(ceil(log10(std::min(fabs(derivative[i]),
            fabs(quotient[i])))))) - significant_digits_);
    assert(fabs(derivative[i] - quotient[i]) < threshold);
  }
}

void GradientTest::TestWeights(const std::string &name,
                               const int size,
                               Real weights[]) {
  // anything to check?
  if (size == 0)
    return;
  Real *quotient = FastMalloc(size);
  ComputeDifferenceQuotient(size, weights, quotient);
  Real *derivative = FastMalloc(size);
  ComputeDerivative(size, weights, derivative);
  Compare(name, size, derivative, quotient);
  FastFree(quotient);
  FastFree(derivative);
}


// Boost.Geometry

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

// This file was modified by Oracle on 2014, 2015, 2016.
// Modifications copyright (c) 2014-2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_DETAIL_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_DETAIL_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

/*!
\brief Check at which side of a segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam InverseFormula Geodesic inverse solution formula.
\tparam Model Reference model of coordinate system.
\tparam CalculationType \tparam_calculation
 */
template <template<typename, bool, bool, bool, bool, bool> class InverseFormula,
          typename Model,
          typename CalculationType = void>
class by_azimuth
{
public:
    by_azimuth(Model const& model = Model())
        : m_model(model)
    {}

    template <typename P1, typename P2, typename P>
    inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calc_t;

        typedef InverseFormula<calc_t, false, true, false, false, false> inverse_formula;

        calc_t a1p = azimuth<calc_t, inverse_formula>(p1, p, m_model);
        calc_t a12 = azimuth<calc_t, inverse_formula>(p1, p2, m_model);

        calc_t const pi = math::pi<calc_t>();

        // instead of the formula from XTD
        //calc_t a_diff = asin(sin(a1p - a12));

        calc_t a_diff = a1p - a12;
        // normalize, angle in [-pi, pi]
        while ( a_diff > pi )
            a_diff -= calc_t(2) * pi;
        while ( a_diff < -pi )
            a_diff += calc_t(2) * pi;

        // NOTE: in general it shouldn't be required to support the pi/-pi case
        // because in non-cartesian systems it makes sense to check the side
        // only "between" the endpoints.
        // However currently the winding strategy calls the side strategy
        // for vertical segments to check if the point is "between the endpoints.
        // This could be avoided since the side strategy is not required for that
        // because meridian is the shortest path. So a difference of
        // longitudes would be sufficient (of course normalized to [-pi, pi]).

        // NOTE: with the above said, the pi/-pi check is temporary
        // however in case if this was required
        // the geodesics on ellipsoid aren't "symmetrical"
        // therefore instead of comparing a_diff to pi and -pi
        // one should probably use inverse azimuths and compare
        // the difference to 0 as well

        // positive azimuth is on the right side
        return math::equals(a_diff, 0)
            || math::equals(a_diff, pi)
            || math::equals(a_diff, -pi) ? 0
             : a_diff > 0 ? -1 // right
             : 1; // left
    }

private:
    template <typename ResultType,
              typename InverseFormulaType,
              typename Point1,
              typename Point2,
              typename ModelT>
    static inline ResultType azimuth(Point1 const& point1, Point2 const& point2, ModelT const& model)
    {
        return InverseFormulaType::apply(get_as_radian<0>(point1),
                                         get_as_radian<1>(point1),
                                         get_as_radian<0>(point2),
                                         get_as_radian<1>(point2),
                                         model).azimuth;
    }

    Model m_model;
};

} // detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_SIDE_DETAIL_HPP


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 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 BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_DEINTERLEAVE_FIRST_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_SSE2_DEINTERLEAVE_FIRST_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/toolbox/swar/functions/deinterleave_first.hpp>
#include <boost/simd/toolbox/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_first_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<single_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128 const a0, __m128 const a1) const
    {
      return details::shuffle<0, 2, 0, 2>(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_first_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<double_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128d const a0, __m128d const a1) const
    {
      return _mm_unpacklo_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_first_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<ints32_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return details::shuffle<0, 2, 0, 2>(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::deinterleave_first_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<ints64_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return  _mm_unpacklo_epi64( a0, a1 );
    }
  };

} } }

#endif
#endif


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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_SYM_DIFFERENCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_SYM_DIFFERENCE_HPP

#include <algorithm>

#include <boost/geometry/algorithms/intersection.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace sym_difference
{

/*!
\brief \brief_calc2{symmetric difference}  \brief_strategy
\ingroup sym_difference
\details \details_calc2{symmetric difference, spatial set theoretic symmetric difference (XOR)}
    \brief_strategy. \details_insert{sym_difference}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy \tparam_strategy_overlay
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{difference}
\param strategy \param_strategy{difference}
\return \return_out

\qbk{distinguish,with strategy}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator sym_difference_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2, OutputIterator out,
            Strategy const& strategy)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    out = geometry::dispatch::intersection_insert
        <
            Geometry1, Geometry2,
            GeometryOut,
            overlay_difference,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry2>::value, true>::value
        >::apply(geometry1, geometry2, out, strategy);
    out = geometry::dispatch::intersection_insert
        <
            Geometry2, Geometry1,
            GeometryOut,
            overlay_difference,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry2>::value>::value,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry1>::value, true>::value,
            geometry::detail::overlay::do_reverse<geometry::point_order<GeometryOut>::value>::value
        >::apply(geometry2, geometry1, out, strategy);
    return out;
}

/*!
\brief \brief_calc2{symmetric difference}
\ingroup sym_difference
\details \details_calc2{symmetric difference, spatial set theoretic symmetric difference (XOR)}
    \details_insert{sym_difference}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{difference}
\return \return_out

*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename OutputIterator
>
inline OutputIterator sym_difference_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2, OutputIterator out)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    typedef strategy_intersection
        <
            typename cs_tag<GeometryOut>::type,
            Geometry1,
            Geometry2,
            typename geometry::point_type<GeometryOut>::type
        > strategy_type;

    return sym_difference_insert<GeometryOut>(geometry1, geometry2, out, strategy_type());
}

}} // namespace detail::sym_difference
#endif // DOXYGEN_NO_DETAIL

/*!
\brief \brief_calc2{symmetric difference}
\ingroup sym_difference
\details \details_calc2{symmetric difference, spatial set theoretic symmetric difference (XOR)}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Collection output collection, either a multi-geometry,
    or a std::vector<Geometry> / std::deque<Geometry> etc
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param output_collection the output collection

\qbk{[include reference/algorithms/sym_difference.qbk]}
*/
template
<
    typename Geometry1,
    typename Geometry2,
    typename Collection
>
inline void sym_difference(Geometry1 const& geometry1,
            Geometry2 const& geometry2, Collection& output_collection)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef typename boost::range_value<Collection>::type geometry_out;
    concept::check<geometry_out>();

    detail::sym_difference::sym_difference_insert<geometry_out>(
            geometry1, geometry2,
            std::back_inserter(output_collection));
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_SYM_DIFFERENCE_HPP


/**
 * This program performance tests for the range image likelihood library.
 *
 *  Esc - quit
 *
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <boost/shared_ptr.hpp>
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#endif
#include <GL/glew.h>

#include <pcl/pcl_config.h>
#ifdef OPENGL_IS_A_FRAMEWORK
# include <OpenGL/gl.h>
# include <OpenGL/glu.h>
#else
# include <GL/gl.h>
# include <GL/glu.h>
#endif
#ifdef GLUT_IS_A_FRAMEWORK
# include <GLUT/glut.h>
#else
# include <GL/glut.h>
#endif

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/common/common.h>
#include <pcl/common/transforms.h>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>

// define the following in order to eliminate the deprecated headers warning
#define VTK_EXCLUDE_STRSTREAM_HEADERS
#include <pcl/io/vtk_lib_io.h>

#include <kinect_sim/camera.h>
#include <kinect_sim/model.h>
#include <kinect_sim/scene.h>
#include <kinect_sim/range_likelihood.h>

#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

using namespace Eigen;
using namespace pcl;
using namespace pcl::console;
using namespace pcl::io;
using namespace pcl::simulation;
using namespace std;

uint16_t t_gamma[2048];

Scene::Ptr scene_;
Camera::Ptr camera_;
RangeLikelihood::Ptr range_likelihood_;
// This is only used for displaying
RangeLikelihood::Ptr range_likelihood_visualization_;

int cols_;
int rows_;
int col_width_;
int row_height_;
int window_width_;
int window_height_;
TexturedQuad::Ptr textured_quad_;

void
printHelp (int, char **argv)
{
  print_error ("Syntax is: %s <filename>\n", argv[0]);
  print_info ("acceptable filenames include vtk, obj and ply. ply can support color\n");
}

void
display_score_image (const float* score_buffer)
{
  int npixels = range_likelihood_->getWidth () * range_likelihood_->getHeight ();
  uint8_t* score_img = new uint8_t[npixels * 3];

  float min_score = score_buffer[0];
  float max_score = score_buffer[0];
  for (int i=1; i<npixels; i++)
  {
    if (score_buffer[i] < min_score) min_score = score_buffer[i];
    if (score_buffer[i] > max_score) max_score = score_buffer[i];
  }
  for (int i=0; i < npixels; i++)
  {
    float d = (score_buffer[i]-min_score)/(max_score-min_score);
    score_img[3*i+0] = 0;
    score_img[3*i+1] = static_cast<unsigned char> (d*255);
    score_img[3*i+2] = 0;
  }
  textured_quad_->setTexture (score_img);
  textured_quad_->render ();

  delete [] score_img;
}

void display_depth_image (const float* depth_buffer, int width, int height)
{
  int npixels = width * height;
  uint8_t* depth_img = new uint8_t[npixels * 3];

  float min_depth = depth_buffer[0];
  float max_depth = depth_buffer[0];
  for (int i = 1; i < npixels; ++i)
  {
    if (depth_buffer[i] < min_depth) min_depth = depth_buffer[i];
    if (depth_buffer[i] > max_depth) max_depth = depth_buffer[i];
  }

  for (int i = 0; i < npixels; ++i)
  {
    float zn = 0.7f;
    float zf = 20.0f;
    float d = depth_buffer[i];
    float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
    float b = 0.075f;
    float f = 580.0f;
    int kd = static_cast<int>(1090 - b*f/z*8);
    if (kd < 0) kd = 0;
    else if (kd > 2047) kd = 2047;

    int pval = t_gamma[kd];
    uint8_t lb = static_cast<uint8_t> (pval & 0xff);
    switch (pval >> 8)
    {
      case 0:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
	  depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
	  break;
      case 1:
	  depth_img[3*i+0] = 255;
	  depth_img[3*i+1] = lb;
	  depth_img[3*i+2] = 0;
	  break;
      case 2:
	  depth_img[3*i+0] = static_cast<uint8_t> (255-lb);
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = 0;
	  break;
      case 3:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 255;
	  depth_img[3*i+2] = lb;
	  break;
      case 4:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = static_cast<uint8_t> (255-lb);
	  depth_img[3*i+2] = 255;
	  break;
      case 5:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = static_cast<uint8_t> (255-lb);
	  break;
      default:
	  depth_img[3*i+0] = 0;
	  depth_img[3*i+1] = 0;
	  depth_img[3*i+2] = 0;
	  break;
    }
  }

  glRasterPos2i (-1,-1);
  glDrawPixels (width, height,
                GL_RGB, GL_UNSIGNED_BYTE, depth_img);

  delete [] depth_img;
}

void
display ()
{
  float* reference = new float[range_likelihood_->getRowHeight () * range_likelihood_->getColWidth ()];
  const float* depth_buffer = range_likelihood_->getDepthBuffer ();
  // Copy one image from our last as a reference.
  for (int i = 0, n = 0; i < range_likelihood_->getRowHeight (); ++i)
  {
    for (int j = 0; j < range_likelihood_->getColWidth (); ++j)
    {
      reference[n++] = depth_buffer[ (i + range_likelihood_->getRowHeight () * range_likelihood_->getRows () / 2) * range_likelihood_->getWidth ()
                                    + j + range_likelihood_->getColWidth () * range_likelihood_->getCols () / 2];
    }
  }

  float* reference_vis = new float[range_likelihood_visualization_->getRowHeight () * range_likelihood_visualization_->getColWidth ()];
  const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer ();
  // Copy one image from our last as a reference.
  for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight (); ++i)
  {
    for (int j = 0; j < range_likelihood_visualization_->getColWidth (); ++j)
    {
      reference_vis[n++] = depth_buffer_vis[i*range_likelihood_visualization_->getWidth () + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  std::vector<float> scores;

  // Render a single pose for visualization
  poses.clear ();
  poses.push_back (camera_->getPose ());
  range_likelihood_visualization_->computeLikelihoods (reference_vis, poses, scores);

  glDrawBuffer (GL_BACK);
  glReadBuffer (GL_BACK);

  // Draw the resulting images from the range_likelihood
  glViewport (range_likelihood_visualization_->getWidth (), 0,
              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();

  // Draw the color image
  glColorMask (true, true, true, true);
  glClearColor (0, 0, 0, 0);
  glClearDepth (1);
  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glDisable (GL_DEPTH_TEST);

  glRasterPos2i (-1,-1);
  glDrawPixels (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
                GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());

  // Draw the depth image
  glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();
  display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
                       range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());

  poses.clear ();
  for (int i = 0; i < range_likelihood_->getRows (); ++i)
  {
    for (int j = 0; j < range_likelihood_->getCols (); ++j)
    {
      Camera camera (*camera_);
      camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
                   (i - range_likelihood_->getRows () / 2.0) * 0.1,
                   0.0);
      poses.push_back (camera.getPose ());
    }
  }
  std::cout << std::endl;

  TicToc tt;
  tt.tic();
  range_likelihood_->computeLikelihoods (reference, poses, scores);
  tt.toc();
  tt.toc_print();

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
  }

#if 0
  std::cout << "score: ";
  for (size_t i = 0; i < scores.size (); ++i)
  {
    std::cout << " " << scores[i];
  }
  std::cout << std::endl;
#endif

  std::cout << "camera: " << camera_->getX ()
       << " " << camera_->getY ()
       << " " << camera_->getZ ()
       << " " << camera_->getRoll ()
       << " " << camera_->getPitch ()
       << " " << camera_->getYaw ()
       << std::endl;

  delete [] reference_vis;
  delete [] reference;

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: before buffers" << std::endl;
  }

  glBindFramebuffer (GL_FRAMEBUFFER, 0);
  glDrawBuffer (GL_BACK);
  glReadBuffer (GL_BACK);

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: after buffers" << std::endl;
  }

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glMatrixMode (GL_MODELVIEW);
  glLoadIdentity ();

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: before viewport" << std::endl;
  }

  // Draw the score image for the particles
  glViewport (0, range_likelihood_visualization_->getHeight (),
              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());

  if (gllib::getGLError () != GL_NO_ERROR)
  {
    std::cerr << "GL Error: after viewport" << std::endl;
  }

  display_score_image (range_likelihood_->getScoreBuffer ());

  // Draw the depth image for the particles
  glViewport (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
              range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());

  display_score_image (range_likelihood_->getDepthBuffer ());

  glutSwapBuffers ();
}

// Handle normal keys
void
on_keyboard (unsigned char key, int, int)
{
  if (key == 27)
    exit (0);
}

// Read in a 3D model
void
loadPolygonMeshModel (char* polygon_file)
{
  pcl::PolygonMesh mesh;
  pcl::io::loadPolygonFile (polygon_file, mesh);
  pcl::PolygonMesh::Ptr cloud (new pcl::PolygonMesh (mesh));

  TriangleMeshModel::Ptr model = TriangleMeshModel::Ptr (new TriangleMeshModel (cloud));
  scene_->add (model);

  std::cout << "Just read " << polygon_file << std::endl;
  std::cout << mesh.polygons.size () << " polygons and "
	    << mesh.cloud.data.size () << " triangles\n";
}

void
initialize (int argc, char** argv)
{
  const GLubyte* version = glGetString (GL_VERSION);
  print_info ("OpenGL Version: %s\n", version);

  // works well for MIT CSAIL model 2nd floor:
  camera_->set (27.4503, 37.383, 4.30908, 0.0, 0.0654498, -2.25802);

  if (argc > 1) loadPolygonMeshModel (argv[1]);
}

int
main (int argc, char** argv)
{
  int width = 640;
  int height = 480;

  window_width_ = width * 2;
  window_height_ = height * 2;

  int cols = 30;
  int rows = 30;
  int col_width = 20;
  int row_height = 15;

  print_info ("Range likelihood performance tests using pcl::simulation. For more information, use: %s -h\n", argv[0]);

  if (argc < 2)
  {
    printHelp (argc, argv);
    return (-1);
  }

  for (int i = 0; i < 2048; ++i)
  {
    float v = static_cast<float> (i / 2048.0);
    v = powf(v, 3)* 6;
    t_gamma[i] = static_cast<uint16_t> (v*6*256);
  }

  glutInit (&argc, argv);
  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
  glutInitWindowPosition (10, 10);
  glutInitWindowSize (window_width_, window_height_);
  glutCreateWindow ("OpenGL range likelihood");

  GLenum err = glewInit ();
  if (GLEW_OK != err)
  {
    std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
    exit (-1);
  }

  std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;

  if (glewIsSupported ("GL_VERSION_2_0"))
    std::cout << "OpenGL 2.0 supported" << std::endl;
  else
  {
    std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
    exit (1);
  }

  std::cout << "GL_MAX_VIEWPORTS: " << GL_MAX_VIEWPORTS << std::endl;

  camera_ = Camera::Ptr (new Camera ());
  scene_ = Scene::Ptr (new Scene ());

  range_likelihood_visualization_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
  range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood (rows, cols, row_height, col_width, scene_));

  // Actually corresponds to default parameters:
  range_likelihood_visualization_->setCameraIntrinsicsParameters (
      640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
  range_likelihood_visualization_->setComputeOnCPU (false);
  range_likelihood_visualization_->setSumOnCPU (false);
  range_likelihood_visualization_->setUseColor (true);

  range_likelihood_->setCameraIntrinsicsParameters (
      640, 480, 576.09757860f, 576.09757860f, 321.06398107f, 242.97676897f);
  range_likelihood_->setComputeOnCPU (false);
  range_likelihood_->setSumOnCPU (false);
  range_likelihood_->setUseColor (false);

  textured_quad_ = TexturedQuad::Ptr (new TexturedQuad (range_likelihood_->getWidth (),
                                                        range_likelihood_->getHeight ()));

  initialize (argc, argv);

  glutDisplayFunc (display);
  glutIdleFunc (display);
  glutKeyboardFunc (on_keyboard);
  glutMainLoop ();
}


///////////////////////////////////////////////////////////////////////////////
// vec_levels.hpp                                                            //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_FACTOR_VEC_LEVELS_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_FACTOR_VEC_LEVELS_HPP_ER_2010
#include <boost/mpl/fold.hpp>
#include <boost/mpl/vector/vector0.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{

   template<typename Keys>
    struct vec_levels : boost::mpl::fold<
        Keys,
        boost::mpl::vector0<>,
        boost::mpl::push_back<
            boost::mpl::_1,
            contingency_table::tag::levels<boost::mpl::_2>
        >
    >{};

}// contingency_table
}// detail
}// statistics
}// boost

#endif