/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2014-, Open Perception, Inc.
 *
 *  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 copyright holder(s) 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.
 *
 */

#ifndef PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP
#define PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP

#include <boost/unordered_map.hpp>

#include <pcl/common/io.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/octree/octree_impl.h>

#include "graph/voxel_grid_graph_builder.h"

/* The function below is required in order to use boost::unordered_map with
 * pcl::octree::OctreeKey key type. It simply hashes the x, y, z array of
 * indices, because it uniquely defines the key. */

namespace pcl
{

  namespace octree
  {

    static size_t hash_value (const OctreeKey& b)
    {
      return boost::hash_value (b.key_);
    }

  }

}

template <typename PointT, typename GraphT> void
pcl::graph::VoxelGridGraphBuilder<PointT, GraphT>::compute (GraphT& graph)
{
  if (!initCompute ())
  {
    graph = GraphT ();
    deinitCompute ();
    return;
  }

  typename pcl::PointCloud<PointT>::Ptr transformed (new pcl::PointCloud<PointT>);
  pcl::copyPointCloud (*input_, *transformed);
  for (size_t i = 0; i < transformed->size (); ++i)
  {
    PointT& p = transformed->points[i];
    p.x /= p.z;
    p.y /= p.z;
    p.z = std::log (p.z);
  }

  Eigen::Vector4f min, max;
  pcl::getMinMax3D (*transformed, *indices_, min, max);

  // Create and initialize an Octree that stores point indices
  typedef pcl::octree::OctreePointCloud<PointT> Octree;
  Octree octree (voxel_resolution_);
  octree.defineBoundingBox (min (0), min (1), min (2), max (0), max (1), max (2));
  octree.setInputCloud (transformed, indices_);
  octree.addPointsFromInputCloud ();

  graph = GraphT (octree.getLeafCount ());

  typedef boost::unordered_map<pcl::octree::OctreeKey, VertexId> KeyVertexMap;
  KeyVertexMap key_to_vertex_map;

  point_to_vertex_map_.clear ();
  point_to_vertex_map_.resize (transformed->size (), std::numeric_limits<VertexId>::max ());

  typename Octree::LeafNodeIterator leaf_itr = octree.leaf_begin ();
  for (VertexId v = 0; leaf_itr != octree.leaf_end (); ++leaf_itr, ++v)
  {
    // Step 1: compute leaf centroid and fill in corresponding elements of the
    // point to vertex map.
    pcl::CentroidPoint<PointInT> centroid;
    std::vector<int>& indices = leaf_itr.getLeafContainer ().getPointIndicesVector ();
    for (size_t i = 0; i < indices.size (); ++i)
    {
      centroid.add (input_->operator[] (indices[i]));
      point_to_vertex_map_[indices[i]] = v;
    }
    centroid.get (graph[v]);

    // Step 2: fill in octree key to vertex map.
    octree::OctreeKey key = leaf_itr.getCurrentOctreeKey ();
    key_to_vertex_map[key] = v;

    // Step 2: find neighbors and insert edges.
    octree::OctreeKey neighbor_key;
    for (int dx = (key.x > 0) ? -1 : 0; dx <= 1; ++dx)
    {
      neighbor_key.x = static_cast<uint32_t> (key.x + dx);
      for (int dy = (key.y > 0) ? -1 : 0; dy <= 1; ++dy)
      {
        neighbor_key.y = static_cast<uint32_t> (key.y + dy);
        for (int dz = (key.z > 0) ? -1 : 0; dz <= 1; ++dz)
        {
          neighbor_key.z = static_cast<uint32_t> (key.z + dz);
          typename KeyVertexMap::iterator f = key_to_vertex_map.find (neighbor_key);
          if (f != key_to_vertex_map.end () && v != f->second)
            boost::add_edge (v, f->second, graph);
        }
      }
    }
  }
}

#endif /* PCL_GRAPH_IMPL_VOXEL_GRID_GRAPH_BUILDER_HPP */


