/*
 * Copyright 2019,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <pinocchio/multibody/liegroup/special-euclidean.hpp>

#include <iostream>
#include <sot/core/debug.hh>

#ifndef WIN32
#include <unistd.h>
#endif

using namespace std;

#include <boost/test/unit_test.hpp>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <sot/core/device.hh>
#include <sstream>

using namespace dynamicgraph;
using namespace dynamicgraph::sot;

class TestDevice : public dg::sot::Device {
public:
  TestDevice(const std::string &RobotName) : Device(RobotName) {
    timestep_ = 0.001;
  }
  ~TestDevice() {}
};

BOOST_AUTO_TEST_CASE(test_device) {

  TestDevice aDevice(std::string("simple_humanoid"));

  /// Fix constant vector for the control entry in position
  dg::Vector aStateVector(38);
  dg::Vector aVelocityVector(38);
  dg::Vector aLowerVelBound(38), anUpperVelBound(38);
  dg::Vector aLowerBound(38), anUpperBound(38);
  dg::Vector anAccelerationVector(38);
  dg::Vector aControlVector(38);

  for (unsigned int i = 0; i < 38; i++) {
    // Specify lower velocity bound
    aLowerVelBound[i] = -3.14;
    // Specify lower position bound
    aLowerBound[i] = -3.14;
    // Specify state vector
    aStateVector[i] = 0.1;
    // Specify upper velocity bound
    anUpperVelBound[i] = 3.14;
    // Specify upper position bound
    anUpperBound[i] = 3.14;
    // Specify control vector
    aControlVector(i) = 0.1;
  }

  dg::Vector expected = aStateVector; // backup initial state vector

  /// Specify state size
  aDevice.setStateSize(38);
  /// Specify state bounds
  aDevice.setPositionBounds(aLowerBound, anUpperBound);
  /// Specify velocity size
  aDevice.setVelocitySize(38);
  /// Specify velocity
  aDevice.setVelocity(aStateVector);
  /// Specify velocity bounds
  aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound);
  /// Specify current state value
  aDevice.setState(aStateVector); // entry signal in position
  /// Specify constant control value
  aDevice.controlSIN.setConstant(aControlVector);

  const double dt = 0.001;
  const unsigned int N = 2000;
  for (unsigned int i = 0; i < N; i++) {
    aDevice.increment(dt);
    if (i == 0) {
      aDevice.stateSOUT.get(std::cout);
      std::ostringstream anoss;
      aDevice.stateSOUT.get(anoss);
    }
    if (i == 1) {
      aDevice.stateSOUT.get(std::cout);
      std::ostringstream anoss;
      aDevice.stateSOUT.get(anoss);
    }
  }

  aDevice.display(std::cout);
  aDevice.cmdDisplay();

  // verify correct integration
  typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
  Eigen::Matrix<double, 7, 1> qin, qout;
  qin.head<3>() = expected.head<3>();

  Eigen::QuaternionMapd quat(qin.tail<4>().data());
  quat = Eigen::AngleAxisd(expected(5), Eigen::Vector3d::UnitZ()) *
         Eigen::AngleAxisd(expected(4), Eigen::Vector3d::UnitY()) *
         Eigen::AngleAxisd(expected(3), Eigen::Vector3d::UnitX());

  const double T = dt * N;
  Eigen::Matrix<double, 6, 1> control = aControlVector.head<6>() * T;
  SE3().integrate(qin, control, qout);

  // Manual integration
  expected.head<3>() = qout.head<3>();
  expected.segment<3>(3) = Eigen::QuaternionMapd(qout.tail<4>().data())
                               .toRotationMatrix()
                               .eulerAngles(2, 1, 0)
                               .reverse();
  for (int i = 6; i < expected.size(); i++)
    expected[i] = 0.3;

  std::cout << expected.transpose() << std::endl;
  std::cout << aDevice.stateSOUT(N).transpose() << std::endl;

  BOOST_CHECK(aDevice.stateSOUT(N).isApprox(expected));
}


#ifndef AGATE_PRIS_APLAS_QVM_HPP
#define AGATE_PRIS_APLAS_QVM_HPP

#include <boost/qvm/mat.hpp>
#include <boost/qvm/quat.hpp>
#include <boost/qvm/vec.hpp>

namespace agate_pris {
namespace aplas {

using vector_2 = boost::qvm::vec<float, 2>;
using vector_3 = boost::qvm::vec<float, 3>;
using vector_4 = boost::qvm::vec<float, 4>;

using vector_2_int = boost::qvm::vec<int, 2>;
using vector_3_int = boost::qvm::vec<int, 3>;

using matrix_4_x_4 = boost::qvm::mat<float, 4, 4>;

using quaternion = boost::qvm::quat<float>;

}
}

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Clement Jamin
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Subsampling - test sparsify_point_set
#include <boost/test/unit_test.hpp>

#include <gudhi/sparsify_point_set.h>

#include <CGAL/Epick_d.h>
#include <CGAL/Random.h>

#include <vector>
#include <iterator>

BOOST_AUTO_TEST_CASE(test_sparsify_point_set)
{
  typedef CGAL::Epick_d<CGAL::Dimension_tag<4> >   K;
  typedef typename K::Point_d                      Point_d;

  CGAL::Random rd;

  std::vector<Point_d> points;
  for (int i = 0 ; i < 500 ; ++i)
    points.push_back(Point_d(rd.get_double(-1.,1),rd.get_double(-1.,1),rd.get_double(-1.,1),rd.get_double(-1.,1)));

  K k;
  std::vector<Point_d> results;
  Gudhi::subsampling::sparsify_point_set(k, points, 0.5, std::back_inserter(results));
  std::clog << "Before sparsification: " << points.size() << " points.\n";
  std::clog << "After  sparsification: " << results.size() << " points.\n";
  //for (auto p : results)
  //  std::clog << p << "\n";

  BOOST_CHECK(points.size() > results.size());
}


//
// VariationalBayesEstimatorOnLDA.hpp
//
// Copyright (c) 2017 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#ifndef VBLDA
#define VBLDA

#include<stdlib.h>
#include<math.h>
#include<cmath>
#include<iostream>
#include<vector>
#include<numeric>
#include<memory>
#include<random>
#include<iomanip>
#include<fstream>
#include<limits>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include"BOWFileParser.hpp"
#include"utils.hpp"

enum BetaUpdateManner{
    SYMMETRY, ASYMMETRY
};

class VariationalBayesEstimatorOnLDA{
protected:
    const std::vector<std::vector<unsigned int> > &_frequencyMatrix;
    const std::vector<std::vector<unsigned int> > &_docVoca;
    const unsigned int _K, _V, _D;
    double _convergenceDiterminationRate;
    std::vector<std::vector<std::vector<double> > > _qz;
    std::vector<std::vector<double> > _thetaEx, _phiEx;
    std::vector<double> _alpha, _beta;
    double _alphaSum, _betaSum;
    std::vector<std::vector<double> > _alphaTimeSeries, _betaTimeSeries;
    std::vector<double> _nd, _nk;
    std::vector<std::vector<double> > _ndk, _nkv;
    double _variationalLowerBound;
    std::vector<double> _VLBTimeSeries;
public:
    VariationalBayesEstimatorOnLDA(const BOWFileParser &parser, const unsigned int K, const double convergenceDiterminationRate);
    virtual ~VariationalBayesEstimatorOnLDA();
    virtual void initializeHyperParam();
    virtual void initializeParam();
    virtual void calculateEx();
    virtual void calculateHyperParamSum();
    virtual void updateQz();
    virtual void updateNEx();
    virtual void updateBeta(BetaUpdateManner manner);
    virtual void updateHyperParameters();
    virtual double calculateVariationalLowerBound()const;
    virtual void writeParameter(std::string thetaFilename, std::string phiFilename, std::string alphaFilename, std::string betaFilename)const;
    virtual void writeVariationalLowerBound(std::string VLBFilename, std::string VLBTimeSeriesFilename)const;
    virtual void runIteraions();
};

#endif


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::prepare_weights.hpp                                  //
//                                                                           //
//  Copyright 2009 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_IMPORTANCE_SAMPLING_WEIGHTS_PREPARE_WEIGHTS_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_PREPARE_WEIGHTS_HPP_ER_2009
#include <iterator>
#include <functional>
#include <boost/format.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/statistics/detail/importance_sampling/weights/apply_exp_offset.hpp>
#include <boost/statistics/detail/importance_sampling/weights/scale_to_finite_sum.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    // Warning: read side effects carefully.
    template<typename T>
    class prepare_weights{
        public:
        typedef T value_type;
        typedef std::size_t size_type;

        prepare_weights()
        :max_log_(default_max_log),offset_(zero),scaling_factor_(zero){}

        prepare_weights(value_type max_log)
        :max_log_(max_log),offset_(zero),scaling_factor_(zero){}

        // Default copy/assign

        // [ Input ]
        // max_log controls precision hence raising it should decr pc_lt_eps
        // but also incr risk that cum_sum isinf.
        value_type max_log()const{ return this->max_log_; }

        // [ Output ]
        // lw <- lw + offset, max{lw}+offset = max_log
        value_type offset()const{ return this->offset_; }
        // w <- w/c such that sum{w/c}<inf
        value_type scaling_factor()const{ return this->scaling_factor_; }
        value_type count()const{ return this->n_; }

        // [ Side effect ]
        // 1) w <- exp(lw+offset)
        // 2) if needed, w <- w/c such that sum{w} < inf
        template<typename ItW>
        void operator()(ItW b_w,ItW e_w) // log( unnormalized weights )
        {
            this->offset_ = apply_exp_offset(
                b_w,
                e_w,
                this->max_log()
            );

            // if max_log is small enough (which costs precision), this does not
            // nothing i.e. scaling_factor = 1
            this->scaling_factor_ = scale_to_finite_sum(
                b_w,
                e_w
            );

            this->n_ = std::distance(b_w,e_w);
        }

        static const char* header;
        private:
        value_type max_log_;
        value_type offset_;
        value_type scaling_factor_;
        size_type n_;
        static const value_type zero;
        static const value_type default_max_log;

    };

    template<typename T>
    std::ostream& operator<<(
        std::ostream& out,
        const prepare_weights<T>& that
    ){
        out <<
            (
                boost::format("(%1%,%2%,%3%)")
                % that.count()
                % that.offset()
                % that.scaling_factor()
            ).str();
        return out;
    }

    template<typename T>
    const char* prepare_weights<T>::header
        = "(count, offset,scaling_factor)";

    template<typename T>
    const typename prepare_weights<T>::value_type
    prepare_weights<T>::default_max_log = static_cast<value_type>(0);

    template<typename T>
    const typename prepare_weights<T>::value_type
    prepare_weights<T>::zero = static_cast<value_type>(0);

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

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file		testDecisionTree.cpp
 * @brief		Develop DecisionTree
 * @author	Frank Dellaert
 * @author	Can Erdogan
 * @date	  Jan 30, 2012
 */

