/*=============================================================================
Copyright (c) 2011, The Trustees of Indiana University
All rights reserved.

Authors: Michael Hansen (mihansen@indiana.edu), Shinya Ito

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
     this list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.

  3. Neither the name of Indiana University nor the names of its contributors
     may be used to endorse or promote products derived from this software
     without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
=============================================================================*/

#include <bitset>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <cassert>

#include <boost/mpl/arithmetic.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/limits.hpp>

#define MAX_XY_ORDER 64

namespace mpl = boost::mpl;
namespace boost { namespace mpl {
  template <std::size_t N, std::size_t Power>
  struct pow
  {
    static const std::size_t value = N * pow<N, Power - 1>::value;
  };

  template <std::size_t N>
  struct pow<N, 0>
  {
    static const std::size_t value = 1;
  };

} }

// Computes the higher-order transfer entropy matrix for all pairs.
// x and y orders must be known at compile time.
template <typename TimeSeriesCollection, typename ResultMatrix,
         std::size_t x_order, std::size_t y_order>
void transent_ho
(const TimeSeriesCollection& all_series,
 const typename TimeSeriesCollection::value_type::value_type y_delay,
 const typename TimeSeriesCollection::value_type::value_type duration,
 ResultMatrix& te_result,
 std::size_t row_start = 0, std::size_t rows = 0,
 std::size_t col_start = 0, std::size_t cols = 0) {

  // Typedefs
  typedef typename TimeSeriesCollection::value_type TimeSeries;
  typedef typename TimeSeries::value_type TimeType;
  typedef typename TimeSeries::const_iterator TimeSeriesIter;

  typedef std::pair<TimeSeriesIter, TimeType> IterShiftPair;
  typedef std::pair<TimeType, std::size_t> TimeIndexPair;

  // Constants
  const std::size_t num_series = 1 + y_order + x_order,
                    num_counts = mpl::pow<2, num_series>::value,
                    num_x = mpl::pow<2, mpl::plus<mpl::int_<x_order>, mpl::int_<1> >::value>::value,
                    num_y = mpl::pow<2, y_order>::value;

  BOOST_STATIC_ASSERT(x_order > 0);
  BOOST_STATIC_ASSERT(y_order > 0);
  assert(y_delay > 0);
  BOOST_STATIC_ASSERT(num_series <= MAX_XY_ORDER);

  if (rows == 0) {
    rows = all_series.size();
  }

  if (cols == 0) {
    cols = all_series.size();
  }

  // Locals
  std::vector<TimeType> counts(num_counts);
  std::bitset<MAX_XY_ORDER> code;
  std::size_t idx = 0;
  double te_final, prob_2, prob_3;

  IterShiftPair ord_iter[num_series];
  TimeType ord_times[num_series];
  TimeSeriesIter ord_end[num_series];

  const std::size_t window = std::max(y_order + y_delay, x_order + 1);
  TimeType cur_time, next_time, end_time = duration - window + 1, shift;

  // Calculate TE
  for (std::size_t i = row_start; i < (rows + row_start); ++i) {
    for (std::size_t j = col_start; j < (cols + col_start); ++j) {

      // NOTE: Time series are assumed to be 1-based, so everything is shifted by 1 too.
      // Order is x^(k+1), y^(l)
      idx = 0;

      // x^(k+1)
      for (std::size_t k = 0; k < (x_order + 1); ++k) {
        shift = (window - 1) - k;
        ord_end[idx] = all_series[i].end();
        ord_iter[idx] = std::make_pair(std::lower_bound(all_series[i].begin(), ord_end[idx], shift + 1), shift);
        ord_times[idx] = *(ord_iter[idx].first) - ord_iter[idx].second;
        ++idx;
      }

      // y^(l)
      for (std::size_t k = 0; k < y_order; ++k) {
        shift = (window - 1) - y_delay - k;
        ord_end[idx] = all_series[j].end();
        ord_iter[idx] = std::make_pair(std::lower_bound(all_series[j].begin(), ord_end[idx], shift + 1), shift);
        ord_times[idx] = *(ord_iter[idx].first) - ord_iter[idx].second;
        ++idx;
      }

      // Count spikes
      std::fill(counts.begin(), counts.end(), 0);
      cur_time = *(std::min_element(ord_times, ord_times + num_series));

      while (cur_time <= end_time) {

        code.reset();
        next_time = std::numeric_limits<TimeType>::max();

        // Calculate hash code for this time
        for (std::size_t k = 0; k < num_series; ++k) {
          if (ord_times[k] == cur_time) {
            code[k] = 1;

            // Next spike
            ++(ord_iter[k].first);

            if (ord_iter[k].first == ord_end[k]) {
              ord_times[k] = std::numeric_limits<TimeType>::max();
            }
            else {
              ord_times[k] = *(ord_iter[k].first) - ord_iter[k].second;
            }
          }

          if (ord_times[k] < next_time) {
            next_time = ord_times[k];
          }
        }

        ++(counts[code.to_ulong()]);
        cur_time = next_time;

      } // while spikes left

      // Fill in zero count
      counts[0] = end_time - std::accumulate(counts.begin() + 1, counts.end(), 0);

      // =====================================================================

      // Use counts to calculate TE
      te_final = 0;

      // Order is x^(k), y^(l), x(n+1)
      for (std::size_t k = 0; k < num_counts; ++k) {
        if (counts[k] == 0) {
          continue;
        }

        prob_2 = (double)counts[k] / (double)(counts[k] + counts[k ^ 1]);

        std::size_t c1 = 0, c2 = 0;
        for (std::size_t l = 0; l < num_y; ++l) {
          idx = (k & (num_x - 1)) + (l << (x_order + 1));
          c1 += counts[idx];
          c2 += (counts[idx] + counts[idx ^ 1]);
        }

        prob_3 = (double)c1 / (double)c2;

        te_final += ((double)counts[k] * (log2(prob_2) - log2(prob_3)));
      }

      te_result[i - row_start][j - col_start] = te_final / (double)end_time;

    } // for j

  } // for i

} //transent_ho

// Computes the 1st order transfer entropy matrix for all pairs.
template <typename TimeSeriesCollection, typename ResultMatrix>
void transent_1
(const TimeSeriesCollection& all_series,
 const typename TimeSeriesCollection::value_type::value_type y_delay,
 const typename TimeSeriesCollection::value_type::value_type duration,
 ResultMatrix& te_result,
 std::size_t row_start = 0, std::size_t rows = 0,
 std::size_t col_start = 0, std::size_t cols = 0) {

  return (transent_ho<TimeSeriesCollection, ResultMatrix, 1, 1>
          (all_series, y_delay, duration, te_result,
           row_start, rows, col_start, cols));

} // transent_1

// Computes the higher-order transfer entropy matrix for all pairs.
template <typename TimeSeriesCollection, typename ResultMatrix>
void transent_ho
(const TimeSeriesCollection& all_series,
 const typename std::size_t x_order, std::size_t y_order,
 const typename TimeSeriesCollection::value_type::value_type y_delay,
 const typename TimeSeriesCollection::value_type::value_type duration,
 ResultMatrix& te_result,
 std::size_t row_start = 0, std::size_t rows = 0,
 std::size_t col_start = 0, std::size_t cols = 0) {

  // Typedefs
  typedef typename TimeSeriesCollection::value_type TimeSeries;
  typedef typename TimeSeries::value_type TimeType;
  typedef typename TimeSeries::const_iterator TimeSeriesIter;

  typedef std::pair<TimeSeriesIter, TimeType> IterShiftPair;
  typedef std::pair<TimeType, std::size_t> TimeIndexPair;

  // Constants
  const std::size_t num_series = 1 + y_order + x_order,
                    num_counts = (std::size_t)pow(2, num_series),
                    num_x = (std::size_t)pow(2, x_order + 1),
                    num_y = (std::size_t)pow(2, y_order);

  assert(x_order > 0);
  assert(y_order > 0);
  assert(y_delay > 0);
  assert(num_series <= MAX_XY_ORDER);

  if (rows == 0) {
    rows = all_series.size();
  }

  if (cols == 0) {
    cols = all_series.size();
  }

  // Locals
  std::vector<TimeType> counts(num_counts);
  std::bitset<MAX_XY_ORDER> code;
  std::size_t idx = 0;
  double te_final, prob_2, prob_3;

  IterShiftPair ord_iter[num_series];
  TimeType ord_times[num_series];
  TimeSeriesIter ord_end[num_series];

  const std::size_t window = std::max(y_order + y_delay, x_order + 1);
  TimeType cur_time, next_time, end_time = duration - window + 1, shift;

  // Calculate TE
  for (std::size_t i = row_start; i < (rows + row_start); ++i) {
    for (std::size_t j = col_start; j < (cols + col_start); ++j) {

      // NOTE: Time series are assumed to be 1-based, so everything is shifted by 1 too.
      // Order is x^(k+1), y^(l)
      idx = 0;

      // x^(k+1)
      for (std::size_t k = 0; k < (x_order + 1); ++k) {
        shift = (window - 1) - k;
        ord_end[idx] = all_series[i].end();
        ord_iter[idx] = std::make_pair(std::lower_bound(all_series[i].begin(), ord_end[idx], shift + 1), shift);
        ord_times[idx] = *(ord_iter[idx].first) - ord_iter[idx].second;
        ++idx;
      }

      // y^(l)
      for (std::size_t k = 0; k < y_order; ++k) {
        ord_end[idx] = all_series[j].end();
        ord_iter[idx] = std::make_pair(all_series[j].begin(), -k);
        ord_times[idx] = *(ord_iter[idx].first) - ord_iter[idx].second;
        ++idx;
      }

      // Count spikes
      std::fill(counts.begin(), counts.end(), 0);
      cur_time = *(std::min_element(ord_times, ord_times + num_series));

      while (cur_time <= end_time) {

        code.reset();
        next_time = std::numeric_limits<TimeType>::max();

        // Calculate hash code for this time
        for (std::size_t k = 0; k < num_series; ++k) {
          if (ord_times[k] == cur_time) {
            code[k] = 1;

            // Next spike
            ++(ord_iter[k].first);

            if (ord_iter[k].first == ord_end[k]) {
              ord_times[k] = std::numeric_limits<TimeType>::max();
            }
            else {
              ord_times[k] = *(ord_iter[k].first) - ord_iter[k].second;
            }
          }

          if (ord_times[k] < next_time) {
            next_time = ord_times[k];
          }
        }

        ++(counts[code.to_ulong()]);
        cur_time = next_time;

      } // while spikes left

      // Fill in zero count
      counts[0] = end_time - std::accumulate(counts.begin() + 1, counts.end(), 0);

      // =====================================================================

      // Use counts to calculate TE
      te_final = 0;

      // Order is x^(k), y^(l), x(n+1)
      for (std::size_t k = 0; k < num_counts; ++k) {
        if (counts[k] == 0) {
          continue;
        }

        prob_2 = (double)counts[k] / (double)(counts[k] + counts[k ^ 1]);

        std::size_t c1 = 0, c2 = 0;
        for (std::size_t l = 0; l < num_y; ++l) {
          idx = (k & (num_x - 1)) + (l << (x_order + 1));
          c1 += counts[idx];
          c2 += (counts[idx] + counts[idx ^ 1]);
        }

        prob_3 = (double)c1 / (double)c2;

        te_final += ((double)counts[k] * (log2(prob_2) - log2(prob_3)));
      }

      te_result[i - row_start][j - col_start] = te_final / (double)end_time;

    } // for j

  } // for i

} // transent_ho


//==================================================================================================
/*!
  @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_SIMD_LOG2_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_LOG2_INCLUDED

#include <boost/simd/function/scalar/log2.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/log2.hpp>

#endif


/*
    This file is part of control-lib.

    Copyright (c) 2020, 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef CONTROLLIB_SPATIAL_RN_HPP
#define CONTROLLIB_SPATIAL_RN_HPP

#include <Eigen/Geometry>

namespace control_lib {
    namespace spatial {
        template <size_t N>
        struct RN {
            /* Init via translation and orientation */
            RN(const Eigen::Matrix<double, N, 1>& pos) : _pos(pos) {}

            RN() = default;

            /* Space elements difference */
            Eigen::Matrix<double, N, 1> operator-(RN const& obj) const { return _pos - obj._pos; }

            /* Space dimension */
            constexpr static size_t dimension() { return N; }

            /* Tangent and contagent plane elements (optionals) */
            Eigen::Matrix<double, N, 1> _pos, _vel, _acc, _eff;
        };
    } // namespace spatial

} // namespace control_lib

#endif // CONTROLLIB_SPATIAL_RN_HPP


#include "biggles/observation.hpp"
#include "biggles/detail/random.hpp"
#include "biggles/mh_moves/mh_moves.hpp"
#include "biggles/mh_moves/utility.hpp"
#include "biggles/simulate.hpp"
#include <boost/foreach.hpp>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <fstream>
#include <algorithm>
#include <string>
#include "biggles/tools/sundries.hpp"
#include "biggles/tools/partition_graph_edit_distance.hpp"

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