#include "message.h"
#include "proof.h"
#include "rsa.h"
#include "proofrepository.h"
#include "logger.h"
#include <sstream>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>

const unsigned long KEYBITS = 4096;
const char * PREPFILE = "proofrep.xml" ;

using namespace std;

XLogDestination d2("test.log");
XLogger testlog(d2);

int main()
{
   LOG( testlog, "Completed test" ) ;

   PublicKeyEncryption * apub = 0;
   PrivateKeyEncryption * apriv = 0;
   RSACore::createkeypair( apub, apriv, KEYBITS ) ;

   PublicKeyEncryption * bpub = 0;
   PrivateKeyEncryption * bpriv = 0;
   RSACore::createkeypair( bpub, bpriv, KEYBITS ) ;

   Proof p;
   p.challenger = "a" ;
   p.responder = "b" ;
   p.observer = "a" ;

   ProofRepository rep;

   UBigNum test, encrypted, recovered;
   test.random(KEYBITS-8);

   bpub->Encrypt( encrypted, test )  ;
   p.challenge.challenge = encrypted;

   bpriv->Decrypt( recovered, encrypted ) ;
   p.response.response = recovered;

   assert( recovered == test ) ;

   rep.putProof(p);
   rep.save(PREPFILE);
   rep.clear();

   cout << "restoring" << endl ;
   rep.restore(PREPFILE);

   cout << "restored" << endl ;

   LOG( testlog, "Completed test" ) ;
   return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_ISQRT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_ISQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/arithmetic/functions/isqrt.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/toolbox/arithmetic/functions/isqrt.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_greater_equal.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/if_else_allbits.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::isqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<uint8_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      bA0 const na  = is_nez(a0);
      A0 n   = add(shri(a0, 4), Four<A0>());
      A0 n1  = shri(n+a0/n, 1);

      bA0 ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shri(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = seladd( gt(n*n,a0), n, Mone<A0>());

      return seladd(na, Zero<A0>(), n);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::isqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<int_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0,signed>::type     int_type;
      typedef typename dispatch::meta::as_integer<A0,unsigned>::type  uint_type;
      return if_else_zero( is_gtz(a0)
                  , simd::bitwise_cast<int_type>(isqrt( simd::bitwise_cast<uint_type>(a0)))
                  );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::isqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      bA0 const na = is_nez(a0);
      A0 const z1 = add(shri(a0, 6),    boost::simd::integral_constant<A0,16>());
      A0 const z2 = add(shri(a0,10),   boost::simd::integral_constant<A0,256>());
      A0 const z3 = add(shri(a0,13),  boost::simd::integral_constant<A0,2048>());
      A0 const z4 = add(shri(a0,16), boost::simd::integral_constant<A0,16384>());
      static A0 const one = One<A0>();

      A0 n  = if_else( gt(a0, boost::simd::integral_constant<A0,177155824>())
                  , z4
                  , if_else( gt(a0, boost::simd::integral_constant<A0,4084387>())
                        , z3
                        , if_else( gt(a0, boost::simd::integral_constant<A0,31679>())
                                , z2
                                , z1
                                )
                        )
                  );
      bA0 ok =  is_gtz(n);
      n = if_else(ok, n, one);
      A0 n1 = if_else(ok, shri(n+a0/n, 1), one);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shri(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shri(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);

      A0 tmp = sub(n*sub(n, one), one);
      n  = seladd( is_greater_equal(tmp+n,a0), n, Mone<A0>());
      n =  seladd(na, Zero<A0>(), n);

      return n;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::isqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<uint64_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return simd::bitwise_cast<A0>(boost::simd::toint(boost::simd::sqrt(boost::simd::tofloat(a0))));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::isqrt_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<uint16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      bA0 const  na = is_nez(a0);
      A0 const  z1 = add(shri(a0, 6), boost::simd::integral_constant<A0, 16>());
      A0 const  z2 = add(shri(a0,10), boost::simd::integral_constant<A0, 256>());
      A0 const  C1 = boost::simd::integral_constant<A0, 31679>();
      // choose a proper starting point for approximation
      A0 n  = if_else(lt(a0, C1), z1, z2);
      bA0 ok =  is_gtz(n);
      static A0 const one = One<A0>();
      n  = if_else(ok, n, one);

      A0 n1 = if_else(ok, shri(n+a0/n, 1), one);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shri(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shri(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = seladd( gt(n*n,a0), n, Mone<A0>());

      return seladd(na, Zero<A0>(), n);
    }
  };
} } }