#include <boost/timer.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>

//#define DT_DEBUG_MEMORY
//#define DT_NO_PRUNING
#define DISABLE_DOT
#include <gtsam/discrete/DecisionTree-inl.h>
using namespace std;
using namespace gtsam;

template<typename T>
void dot(const T&f, const string& filename) {
#ifndef DISABLE_DOT
	f.dot(filename);
#endif
}

#define DOT(x)(dot(x,#x))

struct Crazy { int a; double b; };
typedef DecisionTree<string,Crazy> CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be)

/* ******************************************************************************** */
// Test string labels and int range
/* ******************************************************************************** */

typedef DecisionTree<string, int> DT;

struct Ring {
	static inline int zero() {
		return 0;
	}
	static inline int one() {
		return 1;
	}
	static inline int add(const int& a, const int& b) {
		return a + b;
	}
	static inline int mul(const int& a, const int& b) {
		return a * b;
	}
};

/* ******************************************************************************** */
// test DT
TEST(DT, example)
{
	// Create labels
	string A("A"), B("B"), C("C");

	// create a value
	Assignment<string> x00, x01, x10, x11;
	x00[A] = 0, x00[B] = 0;
	x01[A] = 0, x01[B] = 1;
	x10[A] = 1, x10[B] = 0;
	x11[A] = 1, x11[B] = 1;

	// A
	DT a(A, 0, 5);
	LONGS_EQUAL(0,a(x00))
	LONGS_EQUAL(5,a(x10))
	DOT(a);

	// pruned
	DT p(A, 2, 2);
	LONGS_EQUAL(2,p(x00))
	LONGS_EQUAL(2,p(x10))
	DOT(p);

	// \neg B
	DT notb(B, 5, 0);
	LONGS_EQUAL(5,notb(x00))
	LONGS_EQUAL(5,notb(x10))
	DOT(notb);

	// apply, two nodes, in natural order
	DT anotb = apply(a, notb, &Ring::mul);
	LONGS_EQUAL(0,anotb(x00))
	LONGS_EQUAL(0,anotb(x01))
	LONGS_EQUAL(25,anotb(x10))
	LONGS_EQUAL(0,anotb(x11))
	DOT(anotb);

	// check pruning
	DT pnotb = apply(p, notb, &Ring::mul);
	LONGS_EQUAL(10,pnotb(x00))
	LONGS_EQUAL( 0,pnotb(x01))
	LONGS_EQUAL(10,pnotb(x10))
	LONGS_EQUAL( 0,pnotb(x11))
	DOT(pnotb);

	// check pruning
	DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul);
	LONGS_EQUAL(0,zeros(x00))
	LONGS_EQUAL(0,zeros(x01))
	LONGS_EQUAL(0,zeros(x10))
	LONGS_EQUAL(0,zeros(x11))
	DOT(zeros);

	// apply, two nodes, in switched order
	DT notba = apply(a, notb, &Ring::mul);
	LONGS_EQUAL(0,notba(x00))
	LONGS_EQUAL(0,notba(x01))
	LONGS_EQUAL(25,notba(x10))
	LONGS_EQUAL(0,notba(x11))
	DOT(notba);

	// Test choose 0
	DT actual0 = notba.choose(A, 0);
	EXPECT(assert_equal(DT(0.0), actual0));
	DOT(actual0);

	// Test choose 1
	DT actual1 = notba.choose(A, 1);
	EXPECT(assert_equal(DT(B, 25, 0), actual1));
	DOT(actual1);

	// apply, two nodes at same level
	DT a_and_a = apply(a, a, &Ring::mul);
	LONGS_EQUAL(0,a_and_a(x00))
	LONGS_EQUAL(0,a_and_a(x01))
	LONGS_EQUAL(25,a_and_a(x10))
	LONGS_EQUAL(25,a_and_a(x11))
	DOT(a_and_a);

	// create a function on C
	DT c(C, 0, 5);

	// and a model assigning stuff to C
	Assignment<string> x101;
	x101[A] = 1, x101[B] = 0, x101[C] = 1;

	// mul notba with C
	DT notbac = apply(notba, c, &Ring::mul);
	LONGS_EQUAL(125,notbac(x101))
	DOT(notbac);

	// mul now in different order
	DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul);
	LONGS_EQUAL(125,acnotb(x101))
	DOT(acnotb);
}

/* ******************************************************************************** */
// test Conversion
enum Label {
	U, V, X, Y, Z
};
typedef DecisionTree<Label, bool> BDT;
bool convert(const int& y) {
	return y != 0;
}

TEST(DT, conversion)
{
	// Create labels
	string A("A"), B("B");

	// apply, two nodes, in natural order
	DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul);

	// convert
	map<string, Label> ordering;
	ordering[A] = X;
	ordering[B] = Y;
	boost::function<bool(const int&)> op = convert;
	BDT f2(f1, ordering, op);
	//	f1.print("f1");
	//	f2.print("f2");

	// create a value
	Assignment<Label> x00, x01, x10, x11;
	x00[X] = 0, x00[Y] = 0;
	x01[X] = 0, x01[Y] = 1;
	x10[X] = 1, x10[Y] = 0;
	x11[X] = 1, x11[Y] = 1;
	EXPECT(!f2(x00));
	EXPECT(!f2(x01));
	EXPECT(f2(x10));
	EXPECT(!f2(x11));
}

/* ******************************************************************************** */
// test Compose expansion
TEST(DT, Compose)
{
	// Create labels
	string A("A"), B("B"), C("C");

	// Put two stumps on A together
	DT f1(B, DT(A, 0, 1), DT(A, 2, 3));

	// Create from string
	vector<DT::LabelC> keys;
	keys += DT::LabelC(A,2), DT::LabelC(B,2);
	DT f2(keys, "0 2 1 3");
	EXPECT(assert_equal(f2, f1, 1e-9));

	// Put this AB tree together with another one
	DT f3(keys, "4 6 5 7");
	DT f4(C, f1, f3);
	DOT(f4);

	// a bigger tree
	keys += DT::LabelC(C,2);
	DT f5(keys, "0 4 2 6 1 5 3 7");
	EXPECT(assert_equal(f5, f4, 1e-9));
	DOT(f5);
}

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


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#include <mfmg/common/instantiation.hpp>
#include <mfmg/common/utils.hpp>
#include <mfmg/dealii/dealii_solver.hpp>
#include <mfmg/dealii/dealii_trilinos_matrix_operator.hpp>

#include <deal.II/lac/trilinos_precondition.h>
#include <deal.II/lac/trilinos_solver.h>

#include <ml_MultiLevelPreconditioner.h>
#include <ml_Preconditioner.h>

namespace mfmg
{
template <typename VectorType>
DealIISolver<VectorType>::DealIISolver(
    std::shared_ptr<Operator<VectorType> const> op,
    std::shared_ptr<boost::property_tree::ptree const> params)
    : Solver<VectorType>(op, params)
{
  std::string coarse_type = this->_params->get("coarse.type", "direct");

  // We only do smoothing if the operator is a DealIITrilinosMatrixOperator
  auto trilinos_operator =
      std::dynamic_pointer_cast<DealIITrilinosMatrixOperator<VectorType> const>(
          this->_operator);
  auto sparse_matrix = trilinos_operator->get_matrix();
  // Make parameters case-insensitive
  std::string coarse_type_lower = coarse_type;
  std::transform(coarse_type_lower.begin(), coarse_type_lower.end(),
                 coarse_type_lower.begin(), ::tolower);

  if (coarse_type_lower == "direct")
  {
    _solver.reset(new dealii::TrilinosWrappers::SolverDirect(_solver_control));
    _solver->initialize(*sparse_matrix);
  }
  else
  {
    if (coarse_type_lower == "ml")
    {
      auto ml_tree = params->get_child_optional("coarse.params");

      // For now, always set defaults to SA
      Teuchos::ParameterList ml_params;
      ML_Epetra::SetDefaults("SA", ml_params);

      if (ml_tree)
      {
        // Augment with user provided parameters
        ptree2plist(*ml_tree, ml_params);
      }

      _smoother.reset(new dealii::TrilinosWrappers::PreconditionAMG());
      static_cast<dealii::TrilinosWrappers::PreconditionAMG *>(_smoother.get())
          ->initialize(*sparse_matrix, ml_params);
    }
    else
    {
      ASSERT_THROW(false,
                   "Unknown coarse solver name: \"" + coarse_type_lower + "\"");
    }
  }
}

template <typename VectorType>
void DealIISolver<VectorType>::apply(VectorType const &b, VectorType &x) const
{
  if (_solver)
  {
    _solver->solve(x, b);
  }
  else
  {
    _smoother->vmult(x, b);
  }
}
} // namespace mfmg

// Explicit Instantiation
INSTANTIATE_VECTORTYPE(TUPLE(DealIISolver))


/*!
 * @file
 * Contains unit tests for the @ref Ring typeclass.
 */

#include <boost/mpl11/ring.hpp>