void test1() {
    std::deque<observation> obs_list;
    obs_list.push_back(new_obs(1, 1, 0));
    obs_list.push_back(new_obs(1, 1, 1));
    obs_list.push_back(new_obs(1, 1, 2));
    obs_list.push_back(new_obs(1, 1, 3));
    obs_list.push_back(new_obs(1, 1, 4));
    clutter_ptr clutter1_p(new clutter_t());
    clutter1_p->insert(obs_list.begin(), obs_list.end());
    boost::shared_ptr<track_collection> tracks1_p(new track_collection());
    clutter_ptr clutter2_p(new clutter_t());
    boost::shared_ptr<track_collection> tracks2_p(new track_collection());
    tracks2_p->insert(shared_track_ptr(new track(0, 5, obs_list.begin(), obs_list.end())));
    partition part1(tracks1_p, clutter1_p);
    partition part2(tracks2_p, clutter2_p);
    ok(partition_ged(part1, part2) == 4.f, "dist(part1, part2) == 4");
    ok(partition_ged(part1, part1) == 0.f, "Partition 1 is equal to itself");
    ok(partition_ged(part2, part2) == 0.f, "Partition 2 is equal to itself");
}

void test2() {
    std::deque<observation> obs_list1;
    obs_list1.push_back(new_obs(1, 1, 0));
    obs_list1.push_back(new_obs(1, 1, 1));
    obs_list1.push_back(new_obs(1, 1, 2));
    obs_list1.push_back(new_obs(1, 1, 3));
    obs_list1.push_back(new_obs(1, 1, 4));
    std::deque<observation> obs_list2;
    obs_list2.push_back(new_obs(0, 1, 0));
    obs_list2.push_back(new_obs(0, 1, 1));
    obs_list2.push_back(new_obs(0, 1, 2));
    obs_list2.push_back(new_obs(0, 1, 3));
    obs_list2.push_back(new_obs(0, 1, 4));

    clutter_ptr clutter1_p(new clutter_t());
    boost::shared_ptr<track_collection> tracks1_p(new track_collection());
    clutter_ptr clutter2_p(new clutter_t());
    boost::shared_ptr<track_collection> tracks2_p(new track_collection());
    track t11;
    track t12;
    track t21;
    track t22;
    t11.insert(obs_list1[0]);
    t11.insert(obs_list1[1]);
    t11.insert(obs_list1[2]);
    t11.insert(obs_list2[3]);
    t11.insert(obs_list2[4]);

    t12.insert(obs_list2[0]);
    t12.insert(obs_list2[1]);
    t12.insert(obs_list2[2]);
    t12.insert(obs_list1[3]);
    t12.insert(obs_list1[4]);

    t21.insert(obs_list1[0]);
    t21.insert(obs_list1[1]);
    t21.insert(obs_list1[2]);
    t21.insert(obs_list1[3]);
    t21.insert(obs_list1[4]);

    t22.insert(obs_list2[0]);
    t22.insert(obs_list2[1]);
    t22.insert(obs_list2[2]);
    t22.insert(obs_list2[3]);
    t22.insert(obs_list2[4]);

    tracks1_p->insert(t11);
    tracks1_p->insert(t12);

    tracks2_p->insert(t21);
    tracks2_p->insert(t22);

    partition part1(tracks1_p, clutter1_p);
    partition part2(tracks2_p, clutter2_p);

    diag("distance = %.1f", partition_ged(part1, part2));
    ok(partition_ged(part1, part2) == 4.f, "dist(part1, part2) == 4");
}

int main(int argc, char** argv) {

    general_paras gen_pars;
    gen_pars.total = 200000;
    gen_pars.seed = 0;
    gen_pars.rgp.lambda = 2.5;
    gen_pars.rgp.p_no = 0.2;
    gen_pars.rgp.p_yes = 0.2;
    gen_pars.rgp.p_tr = 0.5;
    gen_pars.rgp.min_tracks = 1;
    gen_pars.rgp.max_tracks = 2;
    if (not parse_args(argc, argv, gen_pars))
        return exit_status();
    biggles::detail::seed_prng(gen_pars.seed);

    plan_no_plan();

    //test1();
    test2();

    diag("random seed = 0x%x", gen_pars.seed);
    return exit_status();
}


// Copyright 2017 Emanuele Palazzolo (emanuele.palazzolo@uni-bonn.de), Cyrill Stachniss, University of Bonn
#include "fastcd/camera.h"
#include <glow/glutil.h>
#include <boost/foreach.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

namespace fastcd {

void Camera::ReadCalibration(const std::string& filename, int id) {
  boost::property_tree::ptree pt, resolution, camera;
  read_xml(filename, pt);

  double cx = pt.get<double>("document.chunk.sensors.sensor.calibration.cx");
  double cy = pt.get<double>("document.chunk.sensors.sensor.calibration.cy");
  double fx = pt.get<double>("document.chunk.sensors.sensor.calibration.fx");
  double fy = pt.get<double>("document.chunk.sensors.sensor.calibration.fy");
  resolution =
      pt.get_child("document.chunk.sensors.sensor.calibration.resolution");
  width_ = resolution.get<int>("<xmlattr>.width");
  height_ = resolution.get<int>("<xmlattr>.height");

  BOOST_FOREACH(boost::property_tree::ptree::value_type const& v,
                 pt.get_child("document.chunk.cameras")) {
    if (v.first == "camera" && v.second.get<int>("<xmlattr>.id") == id) {
      std::string s = v.second.get<std::string>("transform");
      std::stringstream sstream(s);
      for (size_t i = 0; i < 4; i++)
        for (size_t j = 0; j < 4; j++) sstream >> pose_(i, j);
    }
  }

  calibration_.setIdentity();
  calibration_(0, 0) = fx;
  calibration_(1, 1) = fy;
  calibration_(0, 2) = cx;
  calibration_(1, 2) = cy;
  inverse_calibration_ = calibration_.inverse();
}

void Camera::ScaleCalibration(double scaling) {
  Eigen::Matrix3d k_transform;
  k_transform << scaling, 0, scaling/2-0.5,
                 0, scaling, scaling/2-0.5,
                 0,       0,             1;
  width_ *= scaling;
  height_ *= scaling;
  calibration_ = k_transform * calibration_;
  inverse_calibration_ = calibration_.inverse();
}

Eigen::Vector2i Camera::Project(double x, double y, double z) const {
  Eigen::Vector4d world_point;
  Eigen::Vector3d camera_point;
  Eigen::Matrix<double, 3, 4> P;
  Eigen::Matrix3d R = pose_.block<3, 3>(0, 0).transpose();
  world_point << x, y, z, 1;
  P.block<3, 3>(0, 0) = calibration_ * R;
  P.block<3, 1>(0, 3) = -calibration_ * R * pose_.block<3, 1>(0, 3);
  camera_point = P * world_point;
  Eigen::Vector2i uv_coord;
  uv_coord << static_cast<int>(camera_point(0) / camera_point(2)),
      static_cast<int>(camera_point(1) / camera_point(2));
  return uv_coord;
}

Eigen::Vector2i Camera::Project(const Eigen::Vector3d& point) const {
  return Project(point(0), point(1), point(2));
}

Eigen::Vector3d Camera::BackProject(int u, int v) {
  Eigen::Vector3d camera_point;
  camera_point << u, v, 1;
  Eigen::Vector4d world_point, hom_direction;
  Eigen::Vector3d direction;
  direction = inverse_calibration_ * camera_point;
  hom_direction << direction(0), direction(1), direction(2), 1;
  world_point = pose_ * hom_direction;
  direction << world_point(0) / world_point(3), world_point(1) / world_point(3),
      world_point(2) / world_point(3);
  return direction;
}

Eigen::Vector3d Camera::BackProject(const Eigen::Vector2i& point) {
  return BackProject(point(0), point(1));
}

std::vector<float> Camera::GetGlFovVertices(float color, float length) {
  Eigen::Vector3f position = GetPosition().cast<float>();

  Eigen::Vector3f direction_tl = BackProject(0, 0).cast<float>();
  Eigen::Vector3f direction_bl = BackProject(0, height_).cast<float>();
  Eigen::Vector3f direction_tr = BackProject(width_, 0).cast<float>();
  Eigen::Vector3f direction_br = BackProject(width_, height_).cast<float>();
  Eigen::Vector3f corner_tl = (direction_tl - position) * length + position;
  Eigen::Vector3f corner_bl = (direction_bl - position) * length + position;
  Eigen::Vector3f corner_tr = (direction_tr - position) * length + position;
  Eigen::Vector3f corner_br = (direction_br - position) * length + position;

  std::vector<float> verts({position(0),  position(1),  position(2),  color,
                            corner_tl(0), corner_tl(1), corner_tl(2), color,
                            position(0),  position(1),  position(2),  color,
                            corner_bl(0), corner_bl(1), corner_bl(2), color,
                            position(0),  position(1),  position(2),  color,
                            corner_tr(0), corner_tr(1), corner_tr(2), color,
                            position(0),  position(1),  position(2),  color,
                            corner_br(0), corner_br(1), corner_br(2), color});
  return verts;
}

std::ostream& operator<<(std::ostream& os, const Camera& c) {
  os << "Resolution: " << c.width_ << "x" << c.height_ << std::endl;
  os << "K:\n" << c.calibration_ << std::endl;
  os << "\nPose:\n" << c.pose_ << std::endl;
  return os;
}

Eigen::Matrix3d Camera::GetK() const { return calibration_; }

Eigen::Matrix3d Camera::GetInvK() const { return inverse_calibration_; }

Eigen::Matrix4d Camera::GetPose() const { return pose_; }

Eigen::Vector3d Camera::GetPosition() const {
  return pose_.block<3, 1>(0, 3);
}

Eigen::Matrix<double, 3, 4> Camera::GetP() const {
  Eigen::Matrix<double, 3, 4> P;
  Eigen::Matrix3d R = pose_.block<3, 3>(0, 0).transpose();
  P.block<3, 3>(0, 0) = calibration_ * R;
  P.block<3, 1>(0, 3) = -calibration_ * R * pose_.block<3, 1>(0, 3);
  return P;
}

Eigen::Matrix4f Camera::GetGlProjection(float near, float far) {
  float left = 0.0f;
  float right = static_cast<float>(width_);
  float bottom = 0.0f;
  float top = static_cast<float>(height_);
  Eigen::Matrix4f gl_ortho, gl_persp, gl_camera;
  gl_persp.setZero();
  gl_persp.block<3, 3>(0, 0) = calibration_.cast<float>();
  gl_persp.block<2, 1>(0, 2) *= -1;
  gl_persp(2, 2) = near + far;
  gl_persp(2, 3) = near * far;
  gl_persp(3, 2) = -1;

  gl_ortho << 2 / (right - left), 0, 0, -(right + left) / (right - left),
              0, 2 / (top - bottom), 0, -(top + bottom) / (top - bottom),
              0, 0, -2 / (far - near), -(far + near) / (far - near),
              0, 0, 0, 1;

  gl_camera = gl_ortho * gl_persp;

  return gl_camera;
}

Eigen::Matrix4f Camera::GetGlView() {
  return glow::glRotateY(M_PI) * glow::glRotateZ(M_PI) *
         GetPose().inverse().cast<float>();
}

int Camera::GetWidth() { return width_; }

int Camera::GetHeight() { return height_; }

}  // namespace fastcd


//==================================================================================================
/*!
  @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_SIMD_TANPI_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_TANPI_INCLUDED

#include <boost/simd/function/scalar/tanpi.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/tanpi.hpp>

#endif


#include "distance_measures.h"
#include "ProteinMatrix.h"
#include <cmath>
#include <string>

// #include <xmmintrin.h>
// #include <emmintrin.h>
// #include <pmmintrin.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp> // for 'list_of()'

using namespace boost::assign; // bring 'list_of()' into scope
//using namespace boost::assign; // bring 'operator+()' into scope

using namespace std;
using namespace uqam_doc;

//static function pointer calculators
//using uqam_doc::DistSeqFuncStatic;

//abstract class - interface
//dist_2seq(string seq_a, string seq_b)
using uqam_doc::DistSeqInterface;
// virtual functions implementations
using uqam_doc::Hamming;
using uqam_doc::Hamming2;

using uqam_doc::JukesKantorNucl;
using uqam_doc::JukesKantorProt;
using uqam_doc::KimProt;
using uqam_doc::ScoreDist;
using uqam_doc::Blosum80;
using uqam_doc::Blosum62;

/*
float DistSeqFuncStatic::hamming_2seq(string seq_a, string seq_b) {

    float d = 0.0;

    for (int i = 0; i < seq_a.length(); i++) {
        d += (seq_a.at(i) == seq_b.at(i)) ? 0.0 : 1.0;

    }
    d /= seq_a.length();

    return d;

}
 */