#endif
#endif


/**
 *  Author: Cedric LE GENTIL
 *
 *  Copyright 2021 Cedric LE GENTIL
 *
 *  This is a simple example of how to generate the UGPMs
 *
 *  Disclaimer:
 *  This code is not optimised neither for performance neither for maintainability.
 *  There might still be errors in the code, logic or procedure. Every feedback is welcomed.
 *
 *  For any further question or if you see any problem in that code
 *  le.gentil.cedric@gmail.com
 **/

#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include "sensor_input/imu_simulator.h"
#include "imu_preintegration/preintegration.h"
#include "common/random.h"
#include "common/utils.h"

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

    celib::PreintOption preint_opt;
    bool test_jacobians;

    // Program options
    boost::program_options::options_description opt_description("Allowed options");
    opt_description.add_options()
        ("help,h", "Produce help message")
        ("method,m", boost::program_options::value< std::string >(), "LPM | GPM | UGPM(default)")
        ("length,l", boost::program_options::value< double >(), "Length simulated integration (default = 2.0)")
        ("train,t", boost::program_options::bool_switch(&preint_opt.train_gpm), "Flag to activate training")
        ("jacobian,j", boost::program_options::bool_switch(&test_jacobians), "Flag to activate the testing of the Jacobians")
        ("nb_inference,n", boost::program_options::value< int >(), "Nb of inferrences (to simulate lidar nb of inferences), Default = 1")
        ("quantum,q", boost::program_options::value< double >(), "Time quantum for piece-wise integration (useful for long integration intervals, -1 to deactivate (default = 0.2)")
        ;

    boost::program_options::variables_map var_map;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, opt_description), var_map);
    boost::program_options::notify(var_map);

    if(var_map.count("help")) {
        std::cout << opt_description << std::endl;
        return 1;
    }

    preint_opt.type = celib::UGPM;
    if(var_map.count("method"))
    {
        std::string type = var_map["method"].as<std::string>();
        std::transform(type.begin(), type.end(), type.begin(), [](unsigned char c){ return std::tolower(c); });
        if(type == "lpm") preint_opt.type = celib::LPM;
        else if(type == "gpm") preint_opt.type = celib::GPM;
        else if(type == "ugpm") preint_opt.type = celib::UGPM;
        else
        {
            std::cout << "ERROR: The type of preintegration method is unknown, program stopping now" << std::endl;
            return 1;
        }
    }
    std::string to_print;
    switch (preint_opt.type)
    {
    case celib::LPM:
        to_print = "LPM";
        break;

    case celib::GPM:
        to_print = "GPM";
        break;

    case celib::UGPM:
        to_print = "UGPM";
        break;

    default:
        break;
    }
    std::cout << "Preintegration demonstration with " << to_print << std::endl;

    double integration_length = 2;
    if(var_map.count("length"))
    {
        integration_length = var_map["length"].as<double>();
    }

    int nb_infer = 0;
    if(var_map.count("nb_inference"))
    {
        nb_infer = var_map["nb_inference"].as<int>() - 1;
    }

    preint_opt.quantum = 0.2;
    if(var_map.count("quantum"))
    {
        preint_opt.quantum = var_map["quantum"].as<double>();
    }

    // Create an IMU simulator
    celib::ImuSimulatorOption sim_opt;
    sim_opt.acc_std = 0.02;
    sim_opt.gyr_std = 0.002;
    celib::ImuSimulator imu_sim(sim_opt);

    // Create some fake data
    double overlap = 0.15;
    celib::RandomGenerator rand_gen;
    double start_t = rand_gen.randUniform(overlap,sim_opt.dataset_length - integration_length - overlap);
    double end_t = start_t + integration_length;
    celib::ImuData data = imu_sim.get(start_t-overlap, end_t+overlap);

    // Create a preintegration object
    preint_opt.min_freq = 1000;
    celib::PreintPrior prior;
    std::vector<std::vector<double> > t;
    std::vector<double> temp_t;
    temp_t.push_back(end_t);
    t.push_back(temp_t);
    std::vector<double> temp_t_2;
    for(int i = 0; i < nb_infer; ++i)
    {
        double ratio = i/(double)(nb_infer);
        temp_t_2.push_back(start_t*(1-ratio) + end_t*ratio);
    }
    t.push_back(temp_t_2);

    // Test some data
    celib::StopWatch stop_watch;
    stop_watch.start();
    celib::ImuPreintegration preint(data, start_t, t, preint_opt, prior);
    stop_watch.stop();
    stop_watch.print();

    celib::PreintMeas preint_meas = preint.get(0,0);

    std::vector<double> error = imu_sim.testPreint(start_t, end_t, preint.get(0,0));

    std::cout << "Preintegration errors over window of " << preint_meas.dt << " seconds:" << std::endl;
    std::cout << "  Rotation [deg] = " << error[0]*180.0/M_PI << std::endl;
    std::cout << "  Velocity [m/s] = " << error[1] << std::endl;
    std::cout << "  Position [m]   = " << error[2] << std::endl;
    std::cout << std::endl;
    std::cout << "Covariance" << std::endl << preint_meas.cov << std::endl;
    std::cout << std::endl;

    if(test_jacobians)
    {
        double num_quantum = 0.001;

        // Compute numerical preintegrated measurements to compare
        celib::PreintMeas preint_num_jacobian;
        for(int i = 0; i < 3; ++i)
        {
            auto data_temp_bw = data;
            auto data_temp_bf = data;
            for(int j = 0; j < data.gyr.size(); ++j) data_temp_bw.gyr[j].data[i] += num_quantum;
            for(int j = 0; j < data.acc.size(); ++j) data_temp_bf.acc[j].data[i] += num_quantum;

            celib::ImuPreintegration preint_bw(data_temp_bw, start_t, t, preint_opt, prior);
            auto temp_preint = preint_bw.get(0,0);

            preint_num_jacobian.d_delta_R_d_bw.col(i) = celib::LogMap(preint_meas.delta_R.transpose()*temp_preint.delta_R)/num_quantum;
            preint_num_jacobian.d_delta_v_d_bw.col(i) = (temp_preint.delta_v - preint_meas.delta_v)/num_quantum;
            preint_num_jacobian.d_delta_p_d_bw.col(i) = (temp_preint.delta_p - preint_meas.delta_p)/num_quantum;

            celib::ImuPreintegration preint_bf(data_temp_bf, start_t, t, preint_opt, prior);
            temp_preint = preint_bf.get(0,0);

            preint_num_jacobian.d_delta_v_d_bf.col(i) = (temp_preint.delta_v - preint_meas.delta_v)/num_quantum;
            preint_num_jacobian.d_delta_p_d_bf.col(i) = (temp_preint.delta_p - preint_meas.delta_p)/num_quantum;
        }
        auto data_temp_dt = data;
        for(int j = 0; j < data.gyr.size(); ++j) data_temp_dt.gyr[j].t -= num_quantum;
        for(int j = 0; j < data.acc.size(); ++j) data_temp_dt.acc[j].t -= num_quantum;

        celib::ImuPreintegration preint_dt(data_temp_dt, start_t, t, preint_opt, prior);
        auto temp_preint = preint_dt.get(0,0);
        preint_num_jacobian.d_delta_R_d_t = celib::LogMap(preint_meas.delta_R.transpose()*temp_preint.delta_R)/num_quantum;
        preint_num_jacobian.d_delta_v_d_t = (temp_preint.delta_v - preint_meas.delta_v)/num_quantum;
        preint_num_jacobian.d_delta_p_d_t = (temp_preint.delta_p - preint_meas.delta_p)/num_quantum;

        // Print the jacobians 2 by two to "easily" read the difference
        std::cout << std::endl;
        std::cout << "Test Jacobians" << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta R / gyr bias" << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_R_d_bw << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_R_d_bw << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta R / dt " << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_R_d_t.transpose() << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_R_d_t.transpose() << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta v / gyr bias" << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_v_d_bw << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_v_d_bw << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta v / acc bias" << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_v_d_bf << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_v_d_bf << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta v / dt " << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_v_d_t.transpose() << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_v_d_t.transpose() << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta p / gyr bias" << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_p_d_bw << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_p_d_bw << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta p / acc bias" << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_p_d_bf << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_p_d_bf << std::endl;
        std::cout << std::endl;
        std::cout << "Jacobian Delta p / dt " << std::endl;
        std::cout << "Computed" << std::endl;
        std::cout << preint_meas.d_delta_p_d_t.transpose() << std::endl;
        std::cout << "Numerical" << std::endl;
        std::cout << preint_num_jacobian.d_delta_p_d_t.transpose() << std::endl;
        std::cout << std::endl;
    }

    return 0;
}


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

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ITERATORS_POINT_ITERATOR_TYPE_HPP
#define BOOST_GEOMETRY_ITERATORS_POINT_ITERATOR_TYPE_HPP