#include <boost/mpl11/core.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Ring<Archetype<0>> : instantiate<Ring>::with<Archetype<0>> {
        template <typename, typename> using mult_impl = method_tag<mult>;
        template <typename ...>       using one_impl  = method_tag<one>;
    };
}}

struct tests
    : method<mult, archetype<0>, archetype<0>>
    // , method<mult, archetype<0>, archetype<1>>
    // , method<mult, archetype<1>, archetype<0>>
    , method<one, Archetype<0>>
{ };

int main() { }


//
// Created by Saurabh Khanduja on 18.10.18.
//

#include "DatasetReader.h"
#include <sstream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <limits>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include "config.h"

DatasetReader::DatasetReader(const std::string DatasetRootDir)
{
  m_imageDir = DatasetRootDir + imageDir[datasetType];
  m_numImageFiles = numImageFiles[datasetType];
  std::vector<double> intrinsicParams = LoadMatrixFromFile(DatasetRootDir + intrinsicParamsFile, 3 * 3);
  m_depthIntrinsicMatrix << intrinsicParams[0], intrinsicParams[1], intrinsicParams[2],
      intrinsicParams[3], intrinsicParams[4], intrinsicParams[5],
      intrinsicParams[6], intrinsicParams[7], intrinsicParams[8];
  m_depthHeight = 480;
  m_depthWidth = 640;
  analyzeMinMaxDepthValues(datasetType);
}

/**
 * This method reads the value from intrinsic file into a vector.
 * ToDo: Check C++14 standard which allows to return by reference instead of value.
 *
 * @param filename The file to be read
 * @param M The number of elements in matrix
 * @return A vector of all the values read as double.
 */
std::vector<double> DatasetReader::LoadMatrixFromFile(std::string filename, int M)
{
  std::vector<double> matrix;
  FILE *fp = fopen(filename.c_str(), "r");
  for (int i = 0; i < M; i++)
  {
    double tmp;
    int iret = fscanf(fp, "%lf", &tmp);
    matrix.push_back(tmp);
  }
  fclose(fp);
  return matrix;
}

std::vector<cv::Mat> DatasetReader::getImages(int frameIndex)
{
  std::ostringstream frameSuffix;
  frameSuffix << std::setw(6) << std::setfill('0') << frameIndex;
  std::string suffix = frameSuffix.str();
  std::string colorFile = m_imageDir + "/color_" + suffix + ".png";
  std::string depthFile = m_imageDir + "/depth_" + suffix + ".png";
  std::string omaskFile = m_imageDir + "/omask_" + suffix + ".png";

  std::vector<cv::Mat> cdImages;
  cdImages.push_back(cv::imread(colorFile));
  cv::Mat depthImage = readDepthImage(depthFile);
  cv::Mat mask = cv::imread(omaskFile, cv::IMREAD_GRAYSCALE);
  cv::Mat maskedDepthImage;
  depthImage.copyTo(maskedDepthImage, mask);
  cdImages.push_back(maskedDepthImage);
  return cdImages;
}

int DatasetReader::getNumImageFiles() const
{
  return m_numImageFiles;
}

void DatasetReader::analyzeMinMaxDepthValues(const DEFORMABLE_DATASET dataset)
{
  if (dataset < 2) // Always true in current case.
  {
    // Using cached values
    m_minMaxDepth = std::pair<double, double>(datasetDepthMinMaxValues[dataset][0],
                                            datasetDepthMinMaxValues[dataset][1]);
    return;
  }

  std::vector<double> minDepths, maxDepths;
  minDepths.reserve(getNumImageFiles());
  maxDepths.reserve(getNumImageFiles());

  for (size_t frameIndex = 0; frameIndex < getNumImageFiles(); frameIndex++)
  {
    std::vector<cv::Mat> cdoImages = getImages(frameIndex);
    cv::Mat depthMat = cdoImages.at(1);
    int H = depthMat.rows;
    int W = depthMat.cols;
    double min = std::numeric_limits<double>::max();
    double max = std::numeric_limits<double>::min();
    for (int r = 0; r < H; ++r)
    {
      for (int c = 0; c < W; ++c)
      {
        double depth = depthMat.at<double>(r, c);
        // Ignore 0 values which represent invalid data
        if (min > depth && depth > 0)
        {
          min = depth;
        }
        if (max < depth)
        {
          max = depth;
        }
      }
    }
    minDepths.push_back(min);
    maxDepths.push_back(max);
    std::cout << "Depth Frame " << frameIndex << " has valid min*, max value as : " << min << ", " << max << '\n';
  }
  std::sort(minDepths.begin(), minDepths.end());
  std::sort(maxDepths.begin(), maxDepths.end());
  std::cout << "Minimum and Maximum depth in all frames= " << minDepths.at(0) << " " << maxDepths.at(maxDepths.size() - 1) << std::endl;
  m_minMaxDepth = std::pair<double, double>(minDepths.at(0), maxDepths.at(maxDepths.size() - 1));
}

cv::Mat DatasetReader::readDepthImage(std::string depthFilename)
{
  cv::Mat depthImage = cv::imread(depthFilename, CV_LOAD_IMAGE_UNCHANGED);
  if (depthImage.empty())
  {
    std::cout << "Error: depth image file not read!\n";
    cv::waitKey(0);
    exit(-1);
  }

  cv::Mat depthFloatImage(depthImage.size(), CV_64FC1);
  depthImage.convertTo(depthFloatImage, CV_64FC1, 1.0 / 1000); // Future tasks Move depthShift to config.h
  return depthFloatImage;
  // double minVal, maxVal;
  // cv::minMaxLoc(depthFloatImage, &minVal, &maxVal);
  // std::cout << "Depth Frame " << depthFilename.substr(depthFilename.size() - 7, 3) << " has minimum and maximum value as : " << minVal << ", " << maxVal << std::endl;
  // cv::Mat depthFloatFilteredImage;
  // cv::bilateralFilter(depthFloatImage, depthFloatFilteredImage, 5, 50, 50, cv::BORDER_DEFAULT);
  // cv::Mat dst;
  // cv::hconcat(depthFloatImage, depthFloatFilteredImage, dst);
  // dst = (dst - minVal) / (maxVal - minVal);
  // cv::imshow("Depth Image And Depth Image Filtered", dst);
  // cv::waitKey(100);
  // return depthFloatFilteredImage;
}

int DatasetReader::getDepthHeight()
{
  return m_depthHeight;
}

int DatasetReader::getDepthWidth()
{
  return m_depthWidth;
}

Eigen::Matrix3d DatasetReader::getDepthIntrinsicMatrix()
{
  return m_depthIntrinsicMatrix;
}


/******************************************************************************
 * Copyright (c) 2021, Zhengjun Liu <zjliu@casm.ac.cn>
 * Copyright (c) 2008, Mateusz Loskot <mateusz@loskot.net>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following
 * conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in
 *       the documentation and/or other materials provided
 *       with the distribution.
 *     * Neither the name of the Martin Isenburg or Iowa Department
 *       of Natural Resources nor the names of its contributors may be
 *       used to endorse or promote products derived from this software
 *       without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 ****************************************************************************/

#include <cstring>
#include <stdexcept>
#include <string>
#include <vector>
#include <iosfwd>
#include <algorithm>
#include <numeric>
#include <boost/dynamic_bitset.hpp>
#include <boost/lexical_cast.hpp>
#include "detail/private_utility.hpp"
#include "Point.h"
#include "Exception.h"
#include "Header.h"
#include "Field.h"

using namespace boost;