/*************************************************************************************
/*	Nom			:	p_Kimura2Parameter
/*	Parametres	:	pmatSites       (in) : Matrice de sites
/*					pmatDistances   (in) : Matrice de distances
/*					pintNbreEspeces (in) : Nombre d'esp�ces
/*					pintNbreSites   (in) : Nombre de sites
/*	Retour		:	-
/*	Objectif	:	Comparer les s�quences deux par deux, en utilisant la m�thode
/*					Kimura 2-Parameter, et stoker les distances entre elles dans la
/*					matrice des distances.
 **************************************************************************************/

/*
float DistSeqFuncStatic::kimprot_2seq(string seq_a, string seq_b) {

    //cout << "------------------k2p_2seq" << endl;
    int nb_sites = seq_a.length();
    float dist;

    int k; // Compteur
    float dblCorrespandance; // Nombre de correspondances entre les sites
    int intPosComparable; // Nombre de positions comparables, l� o� il n'y a pas de gaps
    int intGaps; // Les lacunes contenues dans les s�quences
    float dblD; // Uncorrected distance

    dblCorrespandance = 0;
    intPosComparable = 0;
    intGaps = 0;

    for (k = 1; k <= nb_sites; k++) {
        if ((seq_a[k] == seq_b[k]) && (seq_a[k] != '-') && (seq_a[k] != '?'))
            dblCorrespandance++;
        else if ((seq_a[k] == '-') || (seq_a[k] == '?') ||
                (seq_b[k] == '-') || (seq_b[k] == '?'))
            intGaps++;
    }
    intPosComparable = nb_sites - intGaps;

    if (intPosComparable != 0) {
        dblD = 1 - dblCorrespandance / intPosComparable;
        dblD = 1 - dblD - 0.2 * dblD * dblD;
        if (dblD <= 0) {
            //non comparable
            dist = INFINITY;
        } else {
            dist = -log(dblD);
        }
    } else {
        //non comparable
        dist = INFINITY;
    }

    return dist;

}
 */

DistAtom Hamming::dist_2seq(string seq_a, string seq_b) {

    float intDist = 0.0;
    float realDist = 0.0;

    for (int i = 0; i < seq_a.length(); i++) {

        intDist += (seq_a[i] == seq_b[i] ? 0 : 1);

    }

    realDist = intDist / seq_a.length();

    return DistAtom(intDist, seq_a.length(), 0, realDist);
}

void Hamming::testIntrin() {
    /**
    int i;

    __m128d a, b, c;
    float x0[2] __attribute__((aligned(16))) = {1.2, 3.5};
    float x1[2] __attribute__((aligned(16))) = {0.7, 2.6};
    a = _mm_load_pd(x0);
    b = _mm_load_pd(x1);
    c = _mm_add_pd(a, b);
    _mm_store_pd(x0, c);
    for (i = 0; i < 2; i++) {
       cout << x0[i] << endl;
    }
     **/

}

DistAtom Hamming2::dist_2seq(string seq_a, string seq_b) {

    float pos_comparable = 0;
    float prop_gaps = 0;

    float two_gap = 0;
    float one_gap = 0;
    float ident = 0;
    float subst = 0;

    float intDist = 0.0;
    float realDist = 0.0;

    for (int i = 0; i < seq_a.length(); i++) {

        //if no gaps
        if (seq_a[i] != '-' && seq_b[i] != '-') {

            if (seq_a[i] == seq_b[i]) { // identities
                ident++;

            } else { //substitutions
                subst++;

            }

        } else if (seq_a[i] == '-' && seq_b[i] == '-') {
            two_gap++;

        } else {
            one_gap++;
        }

        if (seq_a[i] != seq_b[i]) {
            intDist++;
        }

        /*
        //identities
        if ((seq_a[i] == seq_b[i]) && (seq_a[i] != '-') && (seq_a[i] != '?')) {
            intCorrespandance++;
        //gaps
        } else if ((seq_a[i] == '-') || (seq_a[i] == '?') ||
                (seq_b[i] == '-') || (seq_b[i] == '?')) {
            intGaps++;
        //substitutions
        } else {
            intDist++;
        }
         */

    }

    intDist = subst + one_gap;
    pos_comparable = ident + subst + one_gap;

    prop_gaps = (one_gap + two_gap) / float(seq_a.length());

    if (prop_gaps <= 0.5) {
        realDist = intDist /= pos_comparable;
        //realDist = intDist /= seq_a.length();
    } else {
        intDist = numeric_limits<float>::max();
        realDist = numeric_limits<float>::max();
    }

    /*

   intPosComparable = seq_a.length() - intGaps;
   //si trop de gaps, incomparables
   if (intGaps >= seq_a.length() * 0.9) {
       intDist = numeric_limits<float>::max();
       realDist = numeric_limits<float>::max();
   } else {
       realDist = intDist /= seq_a.length();
   }
     */
    return DistAtom(intDist, seq_a.length(), 0, realDist);
    //debug
    //return DistAtom(prop_gaps, seq_a.length(), 0, prop_gaps);
}

void JukesKantorNucl::set_penality(float penality) {
    penalty_ = penality;
}

JukesKantorNucl::JukesKantorNucl(float penality) {
    cout << "================JUKES CANTOR NUCL" << endl;
    penalty_ = penality;
}

JukesKantorNucl::~JukesKantorNucl() {

}

/*************************************************************************************
/*	Nom			:	p_JukesCantorNucleo
/*	Param�tres	:	pmatSites       (in) : Matrice de sites
/*					pmatDistances   (in) : Matrice de distances
/*					pintNbreEspeces (in) : Nombre d'esp�ces
/*					pintNbreSites   (in) : Nombre de sites
/*					pdblPenalite    (in) : P�nalit� donn�e par l'utilisateur
/*	Retour		:	-
/*	Objectif	:	Comparer les s�quences deux par deux, en utilisant la m�thode
/*					Jukes-Cantor pour nucl�otides, et stoker les distances entre elles
/*					dans la matrice des distances.
 **************************************************************************************/
DistAtom JukesKantorNucl::dist_2seq(string seq_a, string seq_b) {

    int nb_sites = seq_a.length();
    float dist;
    int k; // Compteurs
    int intCorrespandance; // Nombre de correspondances entre les sites
    int intPosComparable; // Nombre de positions comparables, la o� il n'y a pas de gaps
    int intGaps; // Les lacunes contenues dans les sequences
    float dblD; // Uncorrected distance
    float dblb; // Param�tre b

    dblb = 3. / 4;

    intCorrespandance = 0;
    intGaps = 0;

    for (k = 1; k <= nb_sites; k++) {
        if ((seq_a[k] == seq_b[k]) && (seq_a[k] != '-') && (seq_a[k] != '?'))
            intCorrespandance++;
        else if ((seq_a[k] == '-') || (seq_a[k] == '?') ||
                (seq_b[k] == '-') || (seq_b[k] == '?'))
            intGaps++;
    }
    intPosComparable = nb_sites - intGaps;

    if (intPosComparable != 0) {
        dblD = 1 - intCorrespandance / (intPosComparable + penalty_ * intGaps);
        dblD = dblD / dblb;
        if (1 - dblD <= 0) dist = numeric_limits<float>::max();
        else dist = -dblb * log(1 - dblD);
    } else {
        dist = numeric_limits<float>::max();
    }
    return DistAtom(numeric_limits<float>::max(), intPosComparable, intGaps, dist);

}

void JukesKantorProt::set_penality(float penality) {
    penality_ = penality;
}

JukesKantorProt::JukesKantorProt(float penality) {
    penality_ = penality;
}

JukesKantorProt::~JukesKantorProt() {

}

/*************************************************************************************
/*	Nom			:	p_JukesCantorProt
/*	Param�tres	:	pmatSites       (in) : Matrice de sites
/*					pmatDistances   (in) : Matrice de distances
/*					pintNbreEspeces (in) : Nombre d'esp�ces
/*					pintNbreSites   (in) : Nombre de sites
/*					pdblPenalite    (in) : P�nalit� donn�e par l'utilisateur
/*	Retour		:	-
/*	Objectif	:	Comparer les s�quences deux par deux, en utilisant la m�thode
/*					Jukes-Cantor pour prot�ines, et stoker les distances entre elles
/*					dans la matrice des distances.
 **************************************************************************************/
DistAtom JukesKantorProt::dist_2seq(string seq_a, string seq_b) {

    int nb_sites = seq_a.length();
    float dist;

    int k; // Compteurs
    int intCorrespandance; // Nombre de correspondances entre les sites
    int intPosComparable; // Nombre de positions comparables, l� o� il n'y a pas de gaps
    int intGaps; // Les lacunes contenues dans les s�quences
    float dblD; // Uncorrected distance
    float dblb; // Param�tre b

    dblb = 19. / 20;

    intCorrespandance = 0;
    intGaps = 0;

    for (k = 1; k <= nb_sites; k++) {
        if ((seq_a[k] == seq_b[k]) && (seq_a[k] != '-') && (seq_a[k] != '?'))
            intCorrespandance++;
        else if ((seq_a[k] == '-') || (seq_a[k] == '?') ||
                (seq_b[k] == '-') || (seq_b[k] == '?'))
            intGaps++;
    }
    intPosComparable = nb_sites - intGaps;

    if (intPosComparable != 0) {
        dblD = 1.0 - intCorrespandance / (intPosComparable + penality_ * intGaps);
        dblD = dblD / dblb;
        if (1 - dblD <= 0)
            dist = numeric_limits<float>::max();
        else
            dist = -dblb * log(1 - dblD);
    } else
        dist = numeric_limits<float>::max();

    return DistAtom(numeric_limits<float>::max(), intPosComparable, intGaps, dist);
    //if(pmatDistances[j][i] < 0 || pmatDistances[j][i] > 10)
    //			printf("\n%d %d %lf",i,j,dist);

}

/*************************************************************************************
/*	Nom			:	p_KimuraProtein
/*	Param�tres	:	pmatSites       (in) : Matrice de sites
/*					pmatDistances   (in) : Matrice de distances
/*					pintNbreEspeces (in) : Nombre d'esp�ces
/*					pintNbreSites   (in) : Nombre de sites
/*	Retour		:	-
/*	Objectif	:	Comparer les s�quences deux par deux, en utilisant la m�thode
/*					Kimura Protein, et stoker les distances entre elles dans la matrice
/*					des distances.
 **************************************************************************************/
DistAtom KimProt::dist_2seq(string seq_a, string seq_b) {
    //cout << "------------------k2p_2seq" << endl;
    int nb_sites = seq_a.length();
    float dist;

    int k; // Compteur
    float dblCorrespandance; // Nombre de correspondances entre les sites
    int intPosComparable; // Nombre de positions comparables, l� o� il n'y a pas de gaps
    int intGaps; // Les lacunes contenues dans les s�quences
    float dblD; // Uncorrected distance

    dblCorrespandance = 0;
    intPosComparable = 0;
    intGaps = 0;

    for (k = 1; k <= nb_sites; k++) {
        if ((seq_a[k] == seq_b[k]) && (seq_a[k] != '-') && (seq_a[k] != '?'))
            dblCorrespandance++;
        else if ((seq_a[k] == '-') || (seq_a[k] == '?') ||
                (seq_b[k] == '-') || (seq_b[k] == '?'))
            intGaps++;
    }
    intPosComparable = nb_sites - intGaps;

    if (intPosComparable != 0) {
        dblD = 1 - dblCorrespandance / intPosComparable;
        dblD = 1 - dblD - 0.2 * dblD * dblD;
        if (dblD <= 0) {
            //non comparable
            dist = numeric_limits<float>::max();
        } else {
            dist = -log(dblD);
        }
    } else {
        //non comparable
        dist = numeric_limits<float>::max();
    }

    return DistAtom(numeric_limits<float>::max(), intPosComparable, intGaps, dist);

}

//load inherited fields