#include <boost/geometry/iterators/dispatch/point_iterator_type.hpp>
#include <boost/geometry/iterators/dispatch/point_iterator.hpp>

#include <boost/range.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/mpl/if.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/iterators/flatten_iterator.hpp>
#include <boost/geometry/iterators/concatenate_iterator.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail_dispatch
{

template <typename Geometry>
struct point_iterator_value_type
{
    typedef typename boost::mpl::if_c
        <
            !boost::is_const<Geometry>::type::value,
            typename geometry::point_type<Geometry>::type,
            typename geometry::point_type<Geometry>::type const
        >::type type;
};

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct point_iterator_inner_range_type
{
    typedef typename boost::mpl::if_c
        <
            !boost::is_const<Geometry>::type::value,
            typename boost::range_value<Geometry>::type,
            typename boost::range_value<Geometry>::type const
        >::type type;
};

template <typename Polygon>
struct point_iterator_inner_range_type<Polygon, polygon_tag>
{
    typedef typename boost::mpl::if_c
        <
            !boost::is_const<Polygon>::type::value,
            typename geometry::ring_type<Polygon>::type,
            typename geometry::ring_type<Polygon>::type const
        >::type type;
};

} // namespace detail_dispatch
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linestring>
struct point_iterator_type<Linestring, linestring_tag>
{
    typedef typename boost::range_iterator<Linestring>::type type;
};