namespace hsl {

// descale the value given our scale/offset
template <typename T>
void getScaledValue(const T &valueIn, T &valueOut, double scale, double offset, ScaleOffsetOp scaleInOut)
{
    if (scaleInOut == SO_In)
    {
        valueOut = static_cast<T>(detail::sround((valueIn - offset) / scale));
    }else if (scaleInOut == SO_Out)
    {
        valueOut = static_cast<T>(detail::sround(scale * valueIn + offset));
    }
}

bool getScaledValue(const Variant &value, DataType rawType, Variant &rawValue, double scale, double offset)
{
    bool state = false;
    switch (rawType)
    {
        case DT_BIT:
        case DT_CHAR:
        case DT_UCHAR:
            {
                rawValue = value;
                state = true;
            }
            break;
        case DT_SHORT:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<int16_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_USHORT:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<uint16_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_LONG:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<int32_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_ULONG:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<uint32_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_LONGLONG:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<int64_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_ULONGLONG:
            {
                double v;
                if (value.getValue(v))
                {
                    rawValue.setValue(static_cast<uint64_t>(detail::sround((v - offset) / scale)));
                    state = true;
                }
            }
            break;
        case DT_FLOAT:
            {
                double v;
                if(value.getValue(v))
                {
                    rawValue.setValue(static_cast<float>((v - offset) / scale));
                    state = true;
                }
            }
            break;
        case DT_DOUBLE:
            {
                double v;
                if (value.getValue(v))
                {
                    rawValue.setValue((v - offset) / scale);
                    state = true;
                }
            }
            break;
        default:
            {
                state = false;
            }
            break;
    }

    return state;
}

Point::Point(const Header * hdr)
    :
     _header(hdr)
    , _default_header(DefaultHeader::get())
{
    _data.resize(hdr->getDataRecordLength());
    _data.assign(hdr->getDataRecordLength(), 0);
}

Point::Point(const Point & other)
    : _data(other._data)
    , _waveformData(other._waveformData)
    , _header(other.getHeader())
    , _default_header(DefaultHeader::get())
{
}

Point& Point::operator=(const Point & rhs)
{
    if (&rhs != this)
    {
        _data = rhs._data;
        _waveformData = rhs._waveformData;
        _header = rhs._header;
    }
    return *this;
}

void Point::setCoordinates(double const& x, double const& y, double const& z)
{
    setX(x);
    setY(y);
    setZ(z);
}

bool Point::equal(Point const& other) const
{
    // TODO - mloskot: Default epsilon is too small.
    //                 Is 0.00001 good as tolerance or too wide?
    //double const epsilon = std::numeric_limits<double>::epsilon();
    double const epsilon = 0.00001;

    double const dx = getX() - other.getX();
    double const dy = getY() - other.getY();
    double const dz = getZ() - other.getZ();

    // TODO: Should we compare other data members, besides the coordinates?

    if ((dx <= epsilon && dx >= -epsilon)
     && (dy <= epsilon && dy >= -epsilon)
     && (dz <= epsilon && dz >= -epsilon))
    {
        return true;
    }

    // If we do other members
    // bool compare_classification(uint8_t cls, uint8_t expected)
    // {
    //    // 31 is max index in classification lookup table
    //    clsidx = (cls & 31);
    //    assert(clsidx <= 31);
    //    return (clsidx == expected);
    // }

    return false;
}

bool Point::validate() const
{
    unsigned int flags = 0;
    //TODO: validate point

    return true;
}

bool Point::isValid() const
{
    //TODO: test if it is valid
    return true;
}

void Point::setHeader(Header const* header)
{

    if (!header)
    {
        throw libhsl_error("header reference for setHeader is void");
    }

    // If we don't have a header initialized, set the point's to the
    // one we were given.
    if (!_header) _header = header;

    // This is hopefully faster than copying everything if we don't have
    // any data set and nothing to worry about.
    uint16_t wanted_length = header->getDataRecordLength();
    uint32_t sum = std::accumulate(_data.begin(), _data.end(), 0);

    if (!sum) {
        std::vector<uint8_t> data;
        data.resize(wanted_length);
        data.assign(wanted_length, 0);
        _data = data;
        _header = header;
        return;
    }

    bool bApplyNewScaling = true;

    if (detail::compare_distance(header->getScaleX(), _header->getScaleX()) &&
        detail::compare_distance(header->getScaleY(), _header->getScaleY()) &&
        detail::compare_distance(header->getScaleZ(), _header->getScaleZ()) &&
        detail::compare_distance(header->getOffsetX(), _header->getOffsetX()) &&
        detail::compare_distance(header->getOffsetY(), _header->getOffsetY()) &&
        detail::compare_distance(header->getOffsetZ(), _header->getOffsetZ()))
        bApplyNewScaling = false;
    else
        bApplyNewScaling = true;

    if (wanted_length != _data.size())
    {
        // Manually copy everything but the header ptr
        // We can't just copy the raw data because its
        // layout is likely changing as a result of the
        // schema change.
        Point p(*this);
        _header = header;

        std::vector<uint8_t> data;
        data.resize(wanted_length);
        data.assign(wanted_length, 0);
        _data = data;
        _header = header;

        setX(p.getX());
        setY(p.getY());
        setZ(p.getZ());

        // FIXME: copy other custom fields here?  resetting the
        // headerptr can be catastrophic in a lot of cases.
    }

    // The header's scale/offset can change the raw storage of xyz.
    // SetHeader can result in a rescaling of the data.
    double x;
    double y;
    double z;

    if (bApplyNewScaling)
    {
        x = getX();
        y = getY();
        z = getZ();
    }

    _header = header;

    if (bApplyNewScaling)
    {
        setX(x);
        setY(y);
        setZ(z);
    }

}
Header const* Point::getHeader() const
{
    if (_header)
        return _header;
     else
        return &_default_header;
}

double Point::getX() const
{
    int32_t v = getRawX();

    double output  = (v * getHeader()->getScaleX()) + getHeader()->getOffsetX();

    return output;
}

double Point::getY() const
{
    int32_t v = getRawY();

    double output = (v * getHeader()->getScaleY()) + getHeader()->getOffsetY();
    return output;
}

double Point::getZ() const
{
    int32_t v = getRawZ();

    double output = 0;

    output  = (v * getHeader()->getScaleZ()) + getHeader()->getOffsetZ();

    return output;
}

void Point::setX( double const& value )
{
    int32_t v;
    double scale;
    double offset;

    scale = getHeader()->getScaleX();
    offset = getHeader()->getOffsetX();

    // descale the value given our scale/offset
    v = static_cast<int32_t>(detail::sround((value - offset) / scale));
    setRawX(v);
}

void Point::setY( double const& value )
{
    int32_t v;
    double scale;
    double offset;

    scale = getHeader()->getScaleY();
    offset = getHeader()->getOffsetY();

    // descale the value given our scale/offset
    v = static_cast<int32_t>(detail::sround((value - offset) / scale));
    setRawY(v);
}

void Point::setZ( double const& value )
{
    int32_t v;
    double scale;
    double offset;

    scale = getHeader()->getScaleZ();
    offset = getHeader()->getOffsetZ();

    // descale the value given our scale/offset
    v = static_cast<int32_t>(detail::sround((value - offset) / scale));
    setRawZ(v);
}

int32_t Point::getRawX() const
{
    std::vector<uint8_t>::size_type pos = 0;

#ifdef LIBHSL_ENDIAN_AWARE
    int32_t output = hsl::detail::bitsToInt<int32_t>(output, _data, pos);
    return output;
#else
    uint8_t* data = const_cast<uint8_t*>(&_data[0] + pos);
    int32_t* output = reinterpret_cast<int32_t*>(data);
    return *output;
#endif
}

int32_t Point::getRawY() const
{
    std::vector<uint8_t>::size_type pos = 4;

#ifdef LIBHSL_ENDIAN_AWARE
    int32_t output = hsl::detail::bitsToInt<int32_t>(output, _data, pos);
    return output;
#else
    uint8_t* data = const_cast<uint8_t*>(&_data[0] + pos);
    int32_t* output = reinterpret_cast<int32_t*>(data);
    return *output;
#endif
}

int32_t Point::getRawZ() const
{
    std::vector<uint8_t>::size_type pos = 8;

#ifdef LIBHSL_ENDIAN_AWARE
    int32_t output = hsl::detail::bitsToInt<int32_t>(output, _data, pos);
    return output;
#else
    uint8_t* data = const_cast<uint8_t*>(&_data[0] + pos);
    int32_t* output = reinterpret_cast<int32_t*>(data);
    return *output;
#endif
}

void Point::setRawX( int32_t const& value)
{
    std::vector<uint8_t>::size_type pos = 0;
    hsl::detail::intToBits<int32_t>(value, _data, pos);
}

void Point::setRawY( int32_t const& value)
{
    std::vector<uint8_t>::size_type pos = 4;
    hsl::detail::intToBits<int32_t>(value, _data, pos);
}

void Point::setRawZ( int32_t const& value)
{
    std::vector<uint8_t>::size_type pos = 8;
    hsl::detail::intToBits<int32_t>(value, _data, pos);
}

size_t Point::getFieldBytePosition(std::size_t pos) const
{
    Field d;
    bool found = _header->getSchema().getField(pos, d);

    if (!found)
    {
        std::ostringstream oss;
        oss <<"Field at position " << pos << " not found";
        throw libhsl_error(oss.str());
    }
    return d.getByteOffset();
}

bool Point::getFieldBytePositionsById(FieldId id, FieldBytePositionArray &bytePositions) const
{
    FieldArray d;
    bool found = _header->getSchema().getFieldsById(id, d);

    if (!found)
    {
        std::ostringstream oss;
        oss <<"Field id " << id << " not found";
        return false;
    }

    for (size_t i = 0; i < d.size(); i++)
    {
        bytePositions.push_back(d[i].getByteOffset());
    }

    return true;
}

bool Point::getValuesById(FieldId id, VariantArray &values) const
{
	FieldArray fields;
	bool found = _header->getSchema().getFieldsById(id, fields);
    if (!found)
    {
        return false;
    }

	double scale = 1.0;
	double offset = 0.0;
	bool state = false;
	values.clear();
	for (size_t i = 0; i < fields.size(); i++)
    {
		Variant rawValue, value;
		if (getRawValueFromField(fields[i], rawValue))
		{
			if (fields[i].isScaled() || fields[i].isOffseted())
			{
				if (fields[i].isScaled())
					scale = fields[i].getScale();
				double offset = 0.0;
				if (fields[i].isOffseted())
					offset = fields[i].getOffset();

				if (!getScaledValue(rawValue, fields[i].getDataType(), value, scale, offset))
					return false;
			}
			else
			{
				value = rawValue;
			}
			values.push_back(value);
		}
		else
		{
			return false;
		}
    }

    return true;
}

bool Point::setValuesById(FieldId id, const VariantArray &values)
{
    FieldArray d;
    const Schema &schema = _header->getSchema();
    bool found = schema.getFieldsById(id, d);
    if (!found || values.size() != d.size())
    {
        return false;
    }

    double scale = 1.0;
    double offset = 0.0;
    bool state = false;

    for (size_t i = 0; i < d.size(); i++)
    {
        const Variant &value = values[i];
        Field &field = d[i];
        if (field.isScaled() || field.isOffseted())
        {
            if (field.isScaled())
                scale = field.getScale();
            double offset = 0.0;
            if (field.isOffseted())
                offset = field.getOffset();

            Variant rawValue;
            if(getScaledValue(value, field.getDataType(), rawValue, scale, offset))
                state = setRawValueToField(d[i], rawValue);
            else
                state = false;
        }else
        {
            Variant rawValue = value;
            state = setRawValueToField(d[i], rawValue);
        }
    }

    return state;
}

bool Point::getValue(std::size_t index, Variant &value) const
{
    Field d;
    bool found = _header->getSchema().getField(index, d);
    if (!found)
    {
        return false;
    }

    getRawValueFromField(d, value);
    return true;
}

bool Point::setValue(size_t index, Variant &value)
{
    Field field;
    bool state = false;
    bool found = _header->getSchema().getField(index, field);
    if (!found)
    {
        return false;
    }

    double scale = 1.0;
    double offset = 0.0;
    if (field.isScaled() || field.isOffseted())
    {
        if (field.isScaled())
            scale = field.getScale();
        double offset = 0.0;
        if (field.isOffseted())
            offset = field.getOffset();
        Variant rawValue;
        if (getScaledValue(value, field.getDataType(), rawValue, scale, offset))
            state = setRawValueToField(field, rawValue);
        else
            state = false;
    }
    else
        state = setRawValueToField(field, value);

    return state;
}

bool Point::getRawValueFromField(const Field &field, Variant &value) const
{
    size_t offset;
	switch (field.getDataType())
	{
	case DT_BIT:
		{
			offset = field.getByteOffset();
            size_t size = field.getByteSize();
            size_t bitOffset = field.getBitOffset();
            size_t sizeInBits = field.getBitSize();
            std::string bitsetValue;
            bool firstByte = true;
            size_t startBitPosition, stopBitPosition;
            boost::dynamic_bitset<> bitset(sizeInBits);
            size_t currentBit = 0;
            for (size_t i = 0; i < size; i++)
            {
                if (firstByte)
                {
					startBitPosition = bitOffset - sizeInBits % 8;
					if ((sizeInBits + startBitPosition) <= 8)
						stopBitPosition = sizeInBits + startBitPosition - 1;
					else
						stopBitPosition = 7;
                    firstByte = false;
                }else if (i == field.getByteSize() - 1) // last byte
                {
                    startBitPosition = 0;
                    stopBitPosition = bitOffset;
                }else
                {
                    startBitPosition = 0;
                    stopBitPosition = 7;
                }

                uint8_t value = _data[offset + i];
                for (size_t j = startBitPosition; j <= stopBitPosition; j++)
                {
                    bitset[currentBit] = (value >> j) & 0x01;
                    currentBit++;
                }
            }

            value = bitset;
		}
		break;
	case DT_CHAR:
		{
			offset = field.getByteOffset();
            //size_t size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int8_t* p_data = reinterpret_cast<int8_t*>(data);
            value = std::string((char*)p_data);
		}
		break;
	case DT_UCHAR:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            value = *data;
		}
		break;
	case DT_SHORT:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int16_t* p_data = reinterpret_cast<int16_t*>(data);
            value = *p_data;
		}
		break;
	case DT_USHORT:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint16_t* p_data = reinterpret_cast<uint16_t*>(data);
            value = *p_data;
		}
		break;
	case DT_LONG:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int32_t* p_data = reinterpret_cast<int32_t*>(data);
            value = *p_data;
		}
		break;
	case DT_ULONG:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint32_t* p_data = reinterpret_cast<uint32_t*>(data);
            value = *p_data;
		}
		break;
	case DT_LONGLONG:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int64_t* p_data = reinterpret_cast<int64_t*>(data);
            value = *p_data;
		}
		break;
	case DT_ULONGLONG:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint64_t* p_data = reinterpret_cast<uint64_t*>(data);
            value = *p_data;
		}
		break;
	case DT_FLOAT:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            float* p_data = reinterpret_cast<float*>(data);
            value = *p_data;
		}
		break;
	case DT_DOUBLE:
		{
			offset = field.getByteOffset();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            double* p_data = reinterpret_cast<double*>(data);
            value = *p_data;
		}
		break;
	default:
		return false;
		break;
	}
	return true;
}