Blosum80::Blosum80() {
    //ftp://ftp.ncbi.nih.gov/blast/matrices/BLOSUM80
    //matrices
    //mat_prot_ident_ += "A", "R", "N", "D", "C", "Q", "E", "G", "H", "I", "L", "K", "M", "F", "P", "S", "T", "W", "Y", "V", "B", "Z", "X", "_";
    mat_prot_ident_ = list_of("A") ("R") ("N") ("D") ("C") ("Q") ("E") ("G") ("H") ("I") ("L") ("K") ("M") ("F") ("P") ("S") ("T") ("W") ("Y") ("V") ("B") ("Z") ("X") ("*");

    mat_prot_ += list_of(7)(-3)(-3)(-3)(-1)(-2)(-2)(0)(-3)(-3)(-3)(-1)(-2)(-4)(-1)(2)(0)(-5)(-4)(-1)(-3)(-2)(-1)(-8);
    mat_prot_ += list_of(-3)(9)(-1)(-3)(-6)(1)(-1)(-4)(0)(-5)(-4)(3)(-3)(-5)(-3)(-2)(-2)(-5)(-4)(-4)(-2)(0)(-2)(-8);
    mat_prot_ += list_of(-3)(-1)(9)(2)(-5)(0)(-1)(-1)(1)(-6)(-6)(0)(-4)(-6)(-4)(1)(0)(-7)(-4)(-5)(5)(-1)(-2)(-8);
    mat_prot_ += list_of(-3)(-3)(2)(10)(-7)(-1)(2)(-3)(-2)(-7)(-7)(-2)(-6)(-6)(-3)(-1)(-2)(-8)(-6)(-6)(6)(1)(-3)(-8);
    mat_prot_ += list_of(-1)(-6)(-5)(-7)(13)(-5)(-7)(-6)(-7)(-2)(-3)(-6)(-3)(-4)(-6)(-2)(-2)(-5)(-5)(-2)(-6)(-7)(-4)(-8);
    mat_prot_ += list_of(-2)(1)(0)(-1)(-5)(9)(3)(-4)(1)(-5)(-4)(2)(-1)(-5)(-3)(-1)(-1)(-4)(-3)(-4)(-1)(5)(-2)(-8);
    mat_prot_ += list_of(-2)(-1)(-1)(2)(-7)(3)(8)(-4)(0)(-6)(-6)(1)(-4)(-6)(-2)(-1)(-2)(-6)(-5)(-4)(1)(6)(-2)(-8);
    mat_prot_ += list_of(0)(-4)(-1)(-3)(-6)(-4)(-4)(9)(-4)(-7)(-7)(-3)(-5)(-6)(-5)(-1)(-3)(-6)(-6)(-6)(-2)(-4)(-3)(-8);
    mat_prot_ += list_of(-3)(0)(1)(-2)(-7)(1)(0)(-4)(12)(-6)(-5)(-1)(-4)(-2)(-4)(-2)(-3)(-4)(3)(-5)(-1)(0)(-2)(-8);
    mat_prot_ += list_of(-3)(-5)(-6)(-7)(-2)(-5)(-6)(-7)(-6)(7)(2)(-5)(2)(-1)(-5)(-4)(-2)(-5)(-3)(4)(-6)(-6)(-2)(-8);
    mat_prot_ += list_of(-3)(-4)(-6)(-7)(-3)(-4)(-6)(-7)(-5)(2)(6)(-4)(3)(0)(-5)(-4)(-3)(-4)(-2)(1)(-7)(-5)(-2)(-8);
    mat_prot_ += list_of(-1)(3)(0)(-2)(-6)(2)(1)(-3)(-1)(-5)(-4)(8)(-3)(-5)(-2)(-1)(-1)(-6)(-4)(-4)(-1)(1)(-2)(-8);
    mat_prot_ += list_of(-2)(-3)(-4)(-6)(-3)(-1)(-4)(-5)(-4)(2)(3)(-3)(9)(0)(-4)(-3)(-1)(-3)(-3)(1)(-5)(-3)(-2)(-8);
    mat_prot_ += list_of(-4)(-5)(-6)(-6)(-4)(-5)(-6)(-6)(-2)(-1)(0)(-5)(0)(10)(-6)(-4)(-4)(0)(4)(-2)(-6)(-6)(-3)(-8);
    mat_prot_ += list_of(-1)(-3)(-4)(-3)(-6)(-3)(-2)(-5)(-4)(-5)(-5)(-2)(-4)(-6)(12)(-2)(-3)(-7)(-6)(-4)(-4)(-2)(-3)(-8);
    mat_prot_ += list_of(2)(-2)(1)(-1)(-2)(-1)(-1)(-1)(-2)(-4)(-4)(-1)(-3)(-4)(-2)(7)(2)(-6)(-3)(-3)(0)(-1)(-1)(-8);
    mat_prot_ += list_of(0)(-2)(0)(-2)(-2)(-1)(-2)(-3)(-3)(-2)(-3)(-1)(-1)(-4)(-3)(2)(8)(-5)(-3)(0)(-1)(-2)(-1)(-8);
    mat_prot_ += list_of(-5)(-5)(-7)(-8)(-5)(-4)(-6)(-6)(-4)(-5)(-4)(-6)(-3)(0)(-7)(-6)(-5)(16)(3)(-5)(-8)(-5)(-5)(-8);
    mat_prot_ += list_of(-4)(-4)(-4)(-6)(-5)(-3)(-5)(-6)(3)(-3)(-2)(-4)(-3)(4)(-6)(-3)(-3)(3)(11)(-3)(-5)(-4)(-3)(-8);
    mat_prot_ += list_of(-1)(-4)(-5)(-6)(-2)(-4)(-4)(-6)(-5)(4)(1)(-4)(1)(-2)(-4)(-3)(0)(-5)(-3)(7)(-6)(-4)(-2)(-8);
    mat_prot_ += list_of(-3)(-2)(5)(6)(-6)(-1)(1)(-2)(-1)(-6)(-7)(-1)(-5)(-6)(-4)(0)(-1)(-8)(-5)(-6)(6)(0)(-3)(-8);
    mat_prot_ += list_of(-2)(0)(-1)(1)(-7)(5)(6)(-4)(0)(-6)(-5)(1)(-3)(-6)(-2)(-1)(-2)(-5)(-4)(-4)(0)(6)(-1)(-8);
    mat_prot_ += list_of(-1)(-2)(-2)(-3)(-4)(-2)(-2)(-3)(-2)(-2)(-2)(-2)(-2)(-3)(-3)(-1)(-1)(-5)(-3)(-2)(-3)(-1)(-2)(-8);
    mat_prot_ += list_of(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(-8)(1);

    //cout << mat_prot_ident_.at(22) << endl;
    //cout << mat_prot_ident_.at(17) << endl;

    //ftp://ftp.ncbi.nih.gov/blast/matrices/BLOSUM80

    //cout << mat_prot_.at(22).at(17) << endl;

    mat_prot_expect_ = -0.7442;

}

//load inherited fields

Blosum62::Blosum62() {
    //ftp://ftp.ncbi.nih.gov/blast/matrices/BLOSUM80
    //matrices
    //mat_prot_ident_ += "A", "R", "N", "D", "C", "Q", "E", "G", "H", "I", "L", "K", "M", "F", "P", "S", "T", "W", "Y", "V", "B", "Z", "X", "_";
    mat_prot_ident_ = list_of("A") ("R") ("N") ("D") ("C") ("Q") ("E") ("G") ("H") ("I") ("L") ("K") ("M") ("F") ("P") ("S") ("T") ("W") ("Y") ("V") ("B") ("Z") ("X") ("*");

    mat_prot_ += list_of (4)(-1)(-2)(-2)(0)(-1)(-1)(0)(-2)(-1)(-1)(-1)(-1)(-2)(-1)(1)(0)(-3)(-2)(0)(-2)(-1)(0)(-4);
    mat_prot_ += list_of (-1)(5)(0)(-2)(-3)(1)(0)(-2)(0)(-3)(-2)(2)(-1)(-3)(-2)(-1)(-1)(-3)(-2)(-3)(-1)(0)(-1)(-4);
    mat_prot_ += list_of (-2)(0)(6)(1)(-3)(0)(0)(0)(1)(-3)(-3)(0)(-2)(-3)(-2)(1)(0)(-4)(-2)(-3)(3)(0)(-1)(-4);
    mat_prot_ += list_of (-2)(-2)(1)(6)(-3)(0)(2)(-1)(-1)(-3)(-4)(-1)(-3)(-3)(-1)(0)(-1)(-4)(-3)(-3)(4)(1)(-1)(-4);
    mat_prot_ += list_of (0)(-3)(-3)(-3)(9)(-3)(-4)(-3)(-3)(-1)(-1)(-3)(-1)(-2)(-3)(-1)(-1)(-2)(-2)(-1)(-3)(-3)(-2)(-4);
    mat_prot_ += list_of (-1)(1)(0)(0)(-3)(5)(2)(-2)(0)(-3)(-2)(1)(0)(-3)(-1)(0)(-1)(-2)(-1)(-2)(0)(3)(-1)(-4);
    mat_prot_ += list_of (-1)(0)(0)(2)(-4)(2)(5)(-2)(0)(-3)(-3)(1)(-2)(-3)(-1)(0)(-1)(-3)(-2)(-2)(1)(4)(-1)(-4);
    mat_prot_ += list_of (0)(-2)(0)(-1)(-3)(-2)(-2)(6)(-2)(-4)(-4)(-2)(-3)(-3)(-2)(0)(-2)(-2)(-3)(-3)(-1)(-2)(-1)(-4);
    mat_prot_  += list_of (-2)(0)(1)(-1)(-3)(0)(0)(-2)(8)(-3)(-3)(-1)(-2)(-1)(-2)(-1)(-2)(-2)(2)(-3)(0)(0)(-1)(-4);
    mat_prot_ += list_of (-1)(-3)(-3)(-3)(-1)(-3)(-3)(-4)(-3)(4)(2)(-3)(1)(0)(-3)(-2)(-1)(-3)(-1)(3)(-3)(-3)(-1)(-4);
    mat_prot_ += list_of (-1)(-2)(-3)(-4)(-1)(-2)(-3)(-4)(-3)(2)(4)(-2)(2)(0)(-3)(-2)(-1)(-2)(-1)(1)(-4)(-3)(-1)(-4);
    mat_prot_ += list_of (-1)(2)(0)(-1)(-3)(1)(1)(-2)(-1)(-3)(-2)(5)(-1)(-3)(-1)(0)(-1)(-3)(-2)(-2)(0)(1)(-1)(-4);
    mat_prot_ += list_of (-1)(-1)(-2)(-3)(-1)(0)(-2)(-3)(-2)(1)(2)(-1)(5)(0)(-2)(-1)(-1)(-1)(-1)(1)(-3)(-1)(-1)(-4);
    mat_prot_ += list_of (-2)(-3)(-3)(-3)(-2)(-3)(-3)(-3)(-1)(0)(0)(-3)(0)(6)(-4)(-2)(-2)(1)(3)(-1)(-3)(-3)(-1)(-4);
    mat_prot_ += list_of (-1)(-2)(-2)(-1)(-3)(-1)(-1)(-2)(-2)(-3)(-3)(-1)(-2)(-4)(7)(-1)(-1)(-4)(-3)(-2)(-2)(-1)(-2)(-4);
    mat_prot_ += list_of (1)(-1)(1)(0)(-1)(0)(0)(0)(-1)(-2)(-2)(0)(-1)(-2)(-1)(4)(1)(-3)(-2)(-2)(0)(0)(0)(-4);
    mat_prot_ += list_of (0)(-1)(0)(-1)(-1)(-1)(-1)(-2)(-2)(-1)(-1)(-1)(-1)(-2)(-1)(1)(5)(-2)(-2)(0)(-1)(-1)(0)(-4);
    mat_prot_ += list_of (-3)(-3)(-4)(-4)(-2)(-2)(-3)(-2)(-2)(-3)(-2)(-3)(-1)(1)(-4)(-3)(-2)(11)(2)(-3)(-4)(-3)(-2)(-4);
    mat_prot_ += list_of (-2)(-2)(-2)(-3)(-2)(-1)(-2)(-3)(2)(-1)(-1)(-2)(-1)(3)(-3)(-2)(-2)(2)(7)(-1)(-3)(-2)(-1)(-4);
    mat_prot_ += list_of (0)(-3)(-3)(-3)(-1)(-2)(-2)(-3)(-3)(3)(1)(-2)(1)(-1)(-2)(-2)(0)(-3)(-1)(4)(-3)(-2)(-1)(-4);
    mat_prot_ += list_of (-2)(-1)(3)(4)(-3)(0)(1)(-1)(0)(-3)(-4)(0)(-3)(-3)(-2)(0)(-1)(-4)(-3)(-3)(4)(1)(-1)(-4);
    mat_prot_ += list_of (-1)(0)(0)(1)(-3)(3)(4)(-2)(0)(-3)(-3)(1)(-1)(-3)(-1)(0)(-1)(-3)(-2)(-2)(1)(4)(-1)(-4);
    mat_prot_ += list_of (0)(-1)(-1)(-1)(-2)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-1)(-2)(0)(0)(-2)(-1)(-1)(-1)(-1)(-1)(-4);
    mat_prot_ += list_of (-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(-4)(1);

    //cout << mat_prot_ident_.at(22) << endl;
    //cout << mat_prot_ident_.at(17) << endl;

    //ftp://ftp.ncbi.nih.gov/blast/matrices/BLOSUM62

    //cout << mat_prot_.at(22).at(17) << endl;

    mat_prot_expect_ = -0.5209;

}

//constructor

ScoreDist::ScoreDist(ApplicationData* appl_data) {

    // mat_prot_ = BLOSUM80_E;

    if (appl_data->protmatrix_ == eBLOSUM80) {
        matrix_ = new Blosum80();
    } else if (appl_data->protmatrix_ == eBLOSUM62) {
        matrix_ = new Blosum62();

    //cout << " mat_prot_ident_[1]: " << matrix_->getMatProtIdent().at(1) << endl;
    //cout << " mat_prot_[1][5]: " << matrix_->getMatProt().at(22).at(17) << endl;
    }

}

DistAtom ScoreDist::dist_2seq(string seq_a, string seq_b) {
    //cout << "------------------k2p_2seq" << endl;
    //cout << "seq_a: " << seq_a << endl;
    //cout << "seq_b: " << seq_b << endl;

    int len = seq_a.length();

    float sigma_s1_s2 = simple_dist_2seq(seq_a, seq_b);
    float sigma_s1_s1 = simple_dist_2seq(seq_a, seq_a);
    float sigma_s2_s2 = simple_dist_2seq(seq_b, seq_b);
    float sigma_r_l = matrix_->getMatProtExpect() * len;

    //correct lower_bound
    if (sigma_s1_s2 < sigma_r_l) {
        sigma_s1_s2 = sigma_r_l;
    }

    float sigma_n = sigma_s1_s2 - sigma_r_l;
    float sigma_u_s1_s2 = (sigma_s1_s1 + sigma_s2_s2) / 2;
    float sigma_un = sigma_u_s1_s2 - sigma_r_l;
    float dist_01 = sigma_n /sigma_un;

    //poisson process correction
    float d_r = - log (dist_01) * 100 ;
    //calibration constant
    float c = 1.3370;

    float d_s = c * d_r;

    /*Evolutionary distances of 250–300 PAM units are commonly considered as the maximum
     * for reasonable distance estimation and, therefore,
     * the Scoredist estimate ds is restricted to the interval [0, 300] PAM.
     */
    float dist = 0;

    float MAX_PAM = 1000;

    //dist = c * dist_01;

    if (d_s > MAX_PAM) {
       dist  = MAX_PAM;
    }

    // cout << "dist: " << dist << endl;
    //wrap d_s
    return DistAtom(floor(dist*seq_a.length()), seq_a.length(), 0, dist);

}

float ScoreDist::simple_dist_2seq(string seq_a, string seq_b) {
    //cout << "------------------k2p_2seq" << endl;
    int nb_sites = seq_a.length();
    float s = 0.0;

    //sum
    for (int k = 0; k < nb_sites; k++) {
        s += dist_2car(seq_a.substr(k, 1), seq_b.substr(k, 1));

    }

    return s;

}

float ScoreDist::dist_2car(string car_a, string car_b) {

    float one_dist = 0.0;

    //cout << "car_a: " << car_a << endl;
    //cout << "car_b: " << car_b << endl;

    int idx_a = std::find(matrix_->getMatProtIdent().begin(), matrix_->getMatProtIdent().end(), car_a) - matrix_->getMatProtIdent().begin();
    int idx_b = std::find(matrix_->getMatProtIdent().begin(), matrix_->getMatProtIdent().end(), car_b) - matrix_->getMatProtIdent().begin();

    //underscore or other defaults to *
    if (idx_a == matrix_->getMatProtIdent().size()) {
        car_a = "*";
        //and refind
        idx_a = std::find(matrix_->getMatProtIdent().begin(), matrix_->getMatProtIdent().end(), car_a) - matrix_->getMatProtIdent().begin();
    }

    if (idx_b == matrix_->getMatProtIdent().size()) {
        car_b = "*";
        //and refind
        idx_b = std::find(matrix_->getMatProtIdent().begin(), matrix_->getMatProtIdent().end(), car_b) - matrix_->getMatProtIdent().begin();
    }

    one_dist = matrix_->getMatProt().at(idx_a).at(idx_b);

    //cout << "one_dist: " << one_dist << endl;

    return one_dist;

}


#pragma once

#include <Eigen/Geometry>

struct BoxQ
{
	Eigen::Vector3f bboxTransform;
	Eigen::Quaternionf bboxQuaternion;
	float cube_length;
    float cube_width;
    float cube_height;
};

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
    int x_mid;
    int y_mid;
};


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/detail/as_range.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/io/wkt/read.hpp>