template <typename Ring>
struct point_iterator_type<Ring, ring_tag>
{
    typedef typename boost::range_iterator<Ring>::type type;
};

template <typename Polygon>
class point_iterator_type<Polygon, polygon_tag>
{
private:
    typedef typename detail_dispatch::point_iterator_inner_range_type
        <
            Polygon
        >::type inner_range;

public:
    typedef concatenate_iterator
        <
            typename boost::range_iterator<inner_range>::type,
            flatten_iterator
                <
                    typename boost::range_iterator
                        <
                            typename geometry::interior_type<Polygon>::type
                        >::type,
                    typename dispatch::point_iterator_type
                        <
                            inner_range
                        >::type,
                    typename detail_dispatch::point_iterator_value_type
                        <
                            Polygon
                        >::type,
                    dispatch::points_begin<inner_range>,
                    dispatch::points_end<inner_range>
                >,
            typename detail_dispatch::point_iterator_value_type<Polygon>::type
        > type;
};

template <typename MultiPoint>
struct point_iterator_type<MultiPoint, multi_point_tag>
{
    typedef typename boost::range_iterator<MultiPoint>::type type;
};

template <typename MultiLinestring>
class point_iterator_type<MultiLinestring, multi_linestring_tag>
{
private:
    typedef typename detail_dispatch::point_iterator_inner_range_type
        <
            MultiLinestring
        >::type inner_range;

public:
    typedef flatten_iterator
        <
            typename boost::range_iterator<MultiLinestring>::type,
            typename dispatch::point_iterator_type<inner_range>::type,
            typename detail_dispatch::point_iterator_value_type
                <
                    MultiLinestring
                >::type,
            dispatch::points_begin<inner_range>,
            dispatch::points_end<inner_range>
        > type;
};