bool Point::setRawValueToField(const Field &field, const Variant &value)
{
    size_t offset, size;
    bool state = false;
	switch (field.getDataType())
	{
	case DT_BIT:
		{
            boost::dynamic_bitset<> bitset(field.getBitSize());
            if (!value.getValue(bitset))
                state = false;
			offset = field.getByteOffset();
            size = field.getByteSize();
            size_t bitOffset = field.getBitOffset();
            size_t sizeInBits = field.getBitSize();
            if (sizeInBits < bitset.size())
                state = false;

            bool firstByte = true;
            size_t startBitPosition, stopBitPosition;
            size_t currentBit = 0;
            for (size_t i = 0; i < size; i++)
            {
                if (firstByte)
                {
					startBitPosition = bitOffset - sizeInBits % 8;
					if ((sizeInBits + startBitPosition) <= 8)
						stopBitPosition = sizeInBits + startBitPosition - 1;
					else
						stopBitPosition = 7;
                    firstByte = false;
                }else if (i == size - 1) // last byte
                {
                    startBitPosition = 0;
                    stopBitPosition = bitOffset;
                }else
                {
                    startBitPosition = 0;
                    stopBitPosition = 7;
                }

                uint8_t v = _data[offset + i];

                // Store value in bits 7
                uint8_t mask = 0;
                uint8_t bv = 0;
                for (size_t j = startBitPosition; j <= stopBitPosition; j++)
                {
                    mask |= 0x1 << j;
					bv |= bitset[currentBit] << j;
                    currentBit++;
                }
                v &= ~mask;
                v |= mask & bv;
				_data[offset + i] = v;
            }

            state = true;
		}
		break;
	case DT_CHAR:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t *>(&_data[0] + offset);
            char* p_data = reinterpret_cast<char *>(data);
            size_t s;
            if(value.getByteSize(s))
            {
                if (s == size && value.getValue(p_data))
                    state = true;
            }
		}
		break;
	case DT_UCHAR:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint8_t v;
            if (value.getValue(v))
            {
                *data = v;
                state = true;
            }
		}
		break;
	case DT_SHORT:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int16_t* p_data = reinterpret_cast<int16_t*>(data);
            int16_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_USHORT:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint16_t* p_data = reinterpret_cast<uint16_t*>(data);
            uint16_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_LONG:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int32_t* p_data = reinterpret_cast<int32_t*>(data);
            int32_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_ULONG:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint32_t* p_data = reinterpret_cast<uint32_t*>(data);
            uint32_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_LONGLONG:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            int64_t* p_data = reinterpret_cast<int64_t*>(data);
            int64_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_ULONGLONG:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            uint64_t* p_data = reinterpret_cast<uint64_t*>(data);
            uint64_t v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_FLOAT:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            float* p_data = reinterpret_cast<float*>(data);
            float v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	case DT_DOUBLE:
		{
			offset = field.getByteOffset();
            size = field.getByteSize();
            uint8_t* data = const_cast<uint8_t*>(&_data[0] + offset);
            double* p_data = reinterpret_cast<double*>(data);
            double v;
            if (value.getValue(v))
            {
                *p_data = v;
                state = true;
            }
		}
		break;
	default:
		break;
	}

	return state;
}

bool Point::getWaveformDataByteOffset(uint64_t &offset) const
{
    VariantArray values;
    FieldId id = FI_ByteOffsetToWaveformData;
    bool hasId = getValuesById(id, values);
	if (!hasId)
		return false;

	bool state = values[0].getValue(offset);

    return state;
}

void Point::setWaveformDataAddress(uint64_t offset, uint32_t size)
{
    setWaveformDataByteOffset(offset);
    setWaveformDataSize(size);
}

void Point::setWaveformDataByteOffset(uint64_t offset)
{
    FieldBytePositionArray fa;
    size_t pos = 0;
    if (getFieldBytePositionsById(FI_ByteOffsetToWaveformData, fa))
    {
        pos = fa[0];
        hsl::detail::intToBits<uint64_t>(offset, _data, pos);
    }
}

bool Point::getWaveformDataSize(uint32_t &size) const
{
    VariantArray values;
    FieldId id = FI_WaveformDataSize;
    bool hasId = getValuesById(id, values);
	if (!hasId)
		return false;

    bool state = values[0].getValue(size);
    return state;
}

void Point::setWaveformDataSize(uint32_t size)
{
    FieldBytePositionArray fa;
    size_t pos = 0;
    if (getFieldBytePositionsById(FI_WaveformDataSize, fa))
    {
        pos = fa[0];
        hsl::detail::intToBits<uint32_t>(size, _data, pos);
    }
}

uint16_t Point::getWaveformBandCount() const
{
    std::vector<uint8_t>::size_type pos = 0;

#ifdef LIBHSL_ENDIAN_AWARE
    uint16_t output = hsl::detail::bitsToInt<uint16_t>(output, _waveformData, pos);
    return output;
#else
    uint8_t* data = const_cast<uint8_t*>(&_waveformData[0] + pos);
    uint16_t* output = reinterpret_cast<uint16_t*>(data);
    return *output;
#endif
}

bool Point::getWaveformDescriptorIndexByBand(uint16_t band, uint16_t &index) const
{
    bool found = false;
    uint16_t count = getWaveformBandCount();

	std::vector<uint8_t>::size_type pos = sizeof(uint16_t);
	for (uint16_t i = 0; i < count; i++)
    {
        uint8_t* data = const_cast<uint8_t*>(&_waveformData[0] + pos);
        WaveformPacketDataDefinition* def = reinterpret_cast<WaveformPacketDataDefinition*>(data);
        if (def->bandIndex == band)
        {
            index = def->descriptorIndex;
            found = true;
            break;
        }
        pos += sizeof(WaveformPacketDataDefinition);
    }

    return found;
}

bool Point::getWaveformPacketDefinition(uint16_t band, WaveformPacketDataDefinition &d) const
{
    bool found = false;
    uint16_t count = getWaveformBandCount();

	std::vector<uint8_t>::size_type pos = sizeof(uint16_t);
    for (auto i = 0; i < count; i++)
    {
        uint8_t* data = const_cast<uint8_t*>(&_waveformData[0] + pos);
        WaveformPacketDataDefinition* def = reinterpret_cast<WaveformPacketDataDefinition*>(data);
        if (def->bandIndex == band)
        {
            d = *def;
            found = true;
            break;
        }
        pos += sizeof(WaveformPacketDataDefinition);
    }

    return found;
}

bool Point::getWaveformPacketDefinitions(std::vector<WaveformPacketDataDefinition> &wds) const
{
    bool found = false;
    uint16_t count = getWaveformBandCount();
    if (count > 0)
        found = true;

    std::vector<uint8_t>::size_type pos = sizeof(uint16_t);
	for (uint16_t i = 0; i < count; i++)
    {
        uint8_t* data = const_cast<uint8_t*>(&_waveformData[0] + pos);
        WaveformPacketDataDefinition* def = reinterpret_cast<WaveformPacketDataDefinition*>(data);
        wds.push_back(*def);
        pos += sizeof(WaveformPacketDataDefinition);
    }

    return found;
}