template <int D, typename Range>
double sum(Range const& range)
{
    double s = 0.0;
    for (typename boost::range_const_iterator<Range>::type it = boost::begin(range);
        it != boost::end(range); ++it)
    {
        s += bg::get<D>(*it);
    }
    return s;
}

template <typename G>
void test_geometry(std::string const& wkt, double expected_x, double expected_y)
{
    G geometry;
    bg::read_wkt(wkt, geometry);

    double s = sum<0>(bg::detail::as_range(geometry));
    BOOST_CHECK_CLOSE(s, expected_x, 0.001);

    s = sum<1>(bg::detail::as_range(geometry));
    BOOST_CHECK_CLOSE(s, expected_y, 0.001);
}

template <typename P>
void test_all()
{
    // As-range utility should consider a geometry as a range, so
    // linestring stays linestring
    test_geometry<bg::model::linestring<P> >("LINESTRING(1 2,3 4)", 4, 6);

    // polygon will only be outer-ring
    test_geometry<bg::model::polygon<P> >("POLYGON((1 2,3 4))", 4, 6);
    test_geometry<bg::model::polygon<P> >("POLYGON((1 2,3 4),(5 6,7 8,9 10))", 4, 6);

    // the utility is useful for:
    // - convex hull (holes do not count)
    // - envelope (idem)
}

int test_main(int, char* [])
{
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}


#include <opw_kinematics/opw_macros.h>
OPW_IGNORE_WARNINGS_PUSH
#include <gtest/gtest-message.h>    // for Message
#include <gtest/gtest-test-part.h>  // for TestPartResult
#include <gtest/gtest.h>            // IWYU pragma: keep
#include <Eigen/Core>               // IWYU pragma: keep
#include <vector>
#include <array>
OPW_IGNORE_WARNINGS_POP

#include "opw_kinematics/opw_kinematics.h"  // IWYU pragma: keep
#include "opw_kinematics/opw_parameters_examples.h"
#include "opw_kinematics/opw_utilities.h"

const double TOLERANCE = 1e-5;  // absolute tolerance for EXPECT_NEAR checks

using opw_kinematics::Solutions;
using opw_kinematics::Transform;

/** @brief Compare every element of two eigen Isometry3 poses.
 */
template <typename T>
void comparePoses(const Transform<T>& Ta, const Transform<T>& Tb)
{
  using Matrix = Eigen::Matrix<T, 3, 3>;
  using Vector = Eigen::Matrix<T, 3, 1>;

  Matrix Ra = Ta.rotation(), Rb = Tb.rotation();
  for (int i = 0; i < Ra.rows(); ++i)
  {
    for (int j = 0; j < Ra.cols(); ++j)
    {
      EXPECT_NEAR(Ra(i, j), Rb(i, j), TOLERANCE);
    }
  }

  Vector pa = Ta.translation(), pb = Tb.translation();
  EXPECT_NEAR(pa[0], pb[0], TOLERANCE);
  EXPECT_NEAR(pa[1], pb[1], TOLERANCE);
  EXPECT_NEAR(pa[2], pb[2], TOLERANCE);
}

TEST(kuka_kr6, forward_kinematics)  // NOLINT
{
  const auto kuka = opw_kinematics::makeKukaKR6_R700_sixx<float>();

  std::array<float, 6> joint_values = { 0.2f, 0.2f, 0.2f, 0.2f, 0.2f, 0.2f };
  Transform<float> forward_pose = opw_kinematics::forward(kuka, joint_values);

  // Compare with copied results from forward kinematics using MoveIt!
  Transform<float> actual_pose;
  actual_pose.matrix() << -0.5965795f, 0.000371195f, 0.8025539f, 0.0f, -0.2724458f, 0.9405218f, -0.202958f, 0.0f,
      -0.7548948f, -0.3397331f, -0.5609949f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f;
  actual_pose.translation() << 0.7341169f, -0.1520347f, 0.182639f;

  comparePoses(forward_pose, actual_pose);
}