template <typename MultiPolygon>
class point_iterator_type<MultiPolygon, multi_polygon_tag>
{
private:
    typedef typename detail_dispatch::point_iterator_inner_range_type
        <
            MultiPolygon
        >::type inner_range;

public:
    typedef flatten_iterator
        <
            typename boost::range_iterator<MultiPolygon>::type,
            typename dispatch::point_iterator_type<inner_range>::type,
            typename detail_dispatch::point_iterator_value_type
                <
                    MultiPolygon
                >::type,
            dispatch::points_begin<inner_range>,
            dispatch::points_end<inner_range>
        > type;
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ITERATORS_POINT_ITERATOR_TYPE_HPP


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/RootFinders/secantRootFinder.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction1.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction2.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction3.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithLargeRootDifference.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithZeroRoot.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( testsuite_rootfinders )

using namespace tudat;
using namespace root_finders;
using namespace root_finders::termination_conditions;

//! Check if Secant method converges on test function #1 (TestFunction1).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction1 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction1 > testFunction = std::make_shared< TestFunction1 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            std::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on test function #2 (TestFunction2).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction2 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction2 > testFunction = std::make_shared< TestFunction2 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            std::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on test function #3 (TestFunction3).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction3 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction3 > testFunction = std::make_shared< TestFunction3 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            std::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on function with large root difference
//! (testFunctionWithLargeRootDifference).
// Not the best test case. Inheritance from old code.
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunctionWithLargeRootDifference )
{
    // Declare tolerance.
    const double tolerance = 1.0e-10;

    // Declare expected roots.
    const double expectedRootLowCase = 1.00000000793634;
    const double expectedRootHighCase = 7937.3386333591;

    // Create objects containing the test functions. Values were obtained during a limit case
    // gravity assist calculation (while evaluating Cassini-1 trajectory).
    std::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionLowCase =
            std::make_shared< TestFunctionWithLargeRootDifference >
            ( 0, -3.24859999867635e18, -3248600.0, 1.5707963267949 );
    std::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionHighCase =
            std::make_shared< TestFunctionWithLargeRootDifference >
            ( 0, -3248600.0, -3.24859999867635e18, 1.5707963267949 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction
            = std::bind( &RootRelativeToleranceTerminationCondition< >::checkTerminationCondition,
                           std::make_shared< RootRelativeToleranceTerminationCondition< > >(
                               1.0e-10 ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Secant object, per case.
    SecantRootFinder secantLowCase( terminationConditionFunction, 1.0 );
    SecantRootFinder secantHighCase( terminationConditionFunction, 1.0 );

    // Let the Secant method search for the root.
    const double rootLowCase = secantLowCase.execute( testFunctionLowCase, 1.0 + 8.0e-9 );
    const double rootHighCase = secantHighCase.execute( testFunctionHighCase, 10000.0 );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( rootLowCase, expectedRootLowCase, tolerance );
    BOOST_CHECK_CLOSE_FRACTION( rootHighCase, expectedRootHighCase, tolerance );
}

//! Check if Secant method converges on function with zero root (testFunctionWithZeroRoot).
// Not the best test case. Inheritance from old code. Not really relevant anymore. The basic
// idea is that Newton-Raphson should work for both a function that becomes zero, as well as for a
// function that does not become zero. A better case should be written.
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunctionWithZeroRoot )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunctionWithZeroRoot > testFunction =
            std::make_shared< TestFunctionWithZeroRoot >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            std::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    1.0e-150 ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_SMALL( root, 1.0e-100 );
    BOOST_CHECK_SMALL( testFunction->evaluate( root ), 1.0e-200 );
}

BOOST_AUTO_TEST_SUITE_END( ) // testsuite_rootfinders

} // namespace unit_tests
} // namespace tudat