bool Point::getRawWaveformPacketData(uint16_t band, std::vector<uint8_t> &data) const
{
    WaveformPacketDataDefinition wd;

	if (!getWaveformPacketDefinition(band, wd))
		return false;

	uint64_t offset = 0;
	if (!getWaveformDataByteOffset(offset))
		return false;

	offset = offset + wd.byteOffset;
    uint32_t size = wd.size;
    data.resize(size);
    memcpy(&data[0], &_waveformData[0] + offset, size);

    return true;
}

void Point::setWaveformData(const WaveformPacketRecord &record)
{
    record.toWaveformData(_waveformData);
}

bool Point::hasWaveformData() const
{
    return (_waveformData.size() > 0);
}

bool Point::getBandValues(size_t startBandIndex, size_t bandCount, unsigned char *data, size_t size) const
{
    size_t startIndex, stopIndex;
    bool state = false;

    // make sure we have sufficient data
    Schema & schema = const_cast<Header *>(_header)->getSchema();

    if(schema.getNthIndex(FI_BandValue, startBandIndex, startIndex) &&
        schema.getNthIndex(FI_BandValue, startBandIndex + bandCount - 1, stopIndex))
    {
        state = getData(startIndex, stopIndex, data, size);
    }

    return state;
}

bool copyData(const Field &field, const unsigned char *in, unsigned char *out, ScaleOffsetOp scaleInOut)
{
    bool state = false;

    switch (field.getDataType())
    {
    case DT_BIT:
        {
            const uint8_t *p_buf = reinterpret_cast<const uint8_t *>(in);
            uint8_t *p_data = reinterpret_cast<uint8_t *>(out);

            size_t byteSize = field.getByteSize();
            size_t bitOffset = field.getBitOffset();
            size_t sizeInBits = field.getBitSize();

            bool firstByte = true;
            size_t startBitPosition, stopBitPosition;
            size_t currentBit = 0;
            for (size_t i = 0; i < byteSize; i++)
            {
                if (firstByte)
                {
                    startBitPosition = bitOffset % 8;
                    stopBitPosition = 7;
                    firstByte = false;
                }else if (i == byteSize - 1) // last byte
                {
                    startBitPosition = 0;
                    stopBitPosition = (sizeInBits + bitOffset) % 8;
                }else
                {
                    startBitPosition = 0;
                    stopBitPosition = 7;
                }

                uint8_t v = *(p_data + i);

                uint8_t mask = 0;
                uint8_t bv = *(p_buf + i);
                for (size_t j = startBitPosition; j < stopBitPosition; j++)
                {
                    mask |= 0x1 << j;
                    currentBit++;
                }
                v &= ~mask;
                v |= mask & bv;
               *(p_data + i) = v;
            }

            state = true;
        }
        break;
    case DT_UCHAR:
        {
            const uint8_t *p_buf = reinterpret_cast<const uint8_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                uint8_t *p_data = reinterpret_cast<uint8_t *>(out);
                getScaledValue<uint8_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                uint8_t *p_data = reinterpret_cast<uint8_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_SHORT:
        {
            const int16_t *p_buf = reinterpret_cast<const int16_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                int16_t *p_data = reinterpret_cast<int16_t *>(out);
                getScaledValue<int16_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                int16_t *p_data = reinterpret_cast<int16_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_USHORT:
        {
            const uint16_t *p_buf = reinterpret_cast<const uint16_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                uint16_t *p_data = reinterpret_cast<uint16_t *>(out);
                getScaledValue<uint16_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                uint16_t *p_data = reinterpret_cast<uint16_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_LONG:
        {
            const int32_t *p_buf = reinterpret_cast<const int32_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                int32_t *p_data = reinterpret_cast<int32_t *>(out);
                getScaledValue<int32_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                int32_t *p_data = reinterpret_cast<int32_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_ULONG:
        {
            const uint32_t *p_buf = reinterpret_cast<const uint32_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                uint32_t *p_data = reinterpret_cast<uint32_t *>(out);
                getScaledValue<uint32_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                uint32_t *p_data = reinterpret_cast<uint32_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_LONGLONG:
        {
            const int64_t *p_buf = reinterpret_cast<const int64_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                int64_t *p_data = reinterpret_cast<int64_t *>(out);
                getScaledValue<int64_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                int64_t *p_data = reinterpret_cast<int64_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_ULONGLONG:
        {
            const uint64_t *p_buf = reinterpret_cast<const uint64_t *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                uint64_t *p_data = reinterpret_cast<uint64_t *>(out);
                getScaledValue<uint64_t>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                uint64_t *p_data = reinterpret_cast<uint64_t *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_FLOAT:
        {
            const float *p_buf = reinterpret_cast<const float *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                float *p_data = reinterpret_cast<float *>(out);
                getScaledValue<float>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                float *p_data = reinterpret_cast<float *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    case DT_DOUBLE:
        {
            const double *p_buf = reinterpret_cast<const double *>(in);

            // check if we need to do scale and offset data
            double scale = 1.0;
            double offset = 0.0;
            if (field.isScaled() || field.isOffseted())
            {
                // we need to scale data
                if (field.isScaled())
                    scale = field.getScale();
                if (field.isOffseted())
                    offset = field.getOffset();

                double *p_data = reinterpret_cast<double *>(out);
                getScaledValue<double>(*p_buf, *p_data, scale, offset, scaleInOut);
            }
            else
            {
                // just assign data
                double *p_data = reinterpret_cast<double *>(out);
                *p_data = *p_buf;
            }
            state = true;
        }
        break;
    default:
        state = false;
        break;
    }

    return state;
}

bool Point::getData(size_t startIndex, size_t stopIndex, unsigned char *data, size_t size) const
{
    if (startIndex >= _header->getSchema().getFieldCount() || startIndex >= _header->getSchema().getFieldCount())
        return false;

    if (startIndex > stopIndex)
    {
        size_t k = startIndex;
        startIndex = stopIndex;
        stopIndex = k;
    }

    size_t outPos = 0;
    ScaleOffsetOp scaleInOut = SO_Out;
    for (size_t i = startIndex; i <= stopIndex; i++)
    {
        Field field;
        _header->getSchema().getField(i, field);
        size_t inPos = field.getByteOffset();
        if (!copyData(field, _data.data() + inPos, data + outPos / 8, scaleInOut))
            return false;
        outPos += field.getBitSize();
    }

    return true;
}

bool Point::setData(size_t startIndex, size_t stopIndex, const unsigned char *data, size_t size)
{
    if (startIndex >= _header->getSchema().getFieldCount() || startIndex >= _header->getSchema().getFieldCount())
        return false;

    if (startIndex > stopIndex)
    {
        size_t k = startIndex;
        startIndex = stopIndex;
        stopIndex = k;
    }

    ScaleOffsetOp scaleInOut = SO_In;
    size_t inPos = 0;
    for (size_t i = startIndex; i <= stopIndex; i++)
    {
        Field field;
        _header->getSchema().getField(i, field);
        size_t outPos = field.getByteOffset();
        if (!copyData(field, data + inPos / 8, _data.data() + outPos, scaleInOut))
            return false;
        inPos += field.getBitSize();
    }

    return true;
}

bool Point::setBandValues(size_t startBandIndex, size_t bandCount, const unsigned char *data, size_t size)
{
    size_t startIndex, stopIndex;
    bool state = false;

    // make sure we have sufficient data
    Schema & schema = const_cast<Header *>(_header)->getSchema();
    schema.calculateSizes();

    if(schema.getNthIndex(FI_BandValue, startBandIndex, startIndex) &&
        schema.getNthIndex(FI_BandValue, startBandIndex + bandCount - 1, stopIndex))
    {
        state = setData(startIndex, stopIndex, data, size);
    }

    return state;
}

}


#ifndef plane_calibration_SRC_PLANE_CALIBRATION_HPP_
#define plane_calibration_SRC_PLANE_CALIBRATION_HPP_

#include <mutex>
#include <vector>
#include <utility>
#include <memory>
#include <Eigen/Dense>

#include "camera_model.hpp"
#include "calibration_parameters.hpp"
#include "deviation_planes.hpp"
#include "planes.hpp"
#include "visualizer_interface.hpp"

namespace plane_calibration
{

class PlaneCalibration
{
public:
  PlaneCalibration(const CameraModel& camera_model, const CalibrationParametersPtr& parameters,
                   const VisualizerInterfacePtr& depth_visualizer);

  virtual ~PlaneCalibration()
  {
  }

  std::pair<double, double> calibrate(const Eigen::MatrixXf& filtered_depth_matrix, const int& iterations = 3);

protected:
  std::pair<double, double> estimateAngles(const Eigen::MatrixXf& filtered_depth_matrix,
                                           const std::pair<double, double>& last_estimation, const double& deviation);
  std::pair<double, double> estimateAngles(const Eigen::MatrixXf& filtered_depth_matrix, const double& x_angle_offset,
                                           const double& y_angle_offset, const double& deviation);

  mutable std::mutex mutex_;
  CameraModel camera_model_;
  CalibrationParametersPtr parameters_;

  PlaneToDepthImage plane_to_depth_;
  DeviationPlanesPtr max_deviation_planes_;

  PlanesPtr precomputed_planes_;

  std::vector<DeviationPlanesPtr> deviation_planes_;

  CalibrationParameters::Parameters temp_parameters_;
  DeviationPlanesPtr temp_deviation_planes_;
  Eigen::MatrixXf temp_estimated_plane_;

  VisualizerInterfacePtr depth_visualizer_;
};
typedef std::shared_ptr<PlaneCalibration> PlaneCalibrationPtr;

} /* end namespace */

#endif


/**
 * Demo program for simulation library
 * A virtual camera generates simulated point clouds
 * No visual output, point clouds saved to file
 *
 * three different demo modes:
 * 0 - static camera, 100 poses
 * 1 - circular camera flying around the scene, 16 poses
 * 2 - camera translates between 2 poses using slerp, 20 poses
 * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply
 */

#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 <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#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 "pcl/simulation/camera.h"
#include "pcl/simulation/model.h"
#include "pcl/simulation/scene.h"
#include "pcl/simulation/range_likelihood.h"

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

// RangeImage:
#include <pcl/range_image/range_image_planar.h>

// Pop-up viewer
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

// Writing PNG files:
#include <opencv2/opencv.hpp>

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_;

int window_width_;
int window_height_;
bool paused_;
bool write_file_;

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

void write_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 y = 0; y <  480; ++y)
  {
    for (int x = 0; x < 640; ++x)
    {
      int i = y*640 + x ;
      int i_in= (480-1 -y) *640 + x ; // flip up

      float d = (score_buffer[i_in]-min_score)/(max_score-min_score);
      score_img[3*i+0] = 0;
      score_img[3*i+1] = d*255;
      score_img[3*i+2] = 0;
    }
  }

  // Write to file:
  IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
  cv::Mat cv_mat(cv_ipl);
  cv_mat.data = score_img;
  cv::imwrite("score_image.png", cv_mat);

  delete [] score_img;
}

void write_depth_image(const float* depth_buffer)
{
  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
  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 y = 0; y <  480; ++y)
  {
    for (int x = 0; x < 640; ++x)
    {
      int i= y*640 + x ;
      int i_in= (480-1 -y) *640 + x ; // flip up down

      float zn = 0.7;
      float zf = 20.0;
      float d = depth_buffer[i_in];
      float z = -zf*zn/((zf-zn)*(d - zf/(zf-zn)));
      float b = 0.075;
      float f = 580.0;
      uint16_t kd = static_cast<uint16_t>(1090 - b*f/z*8);
      if (kd < 0) kd = 0;
      else if (kd>2047) kd = 2047;

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

  // Write to file:
  IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
  cv::Mat cv_mat(cv_ipl);
  cv_mat.data = depth_img;
  cv::imwrite("depth_image.png", cv_mat);

  delete [] depth_img;
}

void write_rgb_image(const uint8_t* rgb_buffer)
{
  int npixels = range_likelihood_->getWidth() * range_likelihood_->getHeight();
  uint8_t* rgb_img = new uint8_t[npixels * 3];

  for (int y = 0; y <  480; ++y)
  {
    for (int x = 0; x < 640; ++x)
    {
      int px= y*640 + x ;
      int px_in= (480-1 -y) *640 + x ; // flip up down
      rgb_img [3* (px) +0] = rgb_buffer[3*px_in+0];
      rgb_img [3* (px) +1] = rgb_buffer[3*px_in+1];
      rgb_img [3* (px) +2] = rgb_buffer[3*px_in+2];
    }
  }

  // Write to file:
  IplImage *cv_ipl = cvCreateImage( cvSize(640 ,480), 8, 3);
  cv::Mat cv_mat(cv_ipl);
  cv_mat.data = rgb_img ;
  cv::imwrite("rgb_image.png", cv_mat);

  delete [] rgb_img;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
  // Snippet taken from PCLVisualizer tutorial:
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();

  //viewer->addModelFromPLYFile("/home/mfallon/projects/kmcl/kmcl/models/table_scene/meta_model.ply");

  return (viewer);
}

void capture (Eigen::Isometry3d pose_in, string point_cloud_fname)
{
  // No reference image - but this is kept for compatability with range_test_v2:
  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_->getWidth() + j];
    }
  }

  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  std::vector<float> scores;
  int n = 1;
  poses.push_back (pose_in);
  range_likelihood_->computeLikelihoods (reference, poses, scores);
  std::cout << "camera: " << camera_->getX ()
       << " " << camera_->getY ()
       << " " << camera_->getZ ()
       << " " << camera_->getRoll ()
       << " " << camera_->getPitch ()
       << " " << camera_->getYaw ()
       << std::endl;

  delete [] reference;

  // Benchmark Values for sim_terminal_demo ( march 27 2012): >>> used PolygonMeshModel<<
  // 27840 triangle faces
  // 13670 vertices

  // 45.00Hz: simuation only
  //  1.28Hz: simuation, addNoise?    , getPointCloud, writeASCII
  // 33.33Hz: simuation, getPointCloud
  // 23.81Hz: simuation, getPointCloud, writeBinary
  // 14.28Hz: simuation, addNoise, getPointCloud, writeBinary
  // MODULE        TIME      FRACTION
  // simulation     0.02222   31%
  // addNoise      0.03	     41%
  // getPointCloud 0.008     11%
  // writeBinary   0.012     16%
  // total	   0.07222

  // Update: march 29: >>> using TriangleMeshModel now <<<
  // 57.00Hz: simulation only
  // 30.61Hz: simulation, getPointCloud
  // 40.00Hz: simulation, getPointCloud, writeBinary (on average
  // 28.50Hz: simulation, addNoise, getPointCloud, writeBinary

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_out (new pcl::PointCloud<pcl::PointXYZRGB>);
  bool write_cloud=true;
  bool demo_other_stuff=false;

  if (write_cloud)
  {
    // Read Color Buffer from the GPU before creating PointCloud:
    // By default the buffers are not read back from the GPU
    range_likelihood_->getColorBuffer ();
    range_likelihood_->getDepthBuffer ();

    // Add noise directly to the CPU depth buffer
    range_likelihood_->addNoise ();

    // Optional argument to save point cloud in global frame:
    // Save camera relative:
    //range_likelihood_->getPointCloud(pc_out);
    // Save in global frame - applying the camera frame:
    //range_likelihood_->getPointCloud(pc_out,true,camera_->pose());
    // Save in local frame
    range_likelihood_->getPointCloud (pc_out,false,camera_->getPose ());
    // TODO: what to do when there are more than one simulated view?
    std::cout << pc_out->points.size() << " points written to file\n";

    pcl::PCDWriter writer;
    //writer.write (point_cloud_fname, *pc_out,	false);  /// ASCII
    writer.writeBinary (point_cloud_fname, *pc_out);
    //cout << "finished writing file\n";
  }
  if (demo_other_stuff && write_cloud)
  {
    write_score_image (range_likelihood_->getScoreBuffer ());
    write_rgb_image (range_likelihood_->getColorBuffer ());
    write_depth_image (range_likelihood_->getDepthBuffer ());

    // Demo interacton with RangeImage:
    pcl::RangeImagePlanar rangeImage;
    range_likelihood_->getRangeImagePlanar (rangeImage);

    // display viewer: (currently seqfaults on exit of viewer)
    if (1==0){
      boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
      viewer = simpleVis(pc_out);

      while (!viewer->wasStopped ())
      {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
      }
    }
  }
}

// Read in a 3D model
void load_PolygonMesh_model (char* polygon_file)
{
  pcl::PolygonMesh mesh;	// (new pcl::PolygonMesh);
  //pcl::io::loadPolygonFile("/home/mfallon/data/models/dalet/Darlek_modified_works.obj",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);
  std::cout << "OpenGL Version: " << version << std::endl;

  // works well for MIT CSAIL model 3rd floor:
  //camera_->set(4.04454, 44.9377, 1.1, 0.0, 0.0, -2.00352);

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

  // works for small files:
  //camera_->set(-5.0, 0.0, 1.0, 0.0, 0.0, 0.0);
  camera_->set(0.471703, 1.59862, 3.10937, 0, 0.418879, -12.2129);
  camera_->setPitch(0.418879); // not sure why this is here:

  cout << "About to read: " << argv[2] << endl;
  load_PolygonMesh_model (argv[2]);
}

// A 'halo' camera - a circular ring of poses all pointing at a center point
// @param: focus_center: the center points
// @param: halo_r: radius of the ring
// @param: halo_dz: elevation of the camera above/below focus_center's z value
// @param: n_poses: number of generated poses
void
generate_halo(
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > &poses,
  Eigen::Vector3d focus_center,double halo_r,double halo_dz,int n_poses){

//  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  for (double t=0;t < (2*M_PI);t =t + (2*M_PI)/ ((double) n_poses) ){
    double x = halo_r*cos(t);
    double y = halo_r*sin(t);
    double z = halo_dz;
    double pitch =atan2( halo_dz,halo_r);
    double yaw = atan2(-y,-x);

    Eigen::Isometry3d pose;
    pose.setIdentity();
    Eigen::Matrix3d m;
    m = AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * AngleAxisd(0, Eigen::Vector3d::UnitZ());

    pose *=m;
    Vector3d v(x,y,z);
    v += focus_center;
    pose.translation() = v;
    poses.push_back(pose);
  }
  return ;//poses;
}

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

  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);

  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }
  int mode=atoi(argv[1]);

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

  glutInit (&argc, argv);
  glutInitDisplayMode (GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);// was GLUT_RGBA
  glutInitWindowPosition (10, 10);
  glutInitWindowSize (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_ = RangeLikelihood::Ptr (new RangeLikelihood (1, 1, height, width, scene_));
  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
  // range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));

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

  // simulation mode:
  // 0 100 fixed poses
  // 1 a 'halo' camera
  // 2 slerp between two different poses
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  if (mode==0)
  {
    // Create a pose:
    Eigen::Isometry3d pose;
    pose.setIdentity();
    Matrix3d m;
    //ypr:
    m = AngleAxisd(-9.14989, Vector3d::UnitZ())     * AngleAxisd(0.20944, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());
    pose *= m;
    Vector3d v;
    v << 1.31762, 0.382931, 1.89533;
    pose.translation() = v;
    for (int i=0;i< 100;i++)
    { // duplicate the pose 100 times
      poses.push_back(pose);
    }
  }
  else if(mode==1)
  {
    Eigen::Vector3d focus_center(0,0,1.3);
    double halo_r = 4;
    double halo_dz = 2;
    int n_poses=16;
    generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
  }
  else if (mode==2)
  {
    Eigen::Isometry3d pose1;
    pose1.setIdentity();
    pose1.translation() << 1,0.75,2;
    Eigen::Matrix3d rot1;
    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()) * AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY()) * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose1.rotate(rot1);
    Eigen::Isometry3d pose2;
    pose2.setIdentity();
    pose2.translation() << 1,-1,3;
    Eigen::Matrix3d rot2;
    rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ()) * AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()) * AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose2.rotate(rot2);

    int n_poses = 20;
    for (double i=0; i<=1;i+= 1/((double) n_poses -1) )
    {
      Eigen::Quaterniond rot3;
      Eigen::Quaterniond r1(pose1.rotation());
      Eigen::Quaterniond r2(pose2.rotation());
      rot3 = r1.slerp(i,r2);
      Eigen::Isometry3d pose;
      pose.setIdentity();
      Eigen::Vector3d trans3 = (1-i)*pose1.translation() +  i*pose2.translation();
      pose.translation() <<  trans3[0] ,trans3[1] ,trans3[2];
      pose.rotate(rot3);
      poses.push_back(pose);
    }
  }

  // Do the simulation:
  double tic_main = getTime();
  for (size_t i=0;i < poses.size();i++)
  {
    std::stringstream ss;
    ss.precision(20);
    ss << "simulated_pcl_" << i << ".pcd";
    double tic = getTime();
    capture(poses[i],ss.str());
    cout << (getTime() -tic) << " sec\n";
  }
  cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
  cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";

  return 0;
}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BA_TO_MAT_HPP__
#define __OPTIMISATION2_BA_TO_MAT_HPP__