TEST(kuka_kr6, inverse_kinematics)  // NOLINT
{
  // this test assumes that the forward kinematics for joint values
  // {0.2, 0.2, 0.2, 0.2, 0.2, 0.2} are correct

  const auto kuka = opw_kinematics::makeKukaKR6_R700_sixx<float>();

  std::array<float, 6> joint_values = { 0.2f, 0.2f, 0.2f, 0.2f, 0.2f, 0.2f };
  Transform<float> forward_pose = opw_kinematics::forward(kuka, joint_values);

  Solutions<float> sols = opw_kinematics::inverse(kuka, forward_pose);

  Transform<float> pose;
  for (const auto& s : sols)
  {
    if (opw_kinematics::isValid(s))
    {
      // Forward kinematics of a solution should result in the same pose
      pose = opw_kinematics::forward(kuka, s);
      comparePoses(forward_pose, pose);
    }
  }
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/*
   Libchecktestdata -- check testdata according to specification.

   It's licensed under the 2-clause BSD license, see the file COPYING.
 */

#define _XOPEN_SOURCE 700

#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <map>
#include <set>
#include <regex>
#include <type_traits>
#include <cctype>
#include <cstdlib>
#include <cstdarg>
#include <climits>
#include <getopt.h>
#include <time.h>
#include <cstdlib>
#include <boost/variant.hpp>
#include <boost/exception_ptr.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <gmpxx.h>

#include "parser.h"
#include "libchecktestdata.hpp"
#include "databuffer.hpp"

using namespace std;

namespace checktestdata {

class doesnt_match_exception {};
class eof_found_exception {};
class generate_exception {};

const int display_before_error = 65;
const int display_after_error  = 50;

size_t prognr;
command currcmd;

gmp_randclass gmp_rnd(gmp_randinit_default);

// Simple function to get a random integer in the range [0,n-1].
// Assumes that the gmp_rnd global variable has been initialized.
unsigned long get_random(unsigned long n)
{
	mpz_class x = gmp_rnd.get_z_range(n);
	return x.get_ui();
}

databuffer data;
vector<command> program;

// This stores array-type variables like x[i,j] as string "x" and
// vector of the indices. Plain variables are stored using an index
// vector of zero length.
typedef map<vector<mpz_class>,value_t> indexmap;
typedef map<value_t,set<vector<mpz_class>>> valuemap;
map<string,indexmap> variable, preset;
map<string,valuemap> rev_variable, rev_preset;

// List of loop starting commands like REP, initialized in checksyntax.
set<string> loop_cmds;

int whitespace_ok;
int debugging;
int quiet;
int gendata;

void debug(const char *, ...) __attribute__((format (printf, 1, 2)));

void debug(const char *format, ...)
{
	va_list ap;
	va_start(ap,format);

	if ( debugging ) {
		fprintf(stderr,"debug: ");

        if ( format!=NULL ) {
			vfprintf(stderr,format,ap);
        } else {
			fprintf(stderr,"<no debug data?>");
        }

		fprintf(stderr,"\n");
	}

	va_end(ap);
}

void readprogram(istream &in)
{
	debug("parsing script...");

	Parser parseprog(in);
	try {
		if ( parseprog.parse()!=0 ) {
			cerr << "parse error reading program" << endl;
			exit(exit_failure);
		}
	} catch ( exception& e ) {
		cerr << "parse error: " << e.what() << endl;
		exit(exit_failure);
	}

	program = parseprog.parseResult.args;

	// Add (implicit) EOF command at end of input
	program.push_back(command("EOF"));

	// Check for correct REP ... END nesting
	vector<string> levels;
	for (size_t i=0; i<program.size(); i++) {
		if ( loop_cmds.count(program[i].name()) ||
		     program[i].name() == "IF" )
			levels.push_back(program[i].name());

		if ( program[i].name()=="END" ) {
			if ( levels.size()==0 ) {
				cerr << "incorrect END statement" << endl;
				exit(exit_failure);
			}
			levels.pop_back();
		}

		if ( program[i].name()=="ELSE" ) {
			if ( levels.size()==0 || levels.back()!="IF") {
				cerr << "incorrect ELSE statement" << endl;
				exit(exit_failure);
			}
			levels.pop_back();
			levels.push_back(program[i].name());
		}
	}
	if ( levels.size()>0 ) {
		cerr << "unclosed block statement(s)" << endl;
		exit(exit_failure);
	}
}

void readtestdata(istream &in)
{
	debug("reading testdata...");

	stringstream ss;
	ss << in.rdbuf();
	if ( in.fail() ) {
		cerr << "error reading testdata" << endl;
		exit(exit_failure);
	}

	data = databuffer(ss.str());
}

void error(string msg = string())
{
	if ( gendata ) {
		cerr << "ERROR: in command " << currcmd << ": " << msg << endl << endl;
		throw generate_exception();
	}

	debug("error at datanr = %d\n",(int)data.pos());

	if ( !quiet ) {
		cerr << data.prev(display_before_error) << endl;
		cerr << string(min(data.lpos(),(size_t)display_before_error),' ') << '^';
		cerr << data.next(display_after_error) << endl << endl;

		cerr << "ERROR: line " << data.line()+1 << " character " << data.lpos()+1;
		cerr << " of testdata doesn't match " << currcmd;
		if ( msg.length()>0 ) cerr << ": " << msg;
		cerr << endl << endl;
	}

	throw doesnt_match_exception();
}

long string2int(string s)
{
	long res;
	char *ptr;

	res = strtol(s.c_str(),&ptr,10);
	if ( *ptr!='\0' || res==LONG_MIN || res==LONG_MAX ) {
		error("cannot parse integer: `" + s + "'");
	}

	return res;
}

value_t eval(const expr&); // forward declaration

value_t getvar(const expr& var, int use_preset = 0)
{
	// Construct index array. The cast to mpz_class automatically
	// verifies that the index value is of type mpz_class.
	vector<mpz_class> ind;
	for(size_t i=0; i<var.nargs(); i++) {
		ind.push_back(mpz_class(eval(var.args[i])));
	}
	if ( use_preset ) {
		if ( preset.count(var.val) && preset[var.val].count(ind) ) {
			return preset[var.val][ind];
		}
		return value_t();
	} else {
		if ( gendata && preset.count(var.val) && preset[var.val].count(ind) ) {
			return preset[var.val][ind];
		}
		if ( variable.count(var.val) && variable[var.val].count(ind) ) {
			return variable[var.val][ind];
		}
	}
	cerr << "variable " << var << " undefined in " << program[prognr] << endl;
	exit(exit_failure);
}

void setvar(const expr& var, value_t val, int use_preset = 0)
{
	// Construct index array. The cast to mpz_class automatically
	// verifies that the index value is of type mpz_class.
	vector<mpz_class> ind;
	for(size_t i=0; i<var.nargs(); i++) {
		ind.push_back(mpz_class(eval(var.args[i])));
	}

	map<string,indexmap> *varlist = &variable;
	map<string,valuemap> *revlist = &rev_variable;
	if ( use_preset ) {
		varlist = &preset;
		revlist = &rev_preset;
	}

	// Remove previously existing value -> index:
	if ( (*varlist)[var].count(ind) ) {
		(*revlist)[var][val].erase(ind);
		if ( (*revlist)[var][val].size()==0 ) (*revlist)[var].erase(val);
	}
	(*varlist)[var][ind] = val;
	(*revlist)[var][val].insert(ind);
}

void setvars(const vector<parse_t>& varlist, int use_preset = 0)
{
	for(size_t i=0; i<varlist.size(); i++) {
		setvar(varlist[i].args[0],eval(varlist[i].args[1]),use_preset);
	}
}

void unsetvars(const args_t& varlist)
{
	for(size_t i=0; i<varlist.size(); i++) {
		variable.erase(varlist[i].val);
		rev_variable.erase(varlist[i].val);
	}
}

value_t value(const expr& x)
{
	debug("value '%s'",x.val.c_str());
	if ( x.cache.val.which()!=value_none ) {
		debug("eval cached");
		return x.cache;
	}

	if ( x.op=='S' ) return x.cache = value_t(x.val);
	if ( isalpha(x.val[0]) ) return getvar(x);

	mpz_class intval;
	mpf_class fltval;
	if ( intval.set_str(x.val,0)==0 ) return x.cache = value_t(intval);
	else if ( fltval.set_str(x.val,0)==0 ) {
		// Set sufficient precision:
		if ( fltval.get_prec()<4*x.val.length() ) {
			fltval.set_prec(4*x.val.length());
			fltval.set_str(x.val,0);
		}
		return x.cache = value_t(fltval);
	}
	return value_t();
}

template<class A, class B>
struct arith_result {
	typedef typename conditional<
		is_same<A,mpz_class>::value && is_same<B,mpz_class>::value,
			mpz_class,
			mpf_class
			>::type type;
};

template<class A, class B> struct arith_compatible {
	constexpr static bool value = (is_same<mpz_class,A>::value || is_same<mpf_class,A>::value) &&
		(is_same<mpz_class,B>::value || is_same<mpf_class,B>::value);
};

template<class A, class B> struct is_comparable {
	constexpr static bool value = arith_compatible<A,B>::value ||
		(is_same<A,string>::value && is_same<B,string>::value);
};

/* We define overloaded arithmetic and comparison operators.
 * As they are all identical, the code is captured in two macro's
 * below, except for the modulus and unary minus.
 */
#define DECL_VALUE_BINOP(op,name) \
struct arithmetic_##name : public boost::static_visitor<value_t> {\
	template<class A, class B, typename enable_if<!arith_compatible<A,B>::value,int>::type = 0>\
	value_t operator()(const A& a, const B& b) const {\
		cerr << "cannot compute " << a << " " #op " " << b << endl; \
		exit(exit_failure);\
	}\
	template<class A, class B, typename enable_if<arith_compatible<A,B>::value,int>::type = 0,\
		class C = typename arith_result<A,B>::type>\
	value_t operator()(const A& a, const B& b)const  {\
		return value_t(C(a op b));\
	}\
};\
value_t operator op(const value_t &x, const value_t &y) \
{ \
	return boost::apply_visitor(arithmetic_##name(), x.val, y.val);\
}

#define DECL_VALUE_CMPOP(op,name) \
struct arithmetic_##name : public boost::static_visitor<bool> {\
	template<class A, class B, typename enable_if<!is_comparable<A,B>::value,int>::type = 0>\
	bool operator()(const A& a, const B& b)const  {\
		cerr << "cannot compute " << a << " " #op " " << b << endl; \
		exit(exit_failure);\
	}\
	template<class A, class B, typename enable_if<is_comparable<A,B>::value,int>::type = 0>\
	bool operator()(const A& a, const B& b)const  {\
		return a op b;\
	}\
};\
bool operator op(const value_t &x, const value_t &y) \
{ \
	return boost::apply_visitor(arithmetic_##name(), x.val, y.val);\
}

DECL_VALUE_BINOP(+,plus)
DECL_VALUE_BINOP(-,minus)
DECL_VALUE_BINOP(*,times)
DECL_VALUE_BINOP(/,div)

DECL_VALUE_CMPOP(>,g)
DECL_VALUE_CMPOP(<,l)
DECL_VALUE_CMPOP(>=,ge)
DECL_VALUE_CMPOP(<=,le)
DECL_VALUE_CMPOP(==,e)
DECL_VALUE_CMPOP(!=,ne)

value_t operator -(const value_t &x)
{
	return value_t(mpz_class(0)) - x;
}

value_t operator %(const value_t &x, const value_t &y)
{
	const mpz_class *xp, *yp;
	if ( (xp = boost::get<const mpz_class>(&x.val)) && (yp = boost::get<const mpz_class>(&y.val))) {
		auto res = *xp;
		res %= *yp;
		return value_t(res);
	}
	cerr << "can only use modulo on integers in " << program[prognr] << endl;
	exit(exit_failure);
}

struct pow_visitor : public boost::static_visitor<value_t> {
	template<class B, class E>
	value_t operator()(const B& b, const E& e) const {
		cerr << "only integer exponents allowed in " << program[prognr] << endl;
		exit(exit_failure);
	}
	template<class B>
	value_t operator()(const B& b, const mpz_class& e) const {
		if(!e.fits_ulong_p()) {
			cerr << "integer exponent " << e
				<< " does not fit in unsigned long in " << program[prognr] << endl;
			exit(exit_failure);
		}
		return pow(b, e);
	}
	value_t pow(const mpz_class& b, const mpz_class& e) const {
		mpz_class res;
		mpz_pow_ui(res.get_mpz_t(), b.get_mpz_t(), e.get_ui());
		return value_t(res);
	}
	value_t pow(const mpf_class& b, const mpz_class& e) const {
		mpf_class res;
		mpf_pow_ui(res.get_mpf_t(), b.get_mpf_t(), e.get_ui());
		return value_t(res);
	}
	template<class B>
	value_t pow(const B&, const mpz_class&) const {
		cerr << "exponentiation base must be of arithmetic type in "
			 << program[prognr] << endl;
		exit(exit_failure);
	}
};

value_t pow(const value_t &x, const value_t &y)
{
	return boost::apply_visitor(pow_visitor(), x.val, y.val);
}

value_t evalfun(args_t funargs)
{
	string fun = funargs[0].val;
	if ( fun=="STRLEN" ) {
		string str = eval(funargs[1]).getstr();
		return value_t(mpz_class(str.length()));
	}

	cerr << "unknown function '" << fun << "' in "
		 << program[prognr] << endl;
	exit(exit_failure);
}

bool cachable(const expr& e)
{
	switch ( e.op ) {
	case 'I':
	case 'F':
	case 'S':
		return true;

	case '+':
	case '-':
	case '*':
	case '%':
	case '/':
	case '^':
	case 'n':
	case '?':
	case '|':
	case '&':
	case '!':
	case 'f':
		for(size_t i=0; i<e.nargs(); i++) if ( !cachable(e.args[i]) ) return false;
		return true;
	}
	return false;
}

value_t eval(const expr& e)
{
	debug("eval op='%c', val='%s', #args=%d",e.op,e.val.c_str(),(int)e.args.size());
	if ( e.cache.val.which()!=value_none ) {
		debug("eval cached");
		return e.cache;
	}
	value_t res;
	switch ( e.op ) {
	case 'I':
	case 'F':
	case 'S':
	case 'v': res = value(e); break;
	case 'n': res = -eval(e.args[0]); break;
	case '+': res = eval(e.args[0]) + eval(e.args[1]); break;
	case '-': res = eval(e.args[0]) - eval(e.args[1]); break;
	case '*': res = eval(e.args[0]) * eval(e.args[1]); break;
	case '/': res = eval(e.args[0]) / eval(e.args[1]); break;
	case '%': res = eval(e.args[0]) % eval(e.args[1]); break;
	case '^': res = pow(eval(e.args[0]),eval(e.args[1])); break;
	case 'f': res = evalfun(e.args); break;
	default:
		cerr << "unknown arithmetic operator '" << e.op << "' in "
		     << program[prognr] << endl;
		exit(exit_failure);
	}
	if ( cachable(e) ) e.cache = res;
	return res;
}

bool compare(const expr& cmp)
{
	string op = cmp.val;
	value_t l = eval(cmp.args[0]);
	value_t r = eval(cmp.args[1]);

	if ( op=="<"  ) return l<r;
	if ( op==">"  ) return l>r;
	if ( op=="<=" ) return l<=r;
	if ( op==">=" ) return l>=r;
	if ( op=="==" ) return l==r;
	if ( op=="!=" ) return l!=r;

	cerr << "unknown compare operator " << op << " in "
	     << program[prognr] << endl;
	exit(exit_failure);
}

bool unique(const args_t& varlist)
{
	debug("unique, #args=%d",(int)varlist.size());

	vector<decltype(&variable[""])> vars;

	// First check if all variables exist.
	for(size_t i=0; i<varlist.size(); i++) {
		if ( !variable.count(varlist[i].val) ) {
			cerr << "variable " << varlist[i].val << " undefined in "
				 << program[prognr] << endl;
			exit(exit_failure);
		}
		vars.push_back(&variable[varlist[i].val]);
	}

	// Check if all variables have equal numbers of indices. Then we
	// can later check if they have the same indices by comparing the
	// indices to that of the first variable.
	for(size_t i=1; i<vars.size(); i++) {
		if ( vars[0]->size()!=vars[i]->size() ) {
			error("variables " + varlist[0].val + " and " + varlist[i].val +
			      " have different indices");
		}
	}
	// Now check if all tuples are unique.
	vector<pair<vector<value_t>,const indexmap::key_type*>> tuples;
	for(indexmap::iterator it=vars[0]->begin();
		it!=vars[0]->end(); ++it) {
		const vector<mpz_class> &index = it->first;
		vector<value_t> tuple;
		for(size_t i=0; i<vars.size(); i++) {
			auto it = vars[i]->find(index);
			if ( it == vars[i]->end() ) {
				string s;
				s = "index [";
				for(size_t j=0; j<index.size(); j++) {
					if ( j>0 ) s += ",";
					s += index[j].get_str();
				}
				s += "] not defined for variable " + varlist[i].val;
				error(s);
			}
			tuple.push_back(it->second);
		}
		tuples.emplace_back(tuple,&index);
	}
	sort(begin(tuples), end(tuples));
	for(size_t i = 0; i + 1 < tuples.size(); ++i) {
		if(tuples[i].first == tuples[i+1].first) {
			auto tuple = tuples[i].first;
			auto index = *tuples[i].second;
			string s;
			s = "non-unique tuple (" + tuple[0].tostr();
			for(size_t j=1; j<tuple.size(); j++) s += "," + tuple[j].tostr();
			s += ") found at index [";
			for(size_t j=0; j<index.size(); j++) {
				if ( j>0 ) s += ",";
				s += index[j].get_str();
			}
			s += "]";
			error(s);
		}
	}
	return true;
}

bool inarray(const expr& e, const expr& array)
{
	string var = array.val;
	value_t val = eval(e);

	debug("inarray, value = %s, array = %s",val.tostr().c_str(),var.c_str());

	if ( !variable.count(var) ) {
		cerr << "variable " << var << " undefined in "
			 << program[prognr] << endl;
		exit(exit_failure);
	}

	return rev_variable[var].count(val) && rev_variable[var][val].size()>0;
}

bool dotest(const test& t)
{
	debug("test op='%c', #args=%d",t.op,(int)t.args.size());
	switch ( t.op ) {
	case '!': return !dotest(t.args[0]);
	case '&': return dotest(t.args[0]) && dotest(t.args[1]);
	case '|': return dotest(t.args[0]) || dotest(t.args[1]);
	case 'E':
		if ( gendata ) {
			return (get_random(10) < 3);
		} else {
			return data.eof();
		}
	case 'M':
		if ( gendata ) {
			return (get_random(2) == 0);
		} else {
			return !data.eof() && t.args[0].val.find(data.next())!=string::npos;
		}
	case 'U': return unique(t.args);
	case 'A': return inarray(t.args[0],t.args[1]);
	case '?': return compare(t);
	default:
		cerr << "unknown test " << t.op << " in " << program[prognr] << endl;
		exit(exit_failure);
	}
}

void checkspace()
{
	if ( data.eof() ) error("end of file");

	if ( whitespace_ok ) {
		// First check at least one space-like character
		if ( !isspace_notnewline(data.readchar()) ) error();
		// Then greedily read non-newline whitespace
		data.readwhitespace();
	} else {
		if ( data.readchar()!=' ' ) error();
	}
}

void checknewline()
{
	// Trailing whitespace before newline
	if ( whitespace_ok ) data.readwhitespace();

	if ( data.eof() ) error("end of file");
	if ( data.readchar()!='\n' ) error();

	// Leading whitespace after newline
	if ( whitespace_ok ) data.readwhitespace();
}

#define MAX_MULT 10
int getmult(string &exp, unsigned int &index)
{
	index++;

	if (index >= exp.length()) {
		return 1;
	}

	int min = 0;
	int max = MAX_MULT;
	switch (exp[index]) {
	case '?':
		index++;
		max = 1;
		break;
	case '+':
		index++;
		min = 1;
		break;
	case '*':
		index++;
		break;
	case '{':
		index++;
		{
			int end = exp.find_first_of('}', index);
			string minmaxs = exp.substr(index, end - index);
			int pos = minmaxs.find_first_of(',');
			if (pos == -1) {
				min = max = string2int(minmaxs);
			} else {
				string mins = minmaxs.substr(0, pos);
				string maxs = minmaxs.substr(pos + 1);
				min = string2int(mins);
				if (maxs.length() > 0) {
					max = string2int(maxs);
				}
			}
			index = end + 1;
		}
		break;
	default:
		min = 1;
		max = 1;
	}

	return (min + get_random(1 + max - min));
}

string genregex(string exp)
{
	unsigned int i = 0;
	string res;
	debug("genregex '%s'",exp.c_str());
	while (i < exp.length()) {
		switch (exp[i]) {
		case '\\':
			{
				i++;
				char c = exp[i];
				int mult = getmult(exp, i);
				for (int cnt = 0; cnt < mult; cnt++) {
					res += c;
				}
			}
			break;
		case '.':
			{
				int mult = getmult(exp, i);
				for (int cnt = 0; cnt < mult; cnt++) {
					res += (char) (' ' + get_random((int) ('~' - ' ')));
				}
			}
			break;
		case '[':
			{
				set<char> possible;
				bool escaped = false, inverted = false;
				if ( i + 1 < exp.length() && exp[i+1] == '^' ) {
					inverted = true;
					i++;
				}
				while (i + 1 < exp.length()) {
					i++;
					if (escaped) {
						escaped = false;
					} else if (exp[i] == ']') {
						break;
					} else if (exp[i] == '\\') {
						escaped = true;
						continue;
					}
					if (i + 2 < exp.length() && exp[i + 1] == '-' && exp[i] != '[' && exp[i+2] != ']') {
						char from = exp[i];
						i += 2;
						char to = exp[i];
						if (to == '\\') {
							i++;
							to = exp[i];
						}
						while (from <= to) {
							possible.insert(from);
							from++;
						}
					} else {
						possible.insert(exp[i]);
					}
				}
				vector<char> possibleVec;
				if ( inverted ) {
					for (char c = ' '; c <= '~'; c++) {
						if ( !possible.count(c) ) possibleVec.push_back(c);
					}
				} else {
					copy(possible.begin(), possible.end(), std::back_inserter(possibleVec));
				}
				int mult = getmult(exp, i);
				for (int cnt = 0; cnt < mult; cnt++) {
					res += possibleVec[get_random(possibleVec.size())];
				}
			}
			break;
		case '(':
			{
				i++;
				vector<string> alternatives;
				int depth = 0;
				int begin = i;
				bool escaped = false;
				while (depth > 0 || escaped || exp[i] != ')') {
					if (escaped) {
						escaped = false;
					} else if (exp[i] == '\\') {
						escaped = true;
					} else if (depth == 0 && exp[i] == '|') {
						alternatives.push_back(exp.substr(begin, i - begin));
						begin = i + 1;
					} else if (exp[i] == '(') {
						depth++;
					} else if (exp[i] == ')') {
						depth--;
					}
					i++;
				}
				alternatives.push_back(exp.substr(begin, i - begin));
				int mult = getmult(exp, i);
				for (int cnt = 0; cnt < mult; cnt++) {
					res += genregex(alternatives[get_random(alternatives.size())]);
				}
			}
			break;
		default:
			{
				char c = exp[i];
				int mult = getmult(exp, i);
				for (int cnt = 0; cnt < mult; cnt++) {
					res += c;
				}
			}
			break;
		}
	}
	return res;
}

// Parse {min,max}decimals in FLOATP command.
void getdecrange(const command& cmd, int *decrange)
{
	// Read {min,max}decimals range for float.
	for(int i=0; i<2; i++) {
		value_t arg = eval(cmd.args[2+i]);
		if ( arg.val.which()!=value_int ) {
			error((i==0 ? "min":"max")+string("decimal is not an integer"));
		}
		mpz_class val = arg;
		if ( val<0 || val>=INT_MAX ) {
			error(string("the value of ")+(i==0 ? "min":"max")+"decimal is out of range");
		}
		decrange[i] = val.get_si();
	}
	if ( decrange[0]>decrange[1] ) error("invalid decimals range specified");
}

void gentoken(command cmd, ostream &datastream)
{
	currcmd = cmd;
	debug("generating token %s", cmd.name().c_str());

	if ( cmd.name()=="SPACE" ) datastream << ' ';

	else if ( cmd.name()=="NEWLINE" ) datastream << '\n';

	else if ( cmd.name()=="INT" ) {
		mpz_class lo = eval(cmd.args[0]);
		mpz_class hi = eval(cmd.args[1]);
		mpz_class x(lo + gmp_rnd.get_z_range(hi - lo + 1));

		if ( cmd.nargs()>=3 ) {
			// Check if we have a preset value, then override the
			// random generated value
			value_t y = getvar(cmd.args[2],1);
			if ( !boost::get<none_t>(&y.val) ) {
				x = y;
				if ( x<lo || x>hi ) {
					error("preset value for '" + string(cmd.args[2]) + "' out of range");
				}
			}
			setvar(cmd.args[2],value_t(x));
		}

		datastream << x.get_str();
	}

	else if ( cmd.name()=="FLOAT" || cmd.name()=="FLOATP" ) {
		mpf_class lo = eval(cmd.args[0]);
		mpf_class hi = eval(cmd.args[1]);

		char floatspec = 'f'; // Default to fixed point notation
		int ndecimals = 15; // Default number of decimals shown

		size_t nbaseargs = 2;
		int decrange[2];
		if ( cmd.name()=="FLOATP" ) {
			nbaseargs = 4;
			getdecrange(cmd,decrange);
			ndecimals = decrange[1];
		}

		if ( cmd.nargs()>=nbaseargs+2 ) {
			if ( cmd.args[nbaseargs+1].name()=="SCIENTIFIC" ) floatspec = 'E';
			else if ( cmd.args[nbaseargs+1].name()=="FIXED" ) floatspec = 'f';
			else {
				cerr << "invalid option in " << program[prognr] << endl;
				exit(exit_failure);
			}
		}

		mpf_class x(lo + gmp_rnd.get_f()*(hi-lo));

		if ( cmd.nargs()>=nbaseargs+1 ) {
			// Check if we have a preset value, then override the
			// random generated value
			value_t y = getvar(cmd.args[nbaseargs],1);
			if ( !boost::get<none_t>(&y.val) ) {
				x = y;
				if ( x<lo || x>hi ) {
					error("preset value for '" + string(cmd.args[nbaseargs]) +
					      "' out of range");
				}
			}
			setvar(cmd.args[2],value_t(x));
		}

		char tmp[256];
		gmp_snprintf(tmp,255,(string("%.*F")+floatspec).c_str(),ndecimals,x.get_mpf_t());
		datastream << tmp;
	}

	else if ( cmd.name()=="STRING" ) {
		string str = eval(cmd.args[0]).getstr();
		datastream << str;
	}

	else if ( cmd.name()=="REGEX" ) {
		string regexstr = eval(cmd.args[0]).getstr();
//		regex e1(regex, regex::extended); // this is only to check the expression
		string str = genregex(regexstr);
		datastream << str;
		if ( cmd.nargs()>=2 ) setvar(cmd.args[1],value_t(str));
	}

	else if ( cmd.name()=="ASSERT" ) {
		if ( !dotest(cmd.args[0]) ) error("assertion failed");
	}

	else if ( cmd.name()=="SET" ) {
		setvars(cmd.args);
	}

	else if ( cmd.name()=="UNSET" ) {
		unsetvars(cmd.args);
	}

	else {
		cerr << "unknown command " << program[prognr] << endl;
		exit(exit_failure);
	}
}

void checktoken(const command& cmd)
{
	currcmd = cmd;
	debug("checking token %s at %lu,%lu",
	      cmd.name().c_str(),data.line(),data.lpos());

	if ( cmd.name()=="SPACE" ) checkspace();

	else if ( cmd.name()=="NEWLINE" ) checknewline();

	else if ( cmd.name()=="INT" ) {
		// Accepts format (0|-?[1-9][0-9]*), i.e. no leading zero's
		// and no '-0' accepted.
		string num;
		while ( isdigit(data.peek()) || (num.empty() && data.peek()=='-') ) {
			num += data.readchar();
		}

		mpz_class lo = eval(cmd.args[0]);
		mpz_class hi = eval(cmd.args[1]);

//		debug("%s <= %s <= %s",lo.get_str().c_str(),num.c_str(),hi.get_str().c_str());
		if ( cmd.nargs()>=3 ) debug("'%s' = '%s'",
		                            const_cast<char *>(cmd.args[2].c_str()),
		                            const_cast<char *>(num.c_str()));

		if ( num.size()==0 ) error();
		if ( num.size()>=2 && num[0]=='0' ) error("prefix zero(s)");
		if ( num.size()>=1 && num[0]=='-' &&
		     (num.size()==1 || num[1]=='0') ) error("invalid minus sign (-0 not allowed)");

		mpz_class x(num);

		if ( x<lo || x>hi ) error("value out of range");
		if ( cmd.nargs()>=3 ) setvar(cmd.args[2],value_t(x));
	}

	else if ( cmd.name()=="FLOAT" || cmd.name()=="FLOATP" ) {
		// Accepts format -?[0-9]+(\.[0-9]+)?([eE][+-]?[0-9]+)?
		// where the last optional part, the exponent, is not allowed
		// with the FIXED option and required with the SCIENTIFIC option.

		size_t nbaseargs = 2;
		int decrange[2];
		if ( cmd.name()=="FLOATP" ) {
			// Read {min,max}decimals range for float.
			nbaseargs = 4;
			getdecrange(cmd,decrange);
		}

		int opt = 0; // 1 = scientific, 2 = fixed.
		if ( cmd.nargs()>=nbaseargs+2 ) {
			if ( cmd.args[nbaseargs+1].name()=="SCIENTIFIC" ) opt = 1;
			else if ( cmd.args[nbaseargs+1].name()=="FIXED" ) opt = 2;
			else {
				cerr << "invalid option in " << program[prognr] << endl;
				exit(exit_failure);
			}
		}

		size_t start = data.pos();
		// Match optional minus sign:
		if ( data.peek()=='-' ) data.readchar();
		// Match base with optional decimal dot:
		if ( !isdigit(data.peek()) ) error("digit expected");
		size_t digitpos = data.pos(), dotpos = string::npos;
		char first_digit = data.peek();
		while ( (isdigit(data.peek()) ||
		         (dotpos==string::npos && digitpos!=data.pos() && data.peek()=='.')) ) {
			if ( data.readchar()=='.' ) dotpos = data.pos()-1;
		}
		// Check that any dot is followed by digit:
		if ( !isdigit(data.peek(-1)) ) error("digit expected");

		size_t exppos = data.pos();
		bool has_exp = false;
		// Match exponent:
		if ( opt==1 || (opt==0 && toupper(data.peek())=='E') ) {
			if ( toupper(data.readchar())!='E' ) error("exponent 'E' expected");
			has_exp = true;
			if ( data.peek()=='-' || data.peek()=='+' ) data.readchar();
			while ( isdigit(data.peek()) ) data.readchar();
			if ( !isdigit(data.peek(-1)) ) error("digit expected");
		}

		if ( cmd.name()=="FLOATP" ) {
			if ( has_exp && (first_digit=='0' || dotpos!=digitpos+1) ) {
				error("exactly one non-zero before the decimal dot expected");
			}
			int ndecimals = (dotpos==string::npos ? 0 : exppos - dotpos - 1);
			if ( ndecimals<decrange[0] || ndecimals>decrange[1] ) {
				char tmp[100];
				sprintf(tmp,"%d not in [%d,%d]",ndecimals,decrange[0],decrange[1]);
				error("number of decimals not within specified range: "+string(tmp));
			}
		}

		string matchstr = data.prev(data.pos()-start);

		debug("parsing float '%s', exponent = %d",matchstr.c_str(),has_exp);

		mpf_class x(matchstr,4*matchstr.length());

		mpf_class lo = eval(cmd.args[0]);
		mpf_class hi = eval(cmd.args[1]);

		if ( x<lo || x>hi ) error("value out of range");
		if ( cmd.nargs()>=nbaseargs+1 ) setvar(cmd.args[nbaseargs],value_t(x));
	}

	else if ( cmd.name()=="STRING" ) {
		string str = eval(cmd.args[0]).getstr();
		for (size_t i=0; i<str.size(); i++) {
			if ( data.eof() ) error("premature end of file");
			if ( data.readchar()!=str[i] ) error();
		}

		debug("'%s' = '%s'",str.c_str(),cmd.args[0].c_str());
	}

	else if ( cmd.name()=="REGEX" ) {
		string str = eval(cmd.args[0]).getstr();
		regex regexstr(str,regex::extended|regex::nosubs|regex::optimize);
		smatch res;
		string matchstr;

		string searchstr = data.next(data.size());
		if ( !regex_search(searchstr,res,regexstr,regex_constants::match_continuous) ) {
			error();
		} else {
			size_t match_len = res[0].second - res[0].first;
			size_t match_end = data.pos() + match_len;
			matchstr = data.next(match_len);
			while ( data.pos()<match_end ) data.readchar();
		}
		debug("'%s' = '%s'",matchstr.c_str(),str.c_str());

		if ( cmd.nargs()>=2 ) setvar(cmd.args[1],value_t(matchstr));
	}

	else if ( cmd.name()=="ASSERT" ) {
		if ( !dotest(cmd.args[0]) ) error("assertion failed");
	}

	else if ( cmd.name()=="SET" ) {
		setvars(cmd.args);
	}

	else if ( cmd.name()=="UNSET" ) {
		unsetvars(cmd.args);
	}

	else {
		cerr << "unknown command " << program[prognr] << endl;
		exit(exit_failure);
	}
}

// This function processes the outer control structure commands both
// for checking and generating testdata (as determined by the global
// variable 'gendata').
void checktestdata(ostream &datastream)
{
	datastream << setprecision(float_precision);

	while ( true ) {
		const command &cmd = program[prognr];
		currcmd = cmd;

		if ( cmd.name()=="EOF" ) {
			if ( gendata ) {
				debug("done: EOF found");
				return;
			} else {
				debug("checking EOF");
				if ( !data.eof() ) error();
				throw eof_found_exception();
			}
		}

		else if ( loop_cmds.count(cmd.name()) ) {

			// Current and maximum loop iterations.
			unsigned long i = 0, times = ULONG_MAX;
			unsigned loopvar = 0; // Optional variable for loop iteration present.

			if ( cmd.name()=="REPI" || cmd.name()=="WHILEI" ) {
				loopvar = 1;
				setvar(cmd.args[0],value_t(mpz_class(i)));
			}

			if ( cmd.name()=="REP" || cmd.name()=="REPI" ) {
				mpz_class n = eval(cmd.args[loopvar]);
				if ( !n.fits_ulong_p() ) {
					cerr << "'" << n << "' does not fit in an unsigned long in "
						 << program[prognr] << endl;
					exit(exit_failure);
				}
				times = n.get_ui();
			}

			// Begin and end of loop commands
			int loopbegin, loopend;

			loopbegin = loopend = prognr + 1;

			for(int looplevel=1; looplevel>0; ++loopend) {
				string cmdstr = program[loopend].name();
				if ( loop_cmds.count(cmdstr) || cmdstr=="IF") looplevel++;
				if ( cmdstr=="END" ) looplevel--;
			}

			// Run loop...
			debug("running %s loop, commands %d - %d, max. times = %ld",
			      cmd.name().c_str(),loopbegin,loopend,times);

			while ( ((cmd.name()=="REP"   || cmd.name()=="REPI")   && i<times) ||
			        ((cmd.name()=="WHILE" || cmd.name()=="WHILEI") &&
			         dotest(cmd.args[loopvar])) ) {

				debug("loop iteration %ld/%ld",i+1,times);
				prognr = loopbegin;
				if ( i>0 && cmd.nargs()>=loopvar+2 ) {
					if ( gendata ) {
						gentoken(cmd.args[loopvar+1], datastream);
					} else {
						checktoken(cmd.args[loopvar+1]);
					}
				}
				checktestdata(datastream);
				i++;
				if ( loopvar ) setvar(cmd.args[0],value_t(mpz_class(i)));
			}

			// And skip to end of loop
			prognr = loopend;
		}

		else if ( cmd.name()=="IF" ) {

			// Find line numbers of matching else/end
			int ifnr   = prognr;
			int elsenr = -1;
			int endnr  = prognr+1;

			for(int looplevel=1; looplevel>0; ++endnr) {
				string cmdstr = program[endnr].name();
				if ( loop_cmds.count(cmdstr) || cmdstr=="IF") looplevel++;
				if ( cmdstr=="END" ) looplevel--;
				if ( cmdstr=="ELSE" && looplevel==1) elsenr = endnr;
			}
			endnr--;

			debug("IF statement, if/else/end commands: %d/%d/%d",
			      ifnr,elsenr,endnr);

			// Test and execute correct command block
			if (dotest(cmd.args[0])) {
				debug("executing IF clause");
				prognr = ifnr+1;
				checktestdata(datastream);
			}
			else if (elsenr!=-1) {
				debug("executing ELSE clause");
				prognr = elsenr+1;
				checktestdata(datastream);
			}

			prognr = endnr+1;
		}

		else if ( cmd.name()=="END" || cmd.name()=="ELSE" ) {
			debug("scope closed by %s",cmd.name().c_str());
			prognr++;
			return;
		}

		else {
			if ( gendata ) {
				gentoken(cmd, datastream);
			} else {
				checktoken(cmd);
			}
			prognr++;
		}
	}
}

void init_checktestdata(std::istream &progstream, int opt_mask)
{
	// Output floats with high precision:
	cout << setprecision(float_precision);
	cerr << setprecision(float_precision);
	mpf_set_default_prec(256);

	// Check the options bitmask
	if (opt_mask & opt_whitespace_ok) whitespace_ok = 1;
	if (opt_mask & opt_debugging    ) debugging = 1;
	if (opt_mask & opt_quiet        ) quiet = 1;

	// Initialize block_cmds here, as a set cannot be initialized on
	// declaration.
	loop_cmds.insert("REP");
	loop_cmds.insert("REPI");
	loop_cmds.insert("WHILE");
	loop_cmds.insert("WHILEI");

	// Read program and testdata
	readprogram(progstream);

	if ( debugging ) {
		for(size_t i=0; i<program.size(); i++) cerr << program[i] << endl;
	}

	// Initialize random generator
	struct timespec time;
	clock_gettime(CLOCK_REALTIME,&time);
	mpz_class seed = 1000000000 * mpz_class(time.tv_sec) + time.tv_nsec;
	gmp_rnd.seed(seed);

	// Initialize current position in program.
	prognr = 0;
}

bool gentestdata(ostream &datastream)
{
	// Generate random testdata
	gendata = 1;

	try {
		checktestdata(datastream);
	} catch (generate_exception) {
		return false;
	}

	return true;
}

bool checksyntax(istream &datastream)
{
	ofstream dummy;

	gendata = 0;
	readtestdata(datastream);

	// If we ignore whitespace, skip leading whitespace on first line
	// as a special case; other lines are handled by checknewline().
	if ( whitespace_ok ) data.readwhitespace();

	try {
		checktestdata(dummy);
	}
	catch (doesnt_match_exception) {
		return false;
	}
	catch (eof_found_exception) {}

	return true;
}

// This doesn't support variables with indices (yet?).
bool parse_preset_list(std::string str)
{
	std::istringstream in(str);
	Parser parselist(in, (int)Parser::PARSE_ASSIGNLIST);

	debug("parsing preset list '%s'", str.c_str());

	try {
		if ( parselist.parse()!=0 ) {
			cerr << "parse error reading preset list" << endl;
			return false;
		}
	} catch ( exception& e ) {
		cerr << "parse error: " << e.what() << endl;
		return false;
	}

	try {
		setvars(parselist.parseResult.args,1);
	} catch ( boost::exception& e ) {
		cerr << "error in boost variant: " << boost::diagnostic_information(e) << endl;
		return false;
	}

	return true;
}

} // namespace checktestdata


#ifndef pear_func_hpp
#define pear_func_hpp

#include <Eigen/Core>
#include <Eigen/Sparse>

// TYPE DEFINITIONS
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpMat;

namespace pear {
void fun_diff(Vec &Cu, Vec &Cv, Vec &Ru, Vec &Rv, Mat &RudCu, Mat &RudCv,
              Mat &RvdCu, Mat &RvdCv);
} // namespace pear

#endif


#include "NuiPolygonMesh.h"

#include "Foundation/NuiLogger.h"

#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

#include <boost/make_shared.hpp>

NuiPolygonMesh::NuiPolygonMesh()
	: m_dirty(false)
	, m_numEvaluatedTriangles(0)
{

}

NuiPolygonMesh::~NuiPolygonMesh()
{
	clear();
}

void	NuiPolygonMesh::clear()
{
	m_indices.clear();
	m_cloud.clear();
	//m_mesh.cloud.clear();
	m_numEvaluatedTriangles = 0;
	setDirty();
}

void	NuiPolygonMesh::calculateMesh(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals, float leafSize)
{
	LOG4CPLUS_INFO(NuiLogger::instance().consoleLogger(), "Calculating polygon mesh...");

	// Create search tree*
	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
	tree2->setInputCloud(cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;

	// Set the maximum distance between connected points (maximum edge length)
	gp3.setSearchRadius(leafSize * 2.0);

	// Set typical values for the parameters
	gp3.setMu(2);
	gp3.setMaximumNearestNeighbors(100);
	gp3.setMaximumSurfaceAngle(M_PI / 2); // 45 degrees
	gp3.setMinimumAngle(M_PI / 36); // 10 degrees
	gp3.setMaximumAngle(2.5 * M_PI / 3); // 120 degrees
	gp3.setNormalConsistency(false);

	// Get result
	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree2);

	gp3.reconstruct(m_mesh);
	setDirty();
	LOG4CPLUS_INFO(NuiLogger::instance().consoleLogger(), "Calculate polygon mesh completed.");
}

void	NuiPolygonMesh::save(std::string file)
{
	LOG4CPLUS_INFO(NuiLogger::instance().consoleLogger(), "Saving polygon mesh...");
	std::string filePLY = file;
	filePLY.append(".ply");
	pcl::io::savePLYFile(filePLY, m_mesh, 5);
	LOG4CPLUS_INFO(NuiLogger::instance().consoleLogger(), "Save polygon mesh completed.");
}

void	NuiPolygonMesh::evaluateMesh()
{
	int numTriangles = getTrianglesNum();
	if (m_numEvaluatedTriangles >= numTriangles)
		return;
	for (int i = m_numEvaluatedTriangles; i < numTriangles; i++)
	{
		m_indices.insert(m_indices.end(), m_mesh.polygons.at(i).vertices.begin(), m_mesh.polygons.at(i).vertices.end());
	}
	pcl::fromPCLPointCloud2(m_mesh.cloud, m_cloud);
	m_numEvaluatedTriangles = numTriangles;
}

const pcl::PointXYZRGBNormal*		NuiPolygonMesh::getVerticesBuffer() const
{
	return m_cloud.points.data();
}

UINT		NuiPolygonMesh::getVerticesBufferSize() const
{
	return m_cloud.points.size() * sizeof(pcl::PointXYZRGBNormal);
}

const uint32_t*	NuiPolygonMesh::getIndicesBuffer() const
{
	return m_indices.empty() ? NULL : &m_indices[0];
}

UINT		NuiPolygonMesh::getIndicesBufferSize() const
{
	return m_indices.size() * sizeof(uint32_t);
}