// (C) Copyright Jeremy Siek 2000. Permission to copy, use, modify, sell and
// distribute this software is granted provided this copyright notice appears
// in all copies. This software is provided "as is" without express or implied
// warranty, and with no claim as to its suitability for any purpose.

#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <boost/counting_iterator.hpp>
#include <boost/iterator_adaptors.hpp>

int main(int, char*[])
{
  // Example of using counting_iterator_generator
  std::cout << "counting from 0 to 4:" << std::endl;
  boost::counting_iterator_generator<int>::type first(0), last(4);
  std::copy(first, last, std::ostream_iterator<int>(std::cout, " "));
  std::cout << std::endl;

  // Example of using make_counting_iterator()
  std::cout << "counting from -5 to 4:" << std::endl;
  std::copy(boost::make_counting_iterator(-5),
            boost::make_counting_iterator(5),
            std::ostream_iterator<int>(std::cout, " "));
  std::cout << std::endl;

  // Example of using counting iterator to create an array of pointers.
  const int N = 7;
  std::vector<int> numbers;
  // Fill "numbers" array with [0,N)
  std::copy(boost::make_counting_iterator(0), boost::make_counting_iterator(N),
            std::back_inserter(numbers));

  std::vector<std::vector<int>::iterator> pointers;

  // Use counting iterator to fill in the array of pointers.
  // causes an ICE with MSVC6
#if !defined(BOOST_MSVC) || (BOOST_MSVC > 1200)
  std::copy(boost::make_counting_iterator(numbers.begin()),
            boost::make_counting_iterator(numbers.end()),
            std::back_inserter(pointers));
#endif

#if !defined(BOOST_MSVC) || (BOOST_MSVC > 1300)
  // Use indirect iterator to print out numbers by accessing
  // them through the array of pointers.
  std::cout << "indirectly printing out the numbers from 0 to "
            << N << std::endl;
  std::copy(boost::make_indirect_iterator(pointers.begin()),
            boost::make_indirect_iterator(pointers.end()),
            std::ostream_iterator<int>(std::cout, " "));
  std::cout << std::endl;
#endif
  return 0;
}


#define BOOST_TEST_MODULE pcraster mldd vertex_iterator
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "mldd_vertexiterator.h"
#include "mldd_dagraster.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace mldd;

  geo::RasterDim rd(2,3);
  DagRaster g(rd);
  // initially empty graph; no cells set

  BOOST_CHECK(g.beginVertex() == g.endVertex());
  BOOST_CHECK(g.beginVertex() == rd.convert(6));

  {
    VertexIterator  begin(g,g.beginVertex());
    VertexIterator  end(g,g.endVertex());
    BOOST_CHECK(begin == end);

    int count=0;
    for(VertexIterator i=begin; i != end; ++i)
      count++;
    BOOST_CHECK(!count);
  }

  // add 2 cells
  g.addFlowNB(0,0, 6); // -> add vertices (0,0) and (0,1)
  g.addFlowNB(0,1, 6); // -> add vertices (0,1) and (0,2)
  // can not add pit: g.addFlowNB(0,2, 5);
  {
      VertexIterator  begin(g,g.beginVertex());
      VertexIterator  end(g,g.endVertex());
      BOOST_CHECK(begin != end);

      int count=0;
      VertexIterator i=begin;
      for(  ; i != end; ++i)
        count++;
      BOOST_CHECK(count==3);
      // this iterator will proceed past the end
      ++i;
      BOOST_CHECK(i!=end);
      ++i;
      BOOST_CHECK(i!=end);
  }
}