#include <libv/lma/global.hpp>
#include <libv/lma/lm/ba/container.hpp>
#include <libv/lma/lm/ba/mat.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/find.hpp>
#include "utils.hpp"
#include <Eigen/Sparse>
#include "../container/container.hpp"
/**
 * Convertir un tuple en matrice dense
 */

namespace lma {
  template<class Tag, class Mat, class Keys, class Tuple, class Dim> struct ConvertToMat
  {
    Mat& mat;
    const Tuple& tuple;
    const Dim& dim;

	template<class T, int I, int J>
    void set_(Mat& m, int x, int y, const Eigen::Matrix<T,I,J>& block)
    {
      for(int i = 0 ; i < I ; ++i)
        for(int j = 0 ; j < J ; ++j)
          m(x+i,y+j) = block(i,j);
//       Blocker<Tag,I,J>::view(m,x,y) = block;
    }

#ifdef USE_TOON
    template<int I, int J, class T>
    void set_(Mat& m, int x, int y, const TooN::Matrix<I,J,T>& block)
    {
      for(int i = 0 ; i < I ; ++i)
        for(int j = 0 ; j < J ; ++j)
          m(x+i,y+j) = block(i,j);
    }
    template<int I, class T>
    void set_(Mat& m, int x, int y, const TooN::Vector<I,T>& block)
    {
      for(int i = 0 ; i < I ; ++i)
//         for(int j = 0 ; j < J ; ++j)
          m(x+i,y) = block[i];
    }
#endif

    template<class T, int I, int J>
    void set_(Eigen::SparseMatrix<T>& m, int x, int y, const Eigen::Matrix<T,I,J>& block)
    {
      for(size_t i = 0 ; i < I ; ++i)
        for(size_t j = 0 ; j < J ; ++j)
          m.insert(x+i,y+j) = block(i,j);
    }

    ConvertToMat(Mat& mat_, const Tuple& tuple_, const Dim& dim_):mat(mat_),tuple(tuple_),dim(dim_) {}

    template<class A, class B> struct Pos : mpl::find<A,B>::type::pos {};

    template<class KeyA, class KeyB,class Value> void operator()(ttt::wrap<bf::pair<bf::pair<KeyA,KeyB>,Value>>)
    {
      add_to_mat<
                  Pos<Keys,KeyA>::value,
                  Pos<Keys,KeyB>::value
                >(bf::at_key< bf::pair<KeyA,KeyB> > (tuple));
    }

    template<int I, int J, class Key1, class Key2, class T, class _> void add_to_mat(const Table<Key1,Key2,T,_>& table)
    {
      for(auto i = table.first() ; i < table.size() ; ++i)
        for(auto j_ = table.first(i) ; j_ < table.size(i) ; ++j_)
          set_(mat,bf::at_c<I>(dim)+i()*table.I,bf::at_c<J>(dim)+table.indice(i,j_)()*table.J,table(i,j_));
    }

    template<int I, int J, class Key, class T> void add_to_mat(const Table<Key,Key,T,Diagonal>& table)
    {
      for(auto i = table.first() ; i < table.size() ; ++i)
      {
        const int x_ = bf::at_c<I>(dim)+i()*table.I;
        set_(mat,x_,x_,table(i));
      }
    }

    template<int I, int J, class Key, class T> void add_to_mat(const Table<Key,Key,T,Symetric>& table)
    {
      for(auto i = table.first() ; i < table.size() ; ++i)
        for(auto j_ = table.first(i) ; j_ < table.size(i) ; ++j_)
        {
          if (i<=table.indice(i,j_))
            set_(mat,bf::at_c<I>(dim)+i()*table.I,bf::at_c<J>(dim)+table.indice(i,j_)()*table.J,table(i,j_));
        }
    }

  };

  template<class Tag,class Keys, class TupleKeys, class SizeTuple, class Tuple> typename ContainerOption<Tag,0,0>::MatrixDD to_mat(const Tuple& tuple, SizeTuple size_tuple)
  {
    typedef typename ContainerOption<Tag,0,0>::MatrixDD MatD;
    MatD mat(bf::back(size_tuple),bf::back(size_tuple));
    set_zero(mat);
    mpl::for_each<TupleKeys,ttt::wrap_>(ConvertToMat<Tag,MatD,Keys,Tuple,SizeTuple>(mat,tuple,size_tuple));
    return mat;
  }

  template<class Tag, class Keys, class TupleKeys, class SizeTuple, class Tuple> Eigen::SparseMatrix<double> to_sparse(const Tuple& tuple, SizeTuple size_tuple)
  {
    Eigen::SparseMatrix<double> mat(bf::back(size_tuple),bf::back(size_tuple));
    mat.reserve(Eigen::VectorXd::Constant(bf::back(size_tuple),bf::back(size_tuple)));
    mpl::for_each<TupleKeys,ttt::wrap_>(ConvertToMat<Tag,Eigen::SparseMatrix<double>,Keys,Tuple,SizeTuple>(mat,tuple,size_tuple));
    mat.makeCompressed();
    return mat;
  }

  // vector toujours dense
  template<class Tag, class VecD, class Tuple> struct AddVec
  {
    VecD& mat;
    const Tuple& tuple;
    int i;
    AddVec(const Tuple& tuple_, VecD& mat_):mat(mat_),tuple(tuple_),i(0){}

    template<class Key, class Value> void operator()(ttt::wrap<bf::pair<Key,Value>>)
    {
      for(auto& x : bf::at_key<Key>(tuple))
      {
        for(size_t k = 0 ; k < Value::I ; ++k)
          mat[i+k] = x[k];
//         Blocker<Tag,Value::I,1>::view(mat,i,0) = x;
        i += Value::I;
      }
    }
  };

  template<class Tag, class Tuple> typename ContainerOption<Tag,0,0>::MatrixD1 to_matv(const Tuple& tuple)
  {
    typename ContainerOption<Tag,0,0>::MatrixD1 mat(nb_element(tuple));
    mpl::for_each<Tuple,wrap_>(AddVec<Tag,typename ContainerOption<Tag,0,0>::MatrixD1,Tuple>(tuple,mat));
    return mat;
  }
}// eon

#endif


#include "TimeCorr.hxx"
#include <boost/format.hpp>
#include <iostream>

int main(int argc, char * argv[]) {
  int i, j;

  if(argc < 3) {
    std::cout << "tctest <fromgrid> <togrid>\n";
    exit(-1);
  }

  float flto = TimeCorr::localTimeOffset(argv[1]);
  float tlto = TimeCorr::localTimeOffset(argv[2]);
  float ftlto = TimeCorr::localTimeOffset(argv[1], argv[2]);
  float tflto = TimeCorr::localTimeOffset(argv[2], argv[1]);
  std::cout << boost::format("from: %s to: %s flto = %f  tlto = %f ftlto = %f tflto = %f\n")
    % argv[1] % argv[2] % flto % tlto % ftlto % tflto;

  long epoch_time = 1522549800L;

  for(long i = 0; i < 24; i++) {
    long et = i * 3600 + epoch_time;
    float fto = TimeCorr::localTime(argv[1], et);
    float tto = TimeCorr::localTime(argv[2],  et);
    float mto = TimeCorr::localTime(argv[1], argv[2],  et);

    struct tm ts, * tsp;
    tsp = gmtime_r(&et, &ts);
    if(tsp == NULL) {
      std::cout << "bad gmtime_r call\n";
    }
    float utchr = (float) ts.tm_hour + ((float) ts.tm_min) / 60.0;
    std::cout << boost::format("fto hrs %5.2f tto hrs %5.2f mto hrs %5.2f utchr %5.2f\n")
      % fto % tto % mto % utchr;
  }
}


#include "example.h"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(add_2_and_2)
{
    BOOST_REQUIRE_EQUAL(4, example::add(2, 2));
}

BOOST_AUTO_TEST_CASE( add_12_and_24 )
{
    BOOST_REQUIRE_EQUAL( 36, example::add( 12, 24 ) );
}

BOOST_AUTO_TEST_CASE( add_100_and_1 )
{
    BOOST_REQUIRE_EQUAL( 101, example::add( 100, 1 ) );
}

BOOST_AUTO_TEST_CASE( add_2_and_minus_2 )
{
    BOOST_REQUIRE_EQUAL( 0, example::add( 2, -2 ) );
}