/*
 * test_visibility.cpp
 *
 * Test the Graph Library for Autonomous and Dynamic Systems
 *
 * author:  Cyril Robin <cyril.robin@laas.fr>
 * created: 2013-09-15
 * license: BSD
 */
#define BOOST_TEST_MODULE const_string test
#include <boost/test/included/unit_test.hpp>

#include <string>
#include <sstream>

#include "gdalwrap/gdal.hpp"
#include "gladys/visibility_map.hpp"

BOOST_AUTO_TEST_SUITE( visibility )

BOOST_AUTO_TEST_CASE( test_visibility_map )
{
    std::string dtm_path = "/tmp/test_visibility.tif";
    std::string vm_path = "/tmp/test_visibility_map.tif";
    std::string robotm_path = "/tmp/robot.json";

    // create a robot model (JSON configuration file)
    std::ofstream robot_cfg(robotm_path);
    robot_cfg
        << "{"
            << "\"robot\":{\"mass\":1.0,\"radius\":1.0,\"velocity\":1.0},"
            << "\"sensor\":{\"range\":20.0,\"fov\":6.28,"
                <<   "\"pose\":{\"x\":0.1,\"y\":0.2,\"z\":0.7,\"t\":0.0}"
            << "}"
        << "}" ;
    robot_cfg.close();

    // create a dtm map (GeoTiff image)
    gdalwrap::gdal dtm;
    dtm.set_size(2, 9, 9);
    // add a small wall in the middle of the map
    dtm.names[0] = "Z_MAX";
    dtm.get_band("Z_MAX").assign(9*9, 0.5);
    for (int i=0 ; i<9 ; i++ )
        dtm.get_band("Z_MAX")[ 5 + i*9 ] = 1.3;
    // two special points to observe + the observer point
    dtm.get_band("Z_MAX")[ 8 + 0*9 ] = 1.9;
    dtm.get_band("Z_MAX")[ 8 + 8*9 ] = 1.1;
    dtm.get_band("Z_MAX")[ 0 + 5*9 ] = 0.6;
    // add a small band of never-observed points
    dtm.names[1] = "N_POINTS";
    dtm.get_band("N_POINTS").assign(9*9, 5.);
    for (int i=0 ; i<9 ; i++ )
        dtm.get_band("N_POINTS")[ 3 + i*9 ] = 0.0;
    // one special point to observe
    dtm.get_band("N_POINTS")[ 8 + 5*9 ] = 0.0;

    dtm.save(dtm_path);

    //// create a visibility map from the dtm
    gladys::visibility_map vm;
    vm.load(dtm_path, robotm_path);
    vm.save(vm_path);

    gladys::point_xy_t s  = {0, 5}; // sensor
    gladys::point_xy_t t1 = {8, 0}; // target 1 -- visible
    gladys::point_xy_t t2 = {8, 8}; // target 2 -- invisible
    gladys::point_xy_t t3 = {8, 5}; // target 3 -- invisible

    bool b ;
    b = vm.is_visible( s, t1) ;
    BOOST_TEST_MESSAGE( "t1 visible from s ? " + b );
    BOOST_CHECK_EQUAL( b, true );

    b = vm.is_visible( s, t2) ;
    BOOST_TEST_MESSAGE( "t2 visible from s ? " + b );
    BOOST_CHECK_EQUAL( b, false );

    b = vm.is_visible( s, t3) ;
    BOOST_TEST_MESSAGE( "t3 visible from s ? " + b );
    BOOST_CHECK_EQUAL( b, false );

}

BOOST_AUTO_TEST_SUITE_END();


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

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

// This file was modified by Oracle on 2017, 2019.
// Modifications copyright (c) 2017, 2019, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_POLICIES_COMPARE_HPP
#define BOOST_GEOMETRY_POLICIES_COMPARE_HPP

#include <cstddef>

#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

/*!
\brief Less functor, to sort points in ascending order.
\ingroup compare
\details This functor compares points and orders them on x,
    then on y, then on z coordinate.
\tparam Point the geometry
\tparam Dimension the dimension to sort on, defaults to -1,
    indicating ALL dimensions. That's to say, first on x,
    on equal x-es then on y, etc.
    If a dimension is specified, only that dimension is considered
*/
template
<
    typename Point = void,
    int Dimension = -1,
    typename CSTag = void
>
struct less
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    inline bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::less,
                Point, Point,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension, typename CSTag>
struct less<void, Dimension, CSTag>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    inline bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::less,
                Point1, Point2,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <typename Point, int Dimension>
struct less<Point, Dimension, void>
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    inline bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::less,
                Point, Point,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension>
struct less<void, Dimension, void>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    inline bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::less,
                Point1, Point2,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

/*!
\brief Greater functor
\ingroup compare
\details Can be used to sort points in reverse order
\see Less functor
*/
template
<
    typename Point = void,
    int Dimension = -1,
    typename CSTag = void
>
struct greater
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::greater,
                Point, Point,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension, typename CSTag>
struct greater<void, Dimension, CSTag>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::greater,
                Point1, Point2,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <typename Point, int Dimension>
struct greater<Point, Dimension, void>
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::greater,
                Point, Point,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension>
struct greater<void, Dimension, void>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::greater,
                Point1, Point2,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

/*!
\brief Equal To functor, to compare if points are equal
\ingroup compare
\tparam Geometry the geometry
\tparam Dimension the dimension to compare on, defaults to -1,
    indicating ALL dimensions.
    If a dimension is specified, only that dimension is considered
*/
template
<
    typename Point,
    int Dimension = -1,
    typename CSTag = void
>
struct equal_to
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::equal_to,
                Point, Point,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension, typename CSTag>
struct equal_to<void, Dimension, CSTag>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::equal_to,
                Point1, Point2,
                Dimension,
                CSTag, CSTag
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <typename Point, int Dimension>
struct equal_to<Point, Dimension, void>
{
    typedef Point first_argument_type;
    typedef Point second_argument_type;
    typedef bool result_type;

    bool operator()(Point const& left, Point const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::equal_to,
                Point, Point,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

template <int Dimension>
struct equal_to<void, Dimension, void>
{
    typedef bool result_type;

    template <typename Point1, typename Point2>
    bool operator()(Point1 const& left, Point2 const& right) const
    {
        typedef typename strategy::compare::services::default_strategy
            <
                strategy::compare::equal_to,
                Point1, Point2,
                Dimension
            >::type strategy_type;

        return strategy_type::apply(left, right);
    }
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_POLICIES_COMPARE_HPP


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2012 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(SUBSET_PROPORTIONS_HPP)
#define SUBSET_PROPORTIONS_HPP

#include <vector>
#include <numeric>
#include <boost/lambda/lambda.hpp>
#include <boost/shared_ptr.hpp>

namespace phycas
{

/*----------------------------------------------------------------------------------------------------------------------
|	Class whose objects keep subset proportions for both PartitionModel and RelativeRateDistribution.
*/
class SubsetProportions
    {
    public:

                                    SubsetProportions();
                                    ~SubsetProportions();

        std::vector<double> const & getSubsetProportions() const;
        double                      getLogProdProportions() const;
        void                        setSubsetProportions(std::vector<double> const & subset_proportions);
        void                        setSubsetProportionsFromNumSites(std::vector<unsigned> const & subset_nsites);

    protected:

        void                        recalcLogProdP();

        std::vector<double>         _subset_proportions;   /**< The stored subset proportions vector */
        double                      _log_prod_p;            /**< Constant function of subset proportions needed in order to compute subset relative rates density function */
    };

typedef boost::shared_ptr<SubsetProportions> SubsetProportionsShPtr;
//typedef boost::shared_ptr<const SubsetProportions> SubsetProportionsConstShPtr;

} // namespace phycas

#endif


/**
 * @brief Scene Texture Class
 */

// ********************************************************************************
// Including files
// ********************************************************************************

#include <boost/assert.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <iostream>

#include "Graphics/Texture.h"
#include "SceneTexture.h"

// ********************************************************************************
// Override functions
// ********************************************************************************

void SceneTexture::OnInit() {
  if (const auto msg = CompileAndLinkShader()) {
    std::cerr << msg.value() << std::endl;
    BOOST_ASSERT_MSG(false, "failed to compile or link!");
  }

  view_ = glm::lookAt(glm::vec3(1.0f, 1.25f, 1.25f),
                      glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f));
  proj_ = glm::mat4(1.0f);
  angle_ = 0.0f;

  // テクスチャのロード
  tex_ = Texture::Load("./Assets/Textures/Wood/hp_wood.png");
}

void SceneTexture::OnDestroy() { glDeleteTextures(1, &tex_); }

void SceneTexture::OnUpdate(float) {}

void SceneTexture::OnRender() {

  glEnable(GL_DEPTH_TEST);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glActiveTexture(GL_TEXTURE0);
  glBindTexture(GL_TEXTURE_2D, tex_);

  prog_.Use();
#ifdef __APPLE__
  prog_.SetUniform("Tex1", 0);
#endif
  prog_.SetUniform("Light.L", glm::vec3(1.0f));
  prog_.SetUniform("Light.La", glm::vec3(0.15f));

  prog_.SetUniform("Light.Position", glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
  prog_.SetUniform("Material.Ks", 0.05f, 0.05f, 0.05f);
  prog_.SetUniform("Material.Shininess", 1.0f);

  model_ = glm::mat4(1.0f);
  const glm::mat4 mv = view_ * model_;
  prog_.SetUniform("ModelViewMatrix", mv);
  prog_.SetUniform("NormalMatrix", glm::mat3(glm::vec3(mv[0]), glm::vec3(mv[1]),
                                             glm::vec3(mv[2])));
  prog_.SetUniform("MVP", proj_ * mv);
  cube_.Render();

  glBindTexture(GL_TEXTURE_2D, 0);
  glDisable(GL_DEPTH_TEST);
}

void SceneTexture::OnResize(int w, int h) {
  SetDimensions(w, h);
  glViewport(0, 0, w, h);
  proj_ = glm::perspective(glm::radians(60.0f),
                           static_cast<float>(w) / static_cast<float>(h), 0.3f,
                           100.0f);
}

// ********************************************************************************
// Shader prepare
// ********************************************************************************

std::optional<std::string> SceneTexture::CompileAndLinkShader() {
  return prog_.CompileAndLink(
      {{"./Assets/Shaders/Texture/Texture.vs.glsl", ShaderType::Vertex},
       {"./Assets/Shaders/Texture/Texture.fs.glsl", ShaderType::Fragment}});
}


// Copyright 2015-2020 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

/// @file
/// Test our implementation of isotopic scattering.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <vc.hpp>
#include <qpoint_grid.hpp>
#include <isotopic_scattering.hpp>
#include <processes.hpp>
#include <bulk_properties.hpp>
#include <bulk_hdf5.hpp>

TEST(isotopic_scattering_case, si_isotopes_test) {
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto hdf5_path = si_dir / boost::filesystem::path("Si_threeph.h5");
    boost::mpi::communicator world;
    auto my_id = world.rank();

    auto hdf5_data = alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
    auto description = std::get<0>(hdf5_data);
    auto poscar = std::move(std::get<1>(hdf5_data));
    auto syms = std::move(std::get<2>(hdf5_data));
    auto grid = std::move(std::get<3>(hdf5_data));
    auto threeph_processes = std::move(std::get<4>(hdf5_data));

    double ref_pure[] = {262.5942,
                         188.08561,
                         147.67158,
                         122.17161,
                         104.50416,
                         91.476633,
                         81.437856,
                         73.445211,
                         66.919382,
                         61.483531};
    double ref_natural[] = {227.49185,
                            168.89437,
                            135.28229,
                            113.38867,
                            97.900208,
                            86.306323,
                            77.268422,
                            70.005637,
                            64.030131,
                            59.020394};

    std::vector<double> Ts;

    for (auto i = 0; i < 10; ++i)
        Ts.emplace_back(200. + 50. * i);

    auto twoph_processes = alma::find_allowed_twoph(*grid, world);
    Eigen::ArrayXXd elastic_w0(
        alma::calc_w0_twoph(*poscar, *grid, twoph_processes, world));

    auto pos = 0;

    for (auto T : Ts) {
        Eigen::ArrayXXd anharmonic_w0(
            alma::calc_w0_threeph(*grid, *threeph_processes, T, world));
        Eigen::ArrayXXd total_w0(anharmonic_w0 + elastic_w0);

        if (my_id == 0) {
            auto kappa_pure =
                alma::calc_kappa(*poscar, *grid, anharmonic_w0, T);
            auto kappa_natural = alma::calc_kappa(*poscar, *grid, total_w0, T);

            EXPECT_NEAR(
                ref_pure[pos], kappa_pure(0, 0), 5e-3 * kappa_pure(0, 0));
            EXPECT_NEAR(
                ref_natural[pos], kappa_natural(0, 0), 5e-3 * kappa_pure(0, 0));
            ++pos;
        }
    }
}

TEST(isotopic_scattering_case, sige_test) {
    boost::mpi::environment env;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);

    auto si_dir = basedir / boost::filesystem::path("Si");
    auto si_poscar_path = si_dir / boost::filesystem::path("POSCAR");
    auto si_poscar = alma::load_POSCAR(si_poscar_path.string().c_str());
    auto si_ifc_path = si_dir / boost::filesystem::path("FORCE_CONSTANTS");
    auto si_ifcs = alma::load_FORCE_CONSTANTS(
        si_ifc_path.string().c_str(), *si_poscar, 5, 5, 5);
    auto si_thirdorder_path =
        si_dir / boost::filesystem::path("FORCE_CONSTANTS_3RD");
    auto si_thirdorder = alma::load_FORCE_CONSTANTS_3RD(
        si_thirdorder_path.string().c_str(), *si_poscar);

    auto ge_dir = basedir / boost::filesystem::path("Ge");
    auto ge_poscar_path = ge_dir / boost::filesystem::path("POSCAR");
    auto ge_poscar = alma::load_POSCAR(ge_poscar_path.string().c_str());
    auto ge_ifc_path = ge_dir / boost::filesystem::path("FORCE_CONSTANTS");
    auto ge_ifcs = alma::load_FORCE_CONSTANTS(
        ge_ifc_path.string().c_str(), *ge_poscar, 5, 5, 5);
    auto ge_thirdorder_path =
        ge_dir / boost::filesystem::path("FORCE_CONSTANTS_3RD");
    auto ge_thirdorder = alma::load_FORCE_CONSTANTS_3RD(
        ge_thirdorder_path.string().c_str(), *ge_poscar);

    std::vector<double> ratios({81.6, 18.4});
    auto vc_poscar = alma::vc_mix_structures({*si_poscar, *ge_poscar}, ratios);
    auto vc_ifcs = alma::vc_mix_harmonic_ifcs({*si_ifcs, *ge_ifcs}, ratios);
    auto vc_thirdorder =
        alma::vc_mix_thirdorder_ifcs({*si_thirdorder, *ge_thirdorder}, ratios);

    auto syms = alma::Symmetry_operations(*vc_poscar);
    auto grid = std::make_unique<alma::Gamma_grid>(
        *vc_poscar, syms, *vc_ifcs, 12, 12, 12);
    grid->enforce_asr();

    boost::mpi::communicator world;
    auto my_id = world.rank();

    auto twoph_processes = alma::find_allowed_twoph(*grid, world);
    Eigen::ArrayXXd elastic_w0(
        alma::calc_w0_twoph(*vc_poscar, *grid, twoph_processes, world));
    auto threeph_processes = alma::find_allowed_threeph(*grid, world, 0.1);

    // Note that these are not converged values. The Brillouin zone is
    // not adequately sampled by such a coarse grid. The values are only
    // intended to be quickly reproducible.
    double ref_pure[] = {209.28311,
                         153.57733,
                         122.15775,
                         101.83702,
                         87.52949,
                         76.864022,
                         68.582445,
                         61.952282,
                         56.516531,
                         51.974401};
    double ref_natural[] = {3.0577846,
                            3.0684651,
                            3.0603444,
                            3.0434214,
                            3.0221372,
                            2.9986542,
                            2.9741031,
                            2.9491094,
                            2.9240331,
                            2.8990882};

    std::vector<double> Ts;

    for (auto i = 0; i < 10; ++i)
        Ts.emplace_back(200. + 50. * i);

    // Precompute the matrix elements.
    for (std::size_t i = 0; i < threeph_processes.size(); ++i)
        threeph_processes[i].compute_vp2(*vc_poscar, *grid, *vc_thirdorder);
    auto pos = 0;

    for (auto T : Ts) {
        Eigen::ArrayXXd anharmonic_w0(
            alma::calc_w0_threeph(*grid, threeph_processes, T, world));
        Eigen::ArrayXXd total_w0(anharmonic_w0 + elastic_w0);

        if (my_id == 0) {
            auto kappa_pure =
                alma::calc_kappa(*vc_poscar, *grid, anharmonic_w0, T);
            auto kappa_natural =
                alma::calc_kappa(*vc_poscar, *grid, total_w0, T);

            EXPECT_NEAR(
                ref_pure[pos], kappa_pure(0, 0), 5e-3 * kappa_pure(0, 0));
            EXPECT_NEAR(ref_natural[pos],
                        kappa_natural(0, 0),
                        5e-3 * kappa_natural(0, 0));
            ++pos;
        }
    }
}

int main(int argc, char** argv) {
    boost::mpi::environment env;

    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <cmath>
#include <ctime>
#include <QDebug>
#include <QAction>
#include <QToolBar>
#include <QMessageBox>
#include <QVBoxLayout>
#include <QTableView>
#include <QDockWidget>
#include <QHeaderView>
#include <QPushButton>
#include <QLabel>
#include <QDoubleSpinBox>
#include <QSpinBox>
#include <functional>
#include <boost/serialization/shared_ptr.hpp>
#include <pcl/search/kdtree.h>
#include <pcl/features/principal_curvatures.h>
#include "plugins/markov/markov.h"
#include "model/layerlist.h"
#include "model/cloudlist.h"
#include "gui/glwidget.h"
#include "gui/flatview.h"
#include "gui/mainwindow.h"
#include "commands/select.h"
#include "pluginsystem/core.h"
#include "plugins/markov/mincut.h"
#include "plugins/normalestimation/normalestimation.h"
#include "utilities/utils.h"
#include "utilities/cv.h"

#include "data.h"
#include "online_rf.h"
#include "experimenter.h"

QString Markov::getName(){
    return "markov";
}

void Markov::initialize(Core *core){
    core_= core;
    cl_ = core_->cl_;
    ll_ = core_->ll_;
    glwidget_ = core_->mw_->glwidget_;
    flatview_ = core_->mw_->flatview_;
    mw_ = core_->mw_;

    pca_radius_ = 0.5f;
    curvature_radius_ = 0.1;
    octree_cell_size_ = 0.05;

    tree_count_ = 100;
    tree_depth_ = 10;
    max_nn_ = 100000;
    density_radius_ = 0.25;

    enable_ = new QAction(QIcon(":/settings.png"), "Forest settings", 0);
    forest_action_ = new QAction(QIcon(":/randomforest.png"), "Run random forest", 0);

    feature_list_ = new FeatureList();
    feature_view_ = new QTableView();
    dock_widget_ = new QWidget();
    dock_ = new QDockWidget();

    pca_radius_spinner_ = new QDoubleSpinBox();
    curvature_radius_spinner_ = new QDoubleSpinBox();
    octree_cell_size_spinner_ = new QDoubleSpinBox();
    density_radius_spinner_ = new QDoubleSpinBox();

    tree_count_spinner_ = new QSpinBox();
    tree_depth_spinner_ = new QSpinBox();
    max_nn_spinner_ = new QSpinBox();

    pca_radius_spinner_->setDecimals(3);
    curvature_radius_spinner_->setDecimals(3);
    octree_cell_size_spinner_->setDecimals(3);

    tree_count_spinner_->setRange(1, 5000);
    tree_depth_spinner_->setRange(1, 100);
    max_nn_spinner_->setRange(1, 100000);

    pca_radius_spinner_->setValue(pca_radius_);
    curvature_radius_spinner_->setValue(curvature_radius_);
    octree_cell_size_spinner_->setValue(octree_cell_size_);

    tree_count_spinner_->setValue(tree_count_);
    tree_depth_spinner_->setValue(tree_depth_);
    max_nn_spinner_->setValue(max_nn_);

    connect(cl_, &CloudList::updatedActive, [&](){
        pca_dirty_ = true;
        curvatures_dirty_ = true;
        downsample_dirty_ = true;
    });

    connect(density_radius_spinner_, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        density_radius_ = value;
    });

    connect(pca_radius_spinner_, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        pca_radius_ = pca_radius_spinner_->value();
        pca_dirty_ = true;
    });

    connect(curvature_radius_spinner_, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        curvature_radius_ = curvature_radius_spinner_->value();
        curvatures_dirty_ = true;
    });

    connect(octree_cell_size_spinner_, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        octree_cell_size_ = octree_cell_size_spinner_->value();
        pca_dirty_ = true;
        curvatures_dirty_ = true;
        downsample_dirty_ = true;
    });

    connect(tree_count_spinner_, static_cast<void(QSpinBox::*)(int)>(&QSpinBox::valueChanged), [=] (int value){
        tree_count_ = value;
    });

    connect(tree_depth_spinner_, static_cast<void(QSpinBox::*)(int)>(&QSpinBox::valueChanged), [=] (int value){
        tree_depth_ = value;
    });

    connect(max_nn_spinner_, static_cast<void(QSpinBox::*)(int)>(&QSpinBox::valueChanged), [=] (int value){
        max_nn_ = value;
        pca_dirty_ = true;
    });

    connect(forest_action_, &QAction::triggered, [this] (bool on) {
        randomforest();
    });

    connect(enable_, SIGNAL(triggered()), this, SLOT(enable()));
}

void Markov::initialize2(PluginManager * pm) {
    ne_ = pm->findPlugin<NormalEstimator>();
    if (ne_ == nullptr) {
        qDebug() << "Normal estimator plugin needed for markov";
        return;
    }

    enable_->setCheckable(true);
    mw_->toolbar_->addAction(enable_);
    mw_->toolbar_->addAction(forest_action_);

    enabled_ = false;

    // DOCK

    feature_view_->setModel(feature_list_);
    feature_view_->setColumnWidth(0, 30);
    feature_view_->verticalHeader()->hide();
    feature_view_->horizontalHeader()->hide();

    feature_view_->horizontalHeader()->setStretchLastSection(true);
    feature_view_->resizeRowsToContents();

    QVBoxLayout * dock_layout = new QVBoxLayout();
    dock_layout->addWidget(feature_view_);

    dock_layout->addWidget(new QLabel("Number of trees"));
    dock_layout->addWidget(tree_count_spinner_);
    dock_layout->addWidget(new QLabel("Max tree depth"));
    dock_layout->addWidget(tree_depth_spinner_);

    dock_layout->addWidget(new QLabel("PCA max nn"));
    dock_layout->addWidget(max_nn_spinner_);
    dock_layout->addWidget(new QLabel("PCA radius"));
    dock_layout->addWidget(pca_radius_spinner_);
    dock_layout->addWidget(new QLabel("Curvature radius"));
    dock_layout->addWidget(curvature_radius_spinner_);
    dock_layout->addWidget(new QLabel("Downsample radius"));
    dock_layout->addWidget(octree_cell_size_spinner_);
    dock_layout->addWidget(new QLabel("Density radius"));
    dock_layout->addWidget(density_radius_spinner_);

    dock_layout->setStretch(100, 100);

    dock_widget_->setLayout(dock_layout);
    dock_->setWindowTitle(tr("Forest options"));
    dock_->setWidget(dock_widget_);
}

void Markov::cleanup(){

    if(enabled_){
        disable();
    }

//    mw_->removeDockWidget(dock_widget_);
    mw_->toolbar_->removeAction(enable_);
    mw_->toolbar_->removeAction(forest_action_);
    delete forest_action_;
    delete enable_;
}

Markov::~Markov(){
    qDebug() << "Markov deleted";
//    delete feature_list_;
//    delete feature_view_;
}

void Markov::enable() {
    if(enabled_){
        disable();
        return;
    }

    QTabWidget * tabs = qobject_cast<QTabWidget *>(glwidget_->parent()->parent());
    tabs->setCurrentWidget(glwidget_);
    enable_->setChecked(true);

    emit enabling();

    connect(core_, SIGNAL(endEdit()), this, SLOT(disable()));

    mw_->addDockWidget(Qt::RightDockWidgetArea, dock_);
    mw_->tabifyDockWidget(mw_->options_dock_, dock_);
    dock_->show();
    dock_->raise();

    enabled_ = true;

}

void Markov::disable() {
    enable_->setChecked(false);
    disconnect(core_, SIGNAL(endEdit()), this, SLOT(disable()));
    mw_->removeDockWidget(dock_);
    enabled_ = false;
}

void Markov::randomforest(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

//    pca_dirty_ = true;
//    curvatures_dirty_ = true;
//    downsample_dirty_ = true;

    uint NUM_FEATURES = feature_list_->activeCount();

    bool use_x = feature_list_->hasFeature("X");
    bool use_y = feature_list_->hasFeature("Y");
    bool use_z = feature_list_->hasFeature("Z");
    bool use_i = feature_list_->hasFeature("Intensity");
    bool use_nx = feature_list_->hasFeature("X Normal");
    bool use_ny = feature_list_->hasFeature("Y Normal");
    bool use_nz = feature_list_->hasFeature("Z Normal");
    bool use_e1 = feature_list_->hasFeature("Eigen 1");
    bool use_e2 = feature_list_->hasFeature("Eigen 2");
    bool use_e3 = feature_list_->hasFeature("Eigen 3");
    bool use_pc1 = feature_list_->hasFeature("Curvature 1");
    bool use_pc2 = feature_list_->hasFeature("Curvature 2");
    bool use_a = feature_list_->hasFeature("Anisotrophy");
    bool use_p = feature_list_->hasFeature("Planarity");
    bool use_s = feature_list_->hasFeature("Sphericity");
    bool use_l = feature_list_->hasFeature("Linearity");
    bool use_o = feature_list_->hasFeature("Omnivariance");
    bool use_ee = feature_list_->hasFeature("Eigenentrophy");
    bool use_rc = feature_list_->hasFeature("Curvature");
    bool use_odist = feature_list_->hasFeature("Distance from origin");
    bool use_n_count = feature_list_->hasFeature("Number of neighbours");

    if(use_x) qDebug() << "using: " << "X";
    if(use_y) qDebug() << "using: " << "Y";
    if(use_z) qDebug() << "using: " << "Z";
    if(use_i) qDebug() << "using: " << "Intensity";
    if(use_nx) qDebug() << "using: " << "X Normal";
    if(use_ny) qDebug() << "using: " << "Y Normal";
    if(use_nz) qDebug() << "using: " << "Z Normal";
    if(use_e1) qDebug() << "using: " << "Eigen 1";
    if(use_e2) qDebug() << "using: " << "Eigen 2";
    if(use_e3) qDebug() << "using: " << "Eigen 3";
    if(use_pc1) qDebug() << "using: " << "Curvature 1";
    if(use_pc2) qDebug() << "using: " << "Curvature 2";
    if(use_a) qDebug() << "using: " << "Anisotrophy";
    if(use_p) qDebug() << "using: " << "Planarity";
    if(use_s) qDebug() << "using: " << "Sphericity";
    if(use_l) qDebug() << "using: " << "Linearity";
    if(use_o) qDebug() << "using: " << "Omnivariance";
    if(use_ee) qDebug() << "using: " << "Eigenentrophy";
    if(use_rc) qDebug() << "using: " << "Curvature";
    if(use_odist) qDebug() << "using: " << "Distance from origin";
    if(use_n_count) qDebug() << "using: " << "Number of neighbours";

    clock_t action_start = std::clock();

    // get normals
    pcl::PointCloud<pcl::Normal>::Ptr normals = ne_->getNormals(cl_->active_);

    clock_t downsample_start = std::clock();

    // zip and downsample
    if(downsample_dirty_) {
        pcl::PointCloud<pcl::PointXYZINormal>::Ptr zipped = zipNormals(cl_->active_, normals);
        big_to_small_.clear();
        smallcloud_ = octreeDownsample(zipped.get(), octree_cell_size_, big_to_small_);
        downsample_dirty_ = false;
    }

    double downsample_elapsed = double(std::clock() - downsample_start) / CLOCKS_PER_SEC;

//    clock_t nn_search_start = std::clock();
//    double total = 0;

//    {
//        typename pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr cptr(smallcloud_.get(), boost::serialization::null_deleter());
//        pcl::KdTreeFLANN<pcl::PointXYZINormal> search;
//        search.setInputCloud(cptr);

//        std::vector<float> kDist;
//        std::vector<int> kIdxs;

//        for(uint i = 0; i < smallcloud_->size(); i++){
//            search.radiusSearch(i, 0.15, kIdxs, kDist);
//            total+=kIdxs.size();
//        }
//    }

//    qDebug() << "cloud size" << smallcloud_->size();
//    qDebug() << "total" << total;
//    qDebug() << "avg" << double(total)/smallcloud_->size();

//    double nn_search_elapsed = double(std::clock() - nn_search_start) / CLOCKS_PER_SEC;
//    qDebug() << "time" << nn_search_elapsed << "avg" << double(nn_search_elapsed)/cloud->size();

//    qDebug() << "downsample: " << downsample_elapsed;

    clock_t upsample_start = std::clock();

    std::vector<int> trash;
    for(int idx = 0; idx < cloud->size(); idx++) {
        trash.push_back(big_to_small_[idx]);
    }

    double upsample_elapsed = double(std::clock() - upsample_start) / CLOCKS_PER_SEC;

    qDebug() << "upsample time" << upsample_elapsed << "trash: " << trash.size();

    return;

    clock_t density_start = std::clock();

    // density

    std::vector<int> neighbour_count;

    if(use_n_count){
        pcl::KdTreeFLANN<pcl::PointXYZINormal> search;
        search.setInputCloud(smallcloud_);

        std::vector<float> kDist;
        std::vector<int> kIdxs;

        for(uint i = 0; i < smallcloud_->size(); i++){
            search.radiusSearch(i, density_radius_, kIdxs, kDist);
            neighbour_count.push_back(kIdxs.size());
        }
    }

    clock_t density_elapsed = double(std::clock() - density_start) / CLOCKS_PER_SEC;

    clock_t pca_start = std::clock();

    // PCA
    if(pca_dirty_) {
        pca_ = getPCA(smallcloud_.get(), pca_radius_, max_nn_);
        pca_dirty_ = false;
    }

    double pca_elapsed = double(std::clock() - pca_start) / CLOCKS_PER_SEC;

    clock_t curvature_start = std::clock();

    // Curvature
    // pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures_;
    if((use_pc1 || use_pc2) && curvatures_dirty_) {
        pcl::PrincipalCurvaturesEstimation<pcl::PointXYZINormal, pcl::PointXYZINormal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;
        principalCurvaturesEstimation.setInputCloud(smallcloud_);
        principalCurvaturesEstimation.setInputNormals(smallcloud_);

        pcl::search::KdTree<pcl::PointXYZINormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZINormal>);
        tree->setInputCloud(smallcloud_);
        principalCurvaturesEstimation.setSearchMethod (tree);
        principalCurvaturesEstimation.setRadiusSearch(curvature_radius_);

        principal_curvatures_ = pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr(new pcl::PointCloud<pcl::PrincipalCurvatures> ());
        principalCurvaturesEstimation.compute (*principal_curvatures_);
        curvatures_dirty_ = false;
    }

    double curvature_elapsed = double(std::clock() - curvature_start) / CLOCKS_PER_SEC;

    // Feature compute
    auto mkFeatureVector = [&](uint idx) {
        Eigen::VectorXd vec(NUM_FEATURES);
        vec.setZero();

        int featnum = -1;

        //set samples
        if(use_x)
            vec(++featnum) = smallcloud_->at(idx).x;
        if(use_y)
            vec(++featnum) = smallcloud_->at(idx).y;
        if(use_z)
            vec(++featnum) = smallcloud_->at(idx).z;
        if(use_i)
            vec(++featnum) = smallcloud_->at(idx).intensity;
        if(use_nx)
            vec(++featnum) = smallcloud_->at(idx).normal_x;
        if(use_ny)
            vec(++featnum) = smallcloud_->at(idx).normal_y;
        if(use_nz)
            vec(++featnum) = smallcloud_->at(idx).normal_z;

        Eigen::Vector3f & pca = (*pca_)[idx];

        if(use_e1)
            vec(++featnum) = pca[0];
        if(use_e2)
            vec(++featnum) = pca[1];
        if(use_e3)
            vec(++featnum) = pca[2];

        if (use_pc1 || use_pc2) {
            const pcl::PrincipalCurvatures & curv = principal_curvatures_->points[idx];

            if(use_pc1)
                vec(++featnum) = curv.pc1;
            if(use_pc2)
                vec(++featnum) = curv.pc2;
        }

        // anisotrophy =
        if(use_a)
            vec(++featnum) = (pca[0] - pca[2]) / pca[0];

        // planarity
        if(use_p)
            vec(++featnum) = (pca[1] - pca[2]) / pca[0];

        // Sphericity
        if(use_s)
            vec(++featnum) = pca[2] / pca[0];

        // linearity
        if(use_l)
            vec(++featnum) = (pca[0] - pca[1]) / pca[0];

        // omnivariance
        if(use_o)
            vec(++featnum) = std::cbrt(pca[0] * pca[1] * pca[0]);

        // eigen entrophy
        if(use_ee)
            vec(++featnum) = -(pca[0] * std::log(pca[0]) + pca[1] * std::log(pca[1]) + pca[2] * std::log(pca[0]));

        // rusu curvature estimation
        if(use_rc)
            vec(++featnum) = pca[0] / (pca[0] + pca[1] + pca[2]);

        if(use_odist)
            vec(++featnum) = smallcloud_->at(idx).getVector3fMap().norm();
        if(use_n_count && neighbour_count[idx]) {
//            vec(++featnum) = double(neighbour_count[idx])/pow(smallcloud_->at(idx).getVector3fMap().norm(), 2);
            vec(++featnum) = double(neighbour_count[idx]);
        }

        return vec;
    };

    /// Random forest

    Hyperparameters hp;

    // Forest
    hp.maxDepth = tree_depth_;
    hp.numRandomTests = 20;
    hp.counterThreshold = 140; // Number of samples seen by tree?
    hp.numTrees = tree_count_;

    // Experimenter
    hp.numEpochs = 1;
    hp.findTrainError = 0;

    // Output
    hp.verbose = 1;

    clock_t get_selections_start = std::clock();

    // Load selection
    std::vector<boost::shared_ptr<std::vector<int>>> selections = cloud->getSelections();

    double get_selections_elapsed = double(std::clock() - get_selections_start) / CLOCKS_PER_SEC;

    clock_t feature_compute_start = std::clock();

    std::vector<int> selection_sources;

    for(uint i = 0; i < selections.size(); i++) {
        if(selections[i]->size() > 30)
            selection_sources.push_back(i);
    }

    if(selection_sources.size() < 2) {
        qDebug() << "Not enough data";
        return;
    }

    // Creating the train data
    DataSet dataset_train;
    dataset_train.m_numFeatures = NUM_FEATURES;
    dataset_train.m_numClasses = 8;

    std::set<int> seen;

    uint sample_size = 0;
    uint smallest_class_size = 999999999;
    for(uint s : selection_sources) {
        sample_size += selections[s]->size();
        smallest_class_size = selections[s]->size() < smallest_class_size ? selections[s]->size() : smallest_class_size;
    }

    const int MAX = 999999999;
    int max_samples = smallest_class_size * selection_sources.size();
    max_samples = max_samples > MAX ? MAX : max_samples;

    const uint max_samples_per_class = MAX/selection_sources.size();
    int samples_per_class = smallest_class_size > max_samples_per_class ? max_samples_per_class : smallest_class_size;

    int count = 0;
    for(uint y : selection_sources) {

        const int sample_count = selections[y]->size();
        const float sample_pct = float(sample_count)/sample_size;
        int s_nth = sample_count / samples_per_class;
        s_nth = s_nth < 1 ? 1 : s_nth;

        qDebug() << "label: " << y << ", pct: " << 100*sample_pct << ", samples " << sample_count << ", every nth point; " << s_nth;

        for(int big_idx : *selections[y]) {
            int idx = big_to_small_[big_idx];

            if(!seen.insert(idx).second)
                continue;

            count++;

            if(count % s_nth != 0)
                continue;

            Sample sample;
            sample.id = idx;
            sample.w = 1.0;
            sample.y = y;
            sample.x = mkFeatureVector(idx);

            // feature compute
            dataset_train.m_samples.push_back(sample);
        }

    }

    dataset_train.m_numSamples = dataset_train.m_samples.size();

    qDebug() << "Size tr:" << dataset_train.m_numSamples;

    if(dataset_train.m_numSamples < 10){
        qDebug() << "Not enough samples";
        return;
    }

    double feature_compute_elapsed = double(std::clock() - feature_compute_start) / CLOCKS_PER_SEC;

    clock_t train_start = std::clock();
    dataset_train.findFeatRange();

    OnlineRF model(hp, dataset_train.m_numClasses, dataset_train.m_numFeatures, dataset_train.m_minFeatRange, dataset_train.m_maxFeatRange);
    train(&model, dataset_train, hp);

    double train_elapsed = double(std::clock() - train_start) / CLOCKS_PER_SEC;

    Result invalid(8);
    invalid.prediction = -1;

    clock_t classify_start = std::clock();

    std::vector<Result> results(smallcloud_->points.size(), invalid);

    for(size_t idx = 0; idx < smallcloud_->points.size(); ++idx) {
        Sample sample;
        sample.id = idx;
        sample.w = 1.0;
        sample.x = mkFeatureVector(idx);
        model.eval(sample, results[idx]);
    }

    double classify_elapsed = double(std::clock() - classify_start) / CLOCKS_PER_SEC;

    clock_t result_select_start = std::clock();

    // Select fg and bg
    std::vector<boost::shared_ptr<std::vector<int>>> seg_selections(8);
    for(int i = 0; i < 8; i++){
        seg_selections[i] = boost::make_shared<std::vector<int>>();
    }

    for(size_t idx = 0; idx < cloud->points.size(); ++idx) {
        int idx_small = big_to_small_[idx];
        Result & res = results[idx_small];
        seg_selections[res.prediction]->push_back(idx);
    }

    core_->us_->beginMacro("Random forest");

    for(int i = 0; i < 8; i++){
        if(seg_selections[i]->size() > 0) {
            core_->us_->push(new Select(cl_->active_, seg_selections[i], false, 1 << i, true, ll_->getHiddenLabels()));
        }
    }

    double result_select_elapsed = double(std::clock() - result_select_start) / CLOCKS_PER_SEC;
    double action_elapsed = double(std::clock() - action_start) / CLOCKS_PER_SEC;

    core_->us_->endMacro();

    qDebug() << "downsample: " << downsample_elapsed << "\n"
                 << "pca: "<< pca_elapsed << "\n"
                 << "curvature: "<< curvature_elapsed << "\n"
                 << "density" << density_elapsed << "\n"
                 << "get_selections: "<< get_selections_elapsed << "\n"
                 << "feature_compute: "<< feature_compute_elapsed << "\n"
                 << "train: "<< train_elapsed << "\n"
                 << "classify: "<< classify_elapsed << "\n"
                 << "result_select: "<< result_select_elapsed << "\n"
                 << "total: "<< action_elapsed << "\n";
}

Q_PLUGIN_METADATA(IID "za.co.circlingthesun.cloudclean.iplugin")


#ifndef SERVO_CONVERTER_HPP
#define SERVO_CONVERTER_HPP

/*
 * Servo to radian converter
 *
 * Author: Andrew Barry, <abarry@csail.mit.edu> 2015
 *
 */

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <sstream>

#include "../../LCM/mav_pose_t.h"

#include <Eigen/Core>

#include "gtest/gtest.h"

#include <bot_param/param_client.h>

struct ServoConfig {
    double elevL_slope;
    double elevL_y_intercept;

    double elevR_slope;
    double elevR_y_intercept;

    double throttle_slope;
    double throttle_y_intercept;
};

class ServoConverter {

    public:
        ServoConverter(BotParam *param);

        Eigen::Vector3i RadiansToServoCommands(Eigen::Vector3d commands) const;

        Eigen::Vector3d ServoCommandsToRadians(Eigen::Vector3i commands) const;

        Eigen::Vector3i MinMaxCommands(Eigen::Vector3i commands) const;

        Eigen::Vector3i GetTrimCommands() const;

    private:
        ServoConfig rad_to_servo_;
        ServoConfig servo_to_rad_;

        int elevL_min_;
        int elevL_max_;

        int elevR_min_;
        int elevR_max_;

        int throttle_min_;
        int throttle_max_;

        int elevL_trim_;
        int elevR_trim_;
        int throttle_trim_;

};

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Rice University
 *  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 Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT 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.
 *********************************************************************/

/* Author: Ryan Luna */

#include <iostream>
#include "PlanarManipulator.h"
#include <boost/math/constants/constants.hpp>
#include <stdexcept>

#define PI boost::math::constants::pi<double>()
#define TWOPI boost::math::constants::two_pi<double>()

PlanarManipulator::PlanarManipulator(unsigned int numLinks, double linkLength, const std::pair<double, double> &origin)
  : numLinks_(numLinks), linkLengths_(numLinks_, linkLength)
{
    baseFrame_ = Eigen::Affine2d::Identity();
    baseFrame_.translation()[0] = origin.first;
    baseFrame_.translation()[1] = origin.second;

    hasBounds_.assign(numLinks_, false);
    lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
    upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
}

PlanarManipulator::PlanarManipulator(unsigned int numLinks, const std::vector<double> &linkLengths,
                                     const std::pair<double, double> &origin)
  : numLinks_(numLinks), linkLengths_(linkLengths)
{
    if (linkLengths_.size() != numLinks)
    {
        std::cerr << "Length of linkLengths (" << linkLengths.size() << ") is not equal to the number of links ("
                  << numLinks << ")" << std::endl;
        throw;
    }

    baseFrame_ = Eigen::Affine2d::Identity();
    baseFrame_.translation()[0] = origin.first;
    baseFrame_.translation()[1] = origin.second;

    hasBounds_.assign(numLinks_, false);
    lowerBounds_.assign(numLinks_, std::numeric_limits<double>::min());
    upperBounds_.assign(numLinks_, std::numeric_limits<double>::max());
}

PlanarManipulator::~PlanarManipulator()
{
}

// Return the number of links
unsigned int PlanarManipulator::getNumLinks() const
{
    return numLinks_;
}

const Eigen::Affine2d &PlanarManipulator::getBaseFrame() const
{
    return baseFrame_;
}

void PlanarManipulator::setBaseFrame(const Eigen::Affine2d &frame)
{
    baseFrame_ = frame;
}

// Return the length of each link
const std::vector<double> &PlanarManipulator::getLinkLengths() const
{
    return linkLengths_;
}

// Set the bounds for link # link
void PlanarManipulator::setBounds(unsigned int link, double low, double high)
{
    assert(link < hasBounds_.size());

    hasBounds_[link] = true;
    lowerBounds_[link] = low;
    upperBounds_[link] = high;
}

// Set the bounds for ALL links
void PlanarManipulator::setBounds(const std::vector<double> &low, const std::vector<double> &high)
{
    assert(low.size() == high.size());
    assert(low.size() == numLinks_);

    hasBounds_.assign(numLinks_, true);
    lowerBounds_ = low;
    upperBounds_ = high;
}

bool PlanarManipulator::hasBounds(unsigned int link) const
{
    return hasBounds_[link];
}

void PlanarManipulator::getBounds(unsigned int link, double &low, double &high) const
{
    low = lowerBounds_[link];
    high = upperBounds_[link];
}
const std::vector<double> &PlanarManipulator::lowerBounds() const
{
    return lowerBounds_;
}

const std::vector<double> &PlanarManipulator::upperBounds() const
{
    return upperBounds_;
}

// Forward kinematics for the given joint configuration.  The frames for links
// 1 through the end-effector are returned.
void PlanarManipulator::FK(const std::vector<double> &joints, std::vector<Eigen::Affine2d> &frames) const
{
    FK(&joints[0], frames);
}

void PlanarManipulator::FK(const Eigen::VectorXd &joints, std::vector<Eigen::Affine2d> &frames) const
{
    FK(&joints(0), frames);
}

void PlanarManipulator::FK(const double *joints, std::vector<Eigen::Affine2d> &frames) const
{
    frames.clear();
    Eigen::Affine2d frame(baseFrame_);

    for (unsigned int i = 0; i < numLinks_; ++i)
    {
        // Rotate, then translate.  Just like the old gypsy woman said.
        Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        frame = frame * offset;
        frames.push_back(frame);
    }
}

void PlanarManipulator::FK(const std::vector<double> &joints, Eigen::Affine2d &eeFrame) const
{
    FK(&joints[0], eeFrame);
}

void PlanarManipulator::FK(const Eigen::VectorXd &joints, Eigen::Affine2d &eeFrame) const
{
    FK(&joints(0), eeFrame);
}

void PlanarManipulator::FK(const double *joints, Eigen::Affine2d &eeFrame) const
{
    eeFrame = baseFrame_;

    for (unsigned int i = 0; i < numLinks_; ++i)
    {
        // Rotate, then translate.  Just like the old gypsy woman said.
        Eigen::Affine2d offset(Eigen::Rotation2Dd(joints[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        eeFrame = eeFrame * offset;
    }
}

// Inverse kinematics for the given end effector frame.  Only one solution is returned.
// Returns false if no solution exists to the given pose.
bool PlanarManipulator::IK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame) const
{
    std::vector<double> seed(numLinks_, M_PI / 2.0);
    return IK(solution, seed, eeFrame);
}

bool PlanarManipulator::IK(std::vector<double> &solution, const std::vector<double> &seed,
                           const Eigen::Affine2d &desiredFrame) const
{
    // desired frame is clearly impossible to achieve
    if (infeasible(desiredFrame))
        return false;

    // This is the orientation for the end effector
    double angle = acos(desiredFrame.rotation()(0, 0));
    // Due to numerical instability, sometimes acos will return nan if
    // the value is slightly larger than one.  Check for this and try the
    // asin of the next value
    if (angle != angle)  // a nan is never equal to itself
        angle = asin(desiredFrame.rotation()(0, 1));

    // If still nan, return false
    if (angle != angle)
        return false;

    // Get the current pose
    Eigen::Affine2d frame;
    FK(seed, frame);
    Eigen::VectorXd current;
    frameToPose(frame, current);

    // Get the desired pose
    Eigen::VectorXd desired;
    frameToPose(desiredFrame, desired);

    // Compute the error
    Eigen::VectorXd e(desired - current);

    Eigen::VectorXd joints(seed.size());
    for (size_t i = 0; i < seed.size(); ++i)
        joints(i) = seed[i];

    double alpha = 0.1;  // step size

    unsigned int iter = 1;
    double eps = 1e-6;

    Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(3, numLinks_);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(3, numLinks_, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(numLinks_, 3);

    // Iterate the jacobian
    while (e.norm() > eps)
    {
        // Get jacobian for this joint configuration
        Jacobian(joints, jac);

        // Compute inverse of the jacobian
        // Really unstable
        // Eigen::MatrixXd jac_inv = ((jac.transpose() * jac).inverse()) * jac.transpose();

        // Moore-Penrose Pseudoinverse
        svd.compute(jac);
        const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType &d = svd.singularValues();

        // "Invert" the singular value matrix
        for (int i = 0; i < d.size(); ++i)
            if (d(i) > eps)
                D(i, i) = 1.0 / d(i);
            else
                D(i, i) = 0.0;

        // Inverse is V*D^-1*U^t
        Eigen::MatrixXd jac_inv = svd.matrixV() * D * svd.matrixU().transpose();

        // Get the joint difference
        Eigen::VectorXd delta_theta = jac_inv * e;

        // Check for failure
        if (delta_theta(0) != delta_theta(0))  // nan
        {
            // std::cout << jac_inv.matrix() << std::endl;
            return false;
        }

        // Increment the current joints by a (small) multiple of the difference
        joints = joints + (alpha * delta_theta);

        // Figure out the current EE pose and update e.
        FK(joints, frame);
        frameToPose(frame, current);
        // double n = e.norm();
        e = desired - current;
        iter++;

        if (iter > 5000)
            return false;  // diverge
    }

    // Store the solution.  Make sure angles are in range [-pi, pi]
    solution.resize(numLinks_);
    for (size_t i = 0; i < solution.size(); ++i)
    {
        double angle = joints(i);
        while (angle > M_PI)
            angle -= 2.0 * M_PI;
        while (angle < -M_PI)
            angle += 2.0 * M_PI;
        solution[i] = angle;
    }

    return true;
}

bool PlanarManipulator::FABRIK(std::vector<double> &solution, const Eigen::Affine2d &eeFrame, double xyTol,
                               double thetaTol) const
{
    std::vector<double> seed(numLinks_, M_PI / 2.0);
    return FABRIK(solution, seed, eeFrame, xyTol, thetaTol);
}

bool PlanarManipulator::FABRIK(std::vector<double> &solution, const std::vector<double> &seed,
                               const Eigen::Affine2d &desiredFrame, double xyTol, double thetaTol) const
{
    unsigned int numLinks = getNumLinks();  // have to use this local variable.  Compiler error (bug?) in
                                            // Eigen::Translation2d when I use numLinks_

    if (seed.size() != numLinks)
    {
        std::cerr << "Seed has length " << seed.size() << " but there are " << numLinks << " links" << std::endl;
        return false;
    }

    // desired frame is clearly impossible to achieve
    if (infeasible(desiredFrame))
        return false;

    // copy the seed into the solution
    solution.assign(seed.begin(), seed.end());

    // Compute the location of the origin of the last link with the correct rotation
    // This location will not move
    Eigen::Vector2d loc(-linkLengths_.back(), 0);
    loc = desiredFrame * loc;

    // The desired orientation of the end effector
    double v = desiredFrame.matrix()(0, 0);
    // Go Go Gadget Numerical Stabilizer!
    if (v < -1.0)
        v = -1.0;
    if (v > 1.0)
        v = 1.0;
    double eeTheta = acos(v);
    if (desiredFrame.matrix()(1, 0) < 0)  // sin is negative, so the angle is negative
        eeTheta = -eeTheta;

    // The location of each joint, starting with the origin and ending with the end effector
    // For an n-joint chain, jointPositions with have n+1 entries.
    std::vector<Eigen::Vector2d> jointPositions;
    jointPositions.push_back(baseFrame_.translation());  // root position

    // Compute the initial locations of each joint
    Eigen::Affine2d frame(baseFrame_);
    for (unsigned int i = 0; i < seed.size(); ++i)
    {
        Eigen::Affine2d offset(Eigen::Rotation2Dd(seed[i]) * Eigen::Translation2d(linkLengths_[i], 0));
        frame = frame * offset;
        jointPositions.push_back(frame.translation());
    }

    // Store the errors here
    double xyError = std::numeric_limits<double>::max();
    double thetaError = std::numeric_limits<double>::max();

    int numIterations = 0;
    int maxIter = 250;  // after this many iterations, declare failure.  This is probably WAY too many - tends to
                        // converge very fast, after 1-3 iterations
    while ((xyError > xyTol || thetaError > thetaTol) && numIterations++ < maxIter)
    {
        xyError = thetaError = 0.0;

        // Placing last link where it must go
        jointPositions[numLinks] = desiredFrame.translation();  // goal position, nailed it.
        jointPositions[numLinks - 1] = loc;                     // goal orientation, nailed it.

        // A reverse coordinate frame, working backward along the chain.  Used to resolve joint limits in backward phase
        // VERY IMPORTANT to translate first, then rotate.  This NOT like the old gypsy woman said.  I'm getting my
        // money back.
        Eigen::Affine2d backwardFrame(Eigen::Translation2d(jointPositions[numLinks - 1]) *
                                      Eigen::Rotation2Dd(PI + eeTheta));  // end of chain, looking down the chain

        // Move backward from the end effector toward the base
        for (int i = jointPositions.size() - 3; i >= 0;
             --i)  // -1 is end effector position.  Skip -2 to preserve the end effector orientation.
        {
            // draw a line between position i+1 and i, and place pt i at the kinematically feasible place along the line
            double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
            jointPositions[i] = (1 - t) * jointPositions[i + 1] + t * jointPositions[i];

            // Ensure the angle for joint i is within bounds.  If not, move the joint position
            Eigen::Vector2d vec = backwardFrame.inverse() * jointPositions[i];
            double angle = -atan2(vec(1), vec(0));  // The angle computed is the negative of the real angle (because we
                                                    // worked backward).

            // If not in bounds, the angle will be one of the joint limits
            if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
            {
                double dlow = fabs(angle - lowerBounds_[i]);
                double dhigh = fabs(angle - upperBounds_[i]);
                angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
            }

            Eigen::Affine2d offset(Eigen::Rotation2Dd(-angle) * Eigen::Translation2d(linkLengths_[i], 0));
            backwardFrame = backwardFrame * offset;
            jointPositions[i] = backwardFrame.translation();
        }

        jointPositions[0] = baseFrame_.translation();  // move base back to where it is supposed to be
        Eigen::Affine2d forwardFrame(baseFrame_);

        // Move forward toward the end effector
        for (size_t i = 0; i < jointPositions.size() - 1;
             ++i)  // This pass moves everything except end effector.  Orientation at the end may be violated, but we
                   // check for this
        {
            // draw a line between position i+1 and i and place i+1 at the kinematically feasible place along the line
            double t = linkLengths_[i] / (jointPositions[i + 1] - jointPositions[i]).norm();
            jointPositions[i + 1] = (1 - t) * jointPositions[i] + t * jointPositions[i + 1];

            // Figure out the joint angle required for this
            Eigen::Vector2d vec = forwardFrame.inverse() * jointPositions[i + 1];
            double angle = atan2(vec(1), vec(0));

            // If not in bounds, the angle will be one of the joint limits
            if (hasBounds_[i] && (angle < lowerBounds_[i] || angle > upperBounds_[i]))
            {
                double dlow = fabs(angle - lowerBounds_[i]);
                double dhigh = fabs(angle - upperBounds_[i]);
                angle = (dlow < dhigh ? lowerBounds_[i] : upperBounds_[i]);
            }
            solution[i] = angle;

            // Update frame
            Eigen::Affine2d offset(Eigen::Rotation2Dd(angle) * Eigen::Translation2d(linkLengths_[i], 0));
            forwardFrame = forwardFrame * offset;
            jointPositions[i + 1] = forwardFrame.translation();  // joint location is the translation of the frame.  Do
                                                                 // this here in case joint limits changed the angle
        }

        // Compute translation and orientation error
        xyError = (jointPositions.back() - desiredFrame.translation()).norm();

        v = forwardFrame.matrix()(0, 0);
        // Go Go Gadget Numerical Stabilizer!
        if (v > 1.0)
            v = 1.0;
        if (v < -1.0)
            v = -1.0;

        // The real angle
        double thetaActual = acos(v);
        if (forwardFrame.matrix()(1, 0) < 0)  // sin is negative, so the angle is negative
            thetaActual = -thetaActual;
        thetaError = fabs(eeTheta - thetaActual);
    }

    // Winning.  Extract joint angles.  Oh wait.  Already done BOOM.
    if (xyError < xyTol && thetaError < thetaTol)
        return true;

    return false;
}

void PlanarManipulator::Jacobian(const double *joints, Eigen::MatrixXd &jac) const
{
    if (jac.rows() != 3 || jac.cols() != numLinks_)
        jac = Eigen::MatrixXd::Zero(3, numLinks_);

    std::vector<double> sins(numLinks_);
    std::vector<double> coss(numLinks_);
    double theta = 0.0;
    for (size_t i = 0; i < numLinks_; ++i)
    {
        theta += joints[i];
        sins[i] = sin(theta);
        coss[i] = cos(theta);
    }

    for (size_t i = 0; i < numLinks_; ++i)
    {
        double entry1 = 0.0;
        double entry2 = 0.0;
        for (size_t j = numLinks_; j > i; --j)
        {
            entry1 += -(linkLengths_[j - 1] * sins[j - 1]);
            entry2 += linkLengths_[j - 1] * coss[j - 1];
        }
        jac(0, i) = entry1;
        jac(1, i) = entry2;
        jac(2, i) = 1.0;
    }
}

// Return the Jacobian for the manipulator at the given joint state.
void PlanarManipulator::Jacobian(const std::vector<double> &joints, Eigen::MatrixXd &jac) const
{
    Jacobian(&joints[0], jac);
}

void PlanarManipulator::Jacobian(const Eigen::VectorXd &joints, Eigen::MatrixXd &jac) const
{
    Jacobian(&joints(0), jac);  // TODO: This is untested.  I dunno if an eigen::vector is contiguous.  It probably is.
}

void PlanarManipulator::frameToPose(const Eigen::Affine2d &frame, Eigen::VectorXd &pose)
{
    pose = Eigen::VectorXd(3);
    pose(0) = frame.translation()(0);
    pose(1) = frame.translation()(1);
    pose(2) = acos(frame.matrix()(0, 0));
}

bool PlanarManipulator::infeasible(const Eigen::Affine2d &frame) const
{
    // Check for impossible query
    // Compute the location of the origin of the last link with the correct rotation
    Eigen::Vector2d loc(-linkLengths_[numLinks_ - 1], 0);
    loc = frame * loc;

    // Length of the chain, except for the last link
    double len = 0.0;
    for (size_t i = 0; i < numLinks_ - 1; ++i)
        len += linkLengths_[i];

    // Make sure the chain (without the last link) is long enough to reach the
    // origin of the last link
    Eigen::Vector2d org(baseFrame_.translation());
    if ((org - loc).norm() > len)  // infeasible IK request
        return true;

    return false;
}


#include "imibot_hardware.h"
#include "ros/ros.h"
#include "imibot_driver/DiffSpeed.h"
#include "imibot_driver/SensorsReadings.h"
#include <boost/assign/list_of.hpp>
#include <iostream>
#include <ros/console.h>

namespace
{
	const uint8_t LEFT = 0, RIGHT = 1;
}

using namespace std;

namespace imibot_base
{

  ImibotHardware::ImibotHardware(ros::NodeHandle nh)
  :
  nh_(nh)
  {
    nh_.param<double>("wheel_diameter", wheel_diameter_, 0.0662);
    nh_.param<double>("max_accel", max_accel_, 5.0);
    nh_.param<double>("max_speed", max_speed_, 1.0);
    this->cmd_pub = nh_.advertise<imibot_driver::DiffSpeed>("robot_mg", 1000, false);
    registerControlInterfaces();
    this->speed_sensors_sub_ = nh_.subscribe("/imibot/speed_sensors", 1, &ImibotHardware::getLastSensorsValues, this);
  }

  void ImibotHardware::registerControlInterfaces()
  {
    ros::V_string joint_names = boost::assign::list_of("front_left_wheel")("front_right_wheel")("rear_left_wheel")("rear_right_wheel");

    for (unsigned int i = 0; i < joint_names.size(); i++)
    {
      hardware_interface::JointStateHandle joint_state_handle(joint_names[i],
                                                              &joints_[i].position, &joints_[i].velocity,
                                                              &joints_[i].effort);
      joint_state_interface_.registerHandle(joint_state_handle);

      hardware_interface::JointHandle joint_handle(joint_state_handle, &joints_[i].velocity_command);
      velocity_joint_interface_.registerHandle(joint_handle);
    }
    registerInterface(&joint_state_interface_);
    registerInterface(&velocity_joint_interface_);

    for (int i = 0; i < 4; i++)
    {
      joints_[i].position = 0;
      joints_[i].position_offset = 0;
      left_speed = right_speed = left_travel = right_travel = 0;
    }
  }

  void ImibotHardware::getLastSensorsValues(const imibot_driver::SensorsReadings::ConstPtr& msg)
  {
    left_speed = msg->left_measured_vel;
    right_speed = msg->right_measured_vel;
    left_travel = msg->left_measured_travel;
    right_travel = msg->right_measured_travel;
  }

  void ImibotHardware::updateJointsFromHardware()
  {
    for (int i = 0; i < 3; i+=2)
    {
      double delta_left = linearToAngular(left_travel) - joints_[i].position;
      joints_[i].position += delta_left;
      joints_[i].velocity = linearToAngular(left_speed);
    }
    for (int i = 1; i < 5; i+=2)
    {
      double delta_right = linearToAngular(right_travel) - joints_[i].position;

      joints_[i].position += delta_right;
      joints_[i].velocity = linearToAngular(right_speed);
    }
  }

  /**
  * Get latest velocity commands from ros_control via joint structure, and send to MCU
  */
  void ImibotHardware::writeCommandsToHardware()
  {
    double diff_speed_left = angularToLinear(joints_[LEFT].velocity_command);
    double diff_speed_right = angularToLinear(joints_[RIGHT].velocity_command);

    limitDifferentialSpeed(diff_speed_left, diff_speed_right);

    imibot_driver::DiffSpeed msg;
    msg.left_speed = diff_speed_left * 100; // Read from velocity_joint_interface_
    msg.right_speed = diff_speed_right * 100;     // Read from velocity_joint_interface_

    this->cmd_pub.publish(msg);
  }

  /**
  * Scale left and right speed outputs to maintain ros_control's desired trajectory without saturating the outputs
  */
  void ImibotHardware::limitDifferentialSpeed(double &diff_speed_left, double &diff_speed_right)
  {
    double large_speed = std::max(std::abs(diff_speed_left), std::abs(diff_speed_right));

    if (large_speed > max_speed_)
    {
      diff_speed_left *= max_speed_ / large_speed;
      diff_speed_right *= max_speed_ / large_speed;
    }
  }

  /**
  * Husky reports travel in metres, need radians for ros_control RobotHW
  */
  double ImibotHardware::linearToAngular(const double &travel) const
  {
    return travel / wheel_diameter_ * 2;
  }

  /**
  * RobotHW provides velocity command in rad/s, Husky needs m/s,
  */
  double ImibotHardware::angularToLinear(const double &angle) const
  {
    return angle * wheel_diameter_ / 2;
  }

}


#include <iostream>
#include <fstream>
#include <armadillo>
#include <math.h>
#include <mpi.h>
#include <sys/time.h>
#include "sys/stat.h"

using namespace arma;

#include "class_data.hpp"
#include "constants.hpp"

//Implement the member function of Data, load_data, which loads data from file with name DATA_FILE_NAME. The data file is assumed to begin with the number of locations (type: unsigned 64-bit integer), followed by an array of longitudes, latitudes, and observations (type: 64-bit real numbers)
void Data::load_data()
{
    if(WORKER == 0)
	{
		gettimeofday(&timeNow, NULL);
		cout<<"===========================> Processor 1: loading data starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
	}

    //Check data file exists
    struct stat fileStatus;
    if(stat(DATA_FILE_NAME.c_str(),&fileStatus) == -1 || !S_ISREG(fileStatus.st_mode))
    {
        if(WORKER == 0) cout<<"Program exits with an error: the data file "<<DATA_FILE_NAME<<" does not exist.\n";
        MPI_Barrier(MPI_COMM_WORLD);
        MPI_Finalize();
        exit(EXIT_FAILURE);
    }

    //Open the data file by DATA_FILE_NAME
    MPI_File file;
    MPI_File_open(MPI_COMM_WORLD,DATA_FILE_NAME.c_str(),MPI_MODE_RDONLY,MPI_INFO_NULL,&file);

    //Read data
    //longitude, latitude, and values of all data points, including observations that have valid values and predicitons that have NaN values
    double *lon, *lat, *values;

    //Get the number of locations, nLocations
    MPI_Status* readStatus = new MPI_Status;
    int count;

    nLocations=0;
    MPI_File_read(file,(void*)&nLocations,1,MPI_UNSIGNED_LONG,readStatus);
    MPI_Get_count(readStatus,MPI_UNSIGNED_LONG,&count);

    if(count!=1)
    {
        if(WORKER == 0) cout<<"Program exits with an error: fail reading data file "<<DATA_FILE_NAME<<" for the number of locations. Double check the data file format."<<endl;
        MPI_Barrier(MPI_COMM_WORLD);
        MPI_Finalize();
        exit(EXIT_FAILURE);
    }

    lon = new double [nLocations];
    lat = new double [nLocations];
    values = new double [nLocations];

    //Get lon
    int interval=INT_MAX;
    unsigned long positionStart = 0, positionEnd = positionStart + interval;
    while(positionStart<nLocations)
    {
        if(positionEnd>nLocations) interval = nLocations - positionStart;

        MPI_File_read(file,(void*)&lon[positionStart],interval,MPI_DOUBLE,readStatus);
        MPI_Get_count(readStatus,MPI_DOUBLE,&count);

        positionStart = positionEnd;
        positionEnd += interval;
        if(count!=interval)
        {
            if(WORKER == 0) cout<<"Program exits with an error: fail reading data file "<<DATA_FILE_NAME<<" for longitude. Double check the data file format."<<endl;
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
        }
    }

    //Get lat
    interval=INT_MAX;
    positionStart = 0, positionEnd = positionStart + interval;
    while(positionStart<nLocations)
    {
        if(positionEnd>nLocations) interval = nLocations - positionStart;
        MPI_File_read(file,(void*)&lat[positionStart],interval,MPI_DOUBLE,readStatus);
        MPI_Get_count(readStatus,MPI_DOUBLE,&count);

        positionStart = positionEnd;
        positionEnd += interval;
        if(count!=interval)
        {
            if(WORKER == 0) cout<<"Program exits with an error: fail reading data file "<<DATA_FILE_NAME<<" for latitude. Double check the data file format."<<endl;
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
        }
    }

    //Get values
    positionStart = 0, positionEnd = positionStart + interval;
    while(positionStart<nLocations)
    {
        if(positionEnd>nLocations) interval = nLocations - positionStart;
        MPI_File_read(file,(void*)&values[positionStart],interval,MPI_DOUBLE,readStatus);
        MPI_Get_count(readStatus,MPI_DOUBLE,&count);

        positionStart = positionEnd;
        positionEnd += interval;
        if(count!=interval)
        {
            if(WORKER == 0) cout<<"Program exits with an error: fail reading data file "<<DATA_FILE_NAME<<" for observation values. Double check the data file format."<<endl;
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
        }
    }

    //Close the data file
    MPI_File_close(&file);

    //Get min and max
    double minLon = 1e9, minLat = 1e9, maxLon = -1e9, maxLat = -1e9;
    #pragma omp parallel reduction(min:minLon,minLat) reduction(max:maxLon,maxLat)
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
    {
        minLon=std::min(minLon,lon[iLocation]);
        minLat=std::min(minLat,lat[iLocation]);
        maxLon=std::max(maxLon,lon[iLocation]);
        maxLat=std::max(maxLat,lat[iLocation]);
    }

    //Assign the boundry, domainBoundaries
    domainBoundaries[0] = minLon;
    domainBoundaries[1] = maxLon+0.1*(maxLon-minLon);
    domainBoundaries[2] = minLat;
    domainBoundaries[3] = maxLat+0.1*(maxLat-minLat);

    //Split the data into observations and missing values
    numObservationsInRawData = 0;
    for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        if(!::isnan(values[iLocation])) numObservationsInRawData++;

    observationLon = new double [numObservationsInRawData];
    observationLat = new double [numObservationsInRawData];
    double *observationValues = new double [numObservationsInRawData];

    numPredictionsInRawData=nLocations;
    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='N')
    {
        predictionLon = new double [numPredictionsInRawData];
        predictionLat = new double [numPredictionsInRawData];

        unsigned long observationCount=0, predictionCount=0;
        for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        {
            if(!::isnan(values[iLocation]))
            {
                observationLon[observationCount]=lon[iLocation];
                observationLat[observationCount]=lat[iLocation];
                observationValues[observationCount++]=values[iLocation];
            }else
            {
                predictionLon[predictionCount]=lon[iLocation];
                predictionLat[predictionCount++]=lat[iLocation];
            }
        }
        numPredictionsInRawData = predictionCount;
    }else
    {
        unsigned long observationCount=0, predictionCount=0;
        for(unsigned long iLocation = 0; iLocation < nLocations; iLocation++)
        {
            if(!::isnan(values[iLocation]))
            {
                observationLon[observationCount]=lon[iLocation];
                observationLat[observationCount]=lat[iLocation];
                observationValues[observationCount++]=values[iLocation];
            }
        }
    }

    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='D')
    {
        predictionLon = lon;
        predictionLat = lat;
    }else
    {
        delete[] lon;
        delete[] lat;
    }

    if(CALCULATION_MODE=="prediction" && PREDICTION_LOCATION_MODE=='A')
    {
        if(stat(PREDICTION_LOCATION_FILE.c_str(),&fileStatus) == -1 || !S_ISREG(fileStatus.st_mode))
        {
            if(WORKER == 0) cout<<"Program exits with an error: the prediction location file "<<PREDICTION_LOCATION_FILE<<" does not exist.\n";
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
        }

        //Open the prediciton location file by PREDICTION_LOCATION_FILE
        MPI_File_open(MPI_COMM_WORLD,PREDICTION_LOCATION_FILE.c_str(),MPI_MODE_RDONLY,MPI_INFO_NULL,&file);

        //Read data
        MPI_File_read(file,(void*)&numPredictionsInRawData,1,MPI_UNSIGNED_LONG,readStatus);
        MPI_Get_count(readStatus,MPI_UNSIGNED_LONG,&count);

        if(count!=1)
        {
            if(WORKER == 0) cout<<"Program exits with an error: fail reading data file "<<PREDICTION_LOCATION_FILE<<" for the number of locations. Double check the data file format."<<endl;
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
        }

        predictionLon = new double [numPredictionsInRawData];
        predictionLat = new double [numPredictionsInRawData];

        //Get prediction lon
        unsigned long positionStart = 0, positionEnd = positionStart + interval;
        while(positionStart<numPredictionsInRawData)
        {
            if(positionEnd>numPredictionsInRawData) interval = numPredictionsInRawData - positionStart;

            MPI_File_read(file,(void*)&predictionLon[positionStart],interval,MPI_DOUBLE,readStatus);
            MPI_Get_count(readStatus,MPI_DOUBLE,&count);

            positionStart = positionEnd;
            positionEnd += interval;
            if(count!=interval)
            {
                if(WORKER == 0) cout<<"Program exits with an error: fail reading prediction location file "<<PREDICTION_LOCATION_FILE<<" for longitude. Double check the data file format."<<endl;
                MPI_Barrier(MPI_COMM_WORLD);
                MPI_Finalize();
                exit(EXIT_FAILURE);
            }
        }

        //Get prediction lat
        positionStart = 0, positionEnd = positionStart + interval;
        while(positionStart<numPredictionsInRawData)
        {
            if(positionEnd>numPredictionsInRawData) interval = numPredictionsInRawData - positionStart;
            MPI_File_read(file,(void*)&predictionLat[positionStart],interval,MPI_DOUBLE,readStatus);
            MPI_Get_count(readStatus,MPI_DOUBLE,&count);

            positionStart = positionEnd;
            positionEnd += interval;
            if(count!=interval)
            {
                if(WORKER == 0) cout<<"Program exits with an error: fail reading prediction location file "<<PREDICTION_LOCATION_FILE<<" for latitude. Double check the data file format."<<endl;
                MPI_Barrier(MPI_COMM_WORLD);
                MPI_Finalize();
                exit(EXIT_FAILURE);
            }
        }

        //Close the prediction location file
        MPI_File_close(&file);
    }

    delete[] values;

    //Eliminate duplicates in the raw data
    if(ELIMINATION_DUPLICATES_FLAG)
        eliminate_duplicate(observationValues);
    else
    {
        NUM_OBSERVATIONS = numObservationsInRawData;
        NUM_PREDICTIONS = numPredictionsInRawData;
    }

    //Detrend data by a linear regression to lon, lat and a constant
    mat *desigenMatrix = new mat(NUM_OBSERVATIONS,3,fill::ones);

    vec *lonVector = new vec(observationLon,NUM_OBSERVATIONS,false,true);
    vec *latVector = new vec(observationLat,NUM_OBSERVATIONS,false,true);
    vec *valueVector = new vec(observationValues,NUM_OBSERVATIONS,false,true);

    (*desigenMatrix).col(1)=(*lonVector);
    (*desigenMatrix).col(2)=(*latVector);

    coefficients = solve(*desigenMatrix,*valueVector);

    vec *residualVector = new vec;
    *residualVector = (*valueVector) - (*desigenMatrix) * coefficients;

    delete[] observationValues;

    observationResiduals = new double [NUM_OBSERVATIONS];
    for(unsigned long iObservation = 0; iObservation < NUM_OBSERVATIONS; iObservation++) observationResiduals[iObservation] = (*residualVector)(iObservation);

    delete lonVector;
    delete latVector;
    delete valueVector;
    delete desigenMatrix;
    delete residualVector;
    delete readStatus;

    if(WORKER == 0)
	{
		gettimeofday(&timeNow, NULL);
		cout<<"===========================> Processor 1: loading data is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
		if(PRINT_DETAIL_FLAG) print_data_summary();
	}
}

//Implement the member function of Data, print_data_summary, which prints a brief summary of the data
void Data::print_data_summary()
{
    cout<<">>The number of locations: "<<nLocations<<endl<<endl;
    cout<<">>The number of observations in the raw data: "<<numObservationsInRawData<<endl<<endl;
    if(ELIMINATION_DUPLICATES_FLAG) cout<<">>The number of observations after eliminating duplicates: "<<NUM_OBSERVATIONS<<endl<<endl;

    cout<<">>The boundry of the spatial domain"<<endl;
    cout<<"  longitude: minimal "<<domainBoundaries[0]<<", maximal "<<domainBoundaries[1]<<endl;
    cout<<"  latitude : minimal "<<domainBoundaries[2]<<", maximal "<<domainBoundaries[3]<<endl<<endl;

    cout<<">>Examples of observations"<<endl;
    cout<<"     longitude: ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationLon[iObs]);
    cout<<"\n     latitude:  ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationLat[iObs]);
    cout<<"\n     residuals: ";
    for(unsigned long iObs = 0; iObs < std::min(NUM_OBSERVATIONS,(unsigned long)5); iObs++)
        printf("%8.2lf ",observationResiduals[iObs]);
    cout<<"\n\n";

    if(CALCULATION_MODE=="prediction")
    {
        cout<<">>The number of prediction locations in the raw data: "<<numPredictionsInRawData<<endl<<endl;
        if(ELIMINATION_DUPLICATES_FLAG) cout<<">>The number of prediction locations after eliminating duplicates: "<<NUM_PREDICTIONS<<endl<<endl;

        cout<<">>Examples of prediction locations"<<endl;
        cout<<"     longitude: ";
        for(unsigned long iPred = 0; iPred < std::min(NUM_PREDICTIONS,(unsigned long)5); iPred++)
            printf("%8.2lf ",predictionLon[iPred]);
        cout<<"\n     latitude:  ";
        for(unsigned long iPred = 0; iPred < std::min(NUM_PREDICTIONS,(unsigned long)5); iPred++)
            printf("%8.2lf ",predictionLat[iPred]);
        cout<<"\n\n";
    }
}

//Implement the function that eliminate duplicate observations and predictions
void Data::eliminate_duplicate(double *& observationValues)
{
    struct compare
    {
        Data* data;
        compare(Data *data): data(data) {}
        bool operator() (unsigned long i, unsigned long j) const
        {
            if(data->observationLon[i]<data->observationLon[j]) return true;
            if(data->observationLon[i]==data->observationLon[j]) return data->observationLat[i]<data->observationLat[j];
            return false;
        }
    };

    struct prediction_compare
    {
        Data* data;
        prediction_compare(Data *data): data(data) {}
        bool operator() (unsigned long i, unsigned long j) const
        {
            if(data->predictionLon[i]<data->predictionLon[j]) return true;
            if(data->predictionLon[i]==data->predictionLon[j]) return data->predictionLat[i]<data->predictionLat[j];
            return false;
        }
    };

    unsigned long *tmpIndex = new unsigned long [numObservationsInRawData];
    for(unsigned long index = 0; index < numObservationsInRawData; index++) tmpIndex[index]=index;

    std::sort(tmpIndex,tmpIndex+numObservationsInRawData, compare(this));

    bool *duplicateFlag = new bool [numObservationsInRawData]();

    NUM_OBSERVATIONS = numObservationsInRawData;

    for(unsigned long index = 1; index < numObservationsInRawData; index++)
        if((observationLon[tmpIndex[index]]==observationLon[tmpIndex[index-1]]) && (observationLat[tmpIndex[index]]==observationLat[tmpIndex[index-1]]))
        {
            duplicateFlag[index] = true;
            NUM_OBSERVATIONS--;
        }

    double *tmpObservationLon, *tmpObservationLat, *tmpObservationValues;
    tmpObservationLon = new double [NUM_OBSERVATIONS];
    tmpObservationLat = new double [NUM_OBSERVATIONS];
    tmpObservationValues = new double [NUM_OBSERVATIONS];

    unsigned long count=0;
    for(unsigned long index = 0; index < numObservationsInRawData; index++)
    {
        if(duplicateFlag[index]) continue;
        tmpObservationLon[count]=observationLon[tmpIndex[index]];
        tmpObservationLat[count]=observationLat[tmpIndex[index]];
        tmpObservationValues[count++]=observationValues[tmpIndex[index]];
    }

    delete[] observationLon;
    delete[] observationLat;
    delete[] observationValues;

    observationLon=tmpObservationLon;
    observationLat=tmpObservationLat;
    observationValues=tmpObservationValues;

    delete[] tmpIndex;
    delete[] duplicateFlag;

    if(CALCULATION_MODE=="prediction")
    {
        unsigned long *tmpIndex = new unsigned long [numPredictionsInRawData];
        for(unsigned long index = 0; index < numPredictionsInRawData; index++) tmpIndex[index]=index;

        std::sort(tmpIndex,tmpIndex+numPredictionsInRawData, prediction_compare(this));

        bool *duplicateFlag = new bool [numPredictionsInRawData]();

        NUM_PREDICTIONS = numPredictionsInRawData;

        for(unsigned long index = 1; index < numPredictionsInRawData; index++)
            if((predictionLon[tmpIndex[index]]==predictionLon[tmpIndex[index-1]]) && (predictionLat[tmpIndex[index]]==predictionLat[tmpIndex[index-1]]))
            {
                duplicateFlag[index] = true;
                NUM_PREDICTIONS--;
            }

        double *tmpPredictionLon, *tmpPredictionLat;
        tmpPredictionLon = new double [NUM_PREDICTIONS];
        tmpPredictionLat = new double [NUM_PREDICTIONS];

        unsigned long count=0;
        for(unsigned long index = 0; index < numPredictionsInRawData; index++)
        {
            if(duplicateFlag[index]) continue;
            tmpPredictionLon[count]=predictionLon[tmpIndex[index]];
            tmpPredictionLat[count++]=predictionLat[tmpIndex[index]];
        }

        delete[] predictionLon;
        delete[] predictionLat;

        predictionLon=tmpPredictionLon;
        predictionLat=tmpPredictionLat;

        delete[] tmpIndex;
        delete[] duplicateFlag;
    }
}

//Implement the destructor of class Data
Data::~Data()
{
    //delete[] predictionLon;
    //delete[] predictionLat;

    //cout<<"Data is deleted.\n";
}


#include <aslam/backend/JacobianContainer.hpp>
#include <aslam/backend/MEstimatorPolicies.hpp>
#include <boost/shared_ptr.hpp>
#include <numpy_eigen/boost_python_headers.hpp>
using namespace boost::python;
using namespace aslam::backend;

DesignVariable* (JacobianContainer::*jc_dvptr)(size_t) = &JacobianContainer::designVariable;

/// \brief Add the rhs container to this one.
void (JacobianContainer::*jc_add1)(const JacobianContainer&) = &JacobianContainer::add;

void jc_add2(JacobianContainer* jc, DesignVariable& dv, Eigen::MatrixXd Jacobian) { jc->add(&dv, Jacobian); }

SparseBlockMatrix (JacobianContainer::*jc_asm1)() const = &JacobianContainer::asSparseMatrix;

SparseBlockMatrix jc_asm2(JacobianContainer* jc, const Eigen::VectorXi& colBlockIndices) {
    std::vector<int> cbi;
    cbi.insert(cbi.begin(), &colBlockIndices[0], &colBlockIndices[0] + colBlockIndices.size());
    return jc->asSparseMatrix(cbi);
}

Eigen::MatrixXd (JacobianContainer::*jc_adm1)() const = &JacobianContainer::asDenseMatrix;

Eigen::MatrixXd jc_adm2(JacobianContainer* jc, const Eigen::VectorXi& colBlockIndices) {
    std::vector<int> cbi;
    cbi.insert(cbi.begin(), &colBlockIndices[0], &colBlockIndices[0] + colBlockIndices.size());
    return jc->asDenseMatrix(cbi);
}

void exportJacobianContainer() {
    class_<JacobianContainer>("JacobianContainer", init<int>())

        // void evaluateHessian(const Eigen::VectorXd & e, const Eigen::MatrixXd & invR, SparseBlockMatrix & outHessian,
        // Eigen::VectorXd & outRhs) const;

        .def("add", jc_add1)
        .def("add", jc_add2)

        /// \brief how many design variables does this jacobian container represent.
        .def("numDesignVariables", &JacobianContainer::numDesignVariables)

        /// \brief Get design variable i.
        .def("designVariable", jc_dvptr, return_internal_reference<>())

        /// Get the Jacobian associated with a particular design variable.
        .def("Jacobian", &JacobianContainer::Jacobian, return_value_policy<copy_const_reference>())

        /// \brief How many rows does this set of Jacobians have?
        .def("rows", &JacobianContainer::rows)

        /// \brief Apply the chain rule to the set of Jacobians.
        /// This may change the number of rows of this set of Jacobians
        /// by multiplying through by df_dx on the left.
        .def("applyChainRule", &JacobianContainer::applyChainRule)

        /// \brief Clear the contents of this container
        .def("clear", &JacobianContainer::clear)

        /// \brief Clear the contents of this container
        .def("reset", &JacobianContainer::reset)

        /// \brief Gets a sparse matrix with the Jacobians. The matrix is, in fact, dense
        ///        and the Jacobian ordering matches the sort order.
        ///
        ///        \todo add a function that allows us to get the full block row
        ///        Jacobian. This is tricky because you have to know the
        ///        full block structure of the Hessian
        .def("asSparseMatrix", jc_asm1)
        /// \brief Get a sparse matrix with the Jacobians. This uses the column block indices
        ///        to build the sparse, full width jacobian matrix
        .def("asSparseMatrix", jc_asm2)

        .def("asDenseMatrix", jc_adm1)
        .def("asDenseMatrix", jc_adm2)

        /// The number of columns in the compressed Jacobian. Warning: this is expensive.
        .def("cols", &JacobianContainer::cols);
}


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2007 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/AbstractSQE.h"
#include "mccomponents/kernels/sample/SQE/GridSQE.h"

namespace wrap_mccomponents {

  using namespace mccomponents::sample;

  double fxy_get( const fxy & fxy, const double & x, const double & y)
  {
    return fxy(x,y);
  }

  fxy * new_fxy( double xbegin, double xend, double xstep,
		 double ybegin, double yend, double ystep,
		 std::vector<double> & v )
  {
    return new fxy( xbegin, xend, xstep,
		    ybegin, yend, ystep,
		    v.begin() );
  }

  void wrap_GridSQE()
  {
    using namespace boost::python;

    typedef GridSQE w_t;

    class_<w_t, bases<mccomponents::sample::AbstractSQE>, boost::noncopyable >
      ("GridSQE",
       init<const fxy &>()
       [with_custodian_and_ward<1,2> () ]
       )
      ;

    class_<fxy>
      ("fxy", no_init)
      .def( "__call__", fxy_get)
      ;

    def( "new_fxy", new_fxy,
	 with_custodian_and_ward_postcall<0,7,
	 return_value_policy< manage_new_object > > () )
      ;

  }

}

// version
// $Id$

// End of file


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include <ctime>
#include <iostream>
#include <iomanip>
#include <limits>
#include <unordered_map>
#include <hash_map>
#include <vector>
#include <map>
#include <sstream>
#include <string>

#include <boost/unordered_map.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

#include <scm/core.h>
#include <scm/core/time/high_res_timer.h>

struct somesome {
    unsigned a;
    unsigned b;
    bool operator == (const somesome& rhs) const {
        return (a == rhs.a && b == rhs.b);
    }
};

std::size_t hash_value(const somesome& s) {
    scm::size_t seed = 0;

    boost::hash_combine(seed, s.a);
    boost::hash_combine(seed, s.b);

    return (seed);
}

struct hasher_some {
    std::size_t operator()(const somesome& s) const {
        scm::size_t seed = 0;

        boost::hash_combine(seed, s.a);
        boost::hash_combine(seed, s.b);

        return (seed);
    }
};

//typedef std::string key_type;
//typedef std::size_t key_type;
typedef somesome key_type;

std::vector<key_type> keys;

template<typename T>
void timemap(const int n, const int r=7, bool single_results = false) {
	std::map<std::string, std::vector<double> > timings;

    scm::time::high_res_timer timer;

	for (int j=0 ; j<r ; ++j) {
		T hsh;

		timer.start();
		// Insert every 2nd number in the sequence
		for (int i = 0; i < n; i+=2) {
			hsh[keys[i]] = i*2;
        }
        timer.stop();
        timings["1: Insert"].push_back(scm::time::to_milliseconds(timer.get_time()));

		timer.start();
		// Lookup every number in the sequence
		for (int i = 0; i < n; ++i) {
			hsh.find(keys[i]);
		}
        timer.stop();
		timings["2: Lookup"].push_back(scm::time::to_milliseconds(timer.get_time()));

		timer.start();
		// Iterate over the entries
		for (typename T::iterator it = hsh.begin(); it != hsh.end(); ++it) {
			int x = it->second;
			++x;
		}
        timer.stop();
		timings["3: Iterate"].push_back(scm::time::to_milliseconds(timer.get_time()));

		timer.start();
		// Erase the entries
		for (int i = 0; i < n; i+=2) {
			hsh.erase(keys[i]);
		}
        timer.stop();
		timings["4: Erase"].push_back(scm::time::to_milliseconds(timer.get_time()));
	}

	for (std::map<std::string,std::vector<double> >::iterator it=timings.begin() ; it!=timings.end() ; ++it) {
		double sum = 0.0;
		std::cout << it->first << "\t(";
	    for (int i = 1; i < r; ++i) {
	    	sum += it->second.at(i);
            if (single_results) {
		    	std::cout << std::fixed << std::setprecision(4) << it->second.at(i) << "msec";
                if (i != r-1) {
                    std::cout << " ";
                }
		    }
        }
		std::cout << ") " << sum/double(r-1) << "msec" << std::endl;
	}
}

int main(int argc, char **argv)
{
#ifndef NDEBUG
    std::cout << "Debug" << std::endl;
#else
    std::cout << "Release" << std::endl;
#endif
    //std::cout << "IDL: " << _ITERATOR_DEBUG_LEVEL << std::endl;
    std::cout << "SCL: " << _SECURE_SCL << std::endl;
    std::cout << "HID: " << _HAS_ITERATOR_DEBUGGING << std::endl;

    std::wcout << "SCL: " << _SECURE_SCL << std::endl;
    std::wcout << "HID: " << _HAS_ITERATOR_DEBUGGING << std::endl;

    scm::shared_ptr<scm::core>      scm_core(new scm::core(argc, argv));

    std::string alpha="abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890";
	//string alpha="ABCDEFGHIJKLMNOPQRSTUVWXYZ";

    boost::mt19937          generator(static_cast<unsigned int>(std::time(0)));
	boost::uniform_int<>    uni_int_dist(0, alpha.length());
	boost::variate_generator<boost::mt19937&, boost::uniform_int<> > char_gen(generator, uni_int_dist);

	boost::uniform_int<>    uni_int_dist2(5, 10);
	boost::variate_generator<boost::mt19937&, boost::uniform_int<> > len_gen(generator, uni_int_dist2);

    boost::uniform_int<>    uni_int_dist3(0, (std::numeric_limits<std::size_t>::max)());
	boost::variate_generator<boost::mt19937&, boost::uniform_int<> > num_gen(generator, uni_int_dist3);

	int n = 10000;
    int i = 10000;
	keys.reserve(n);
	keys.resize(n);
	std::stringstream ss;
	for (int i=0 ; i<n ; ++i) {
#if 0
        keys[i].clear();

        int l = len_gen();
        for (int k = 0; k < l; ++k) {
            keys[i].push_back(alpha[char_gen()]);
        }
        //std::cout << strings[i] << std::endl;
        assert(strings[i].size() == l);
#elif 0
        keys[i] = num_gen();
#else
        keys[i].a = num_gen();
        keys[i].b = num_gen();
#endif
	}

	std::cout << "Timing boost::unordered_map<std::string, int>" << std::endl;
	timemap<boost::unordered_map<key_type, int> >(n, i);
	std::cout << std::endl;

	std::cout << "Timing std::tr1::unordered_map<std::string, int>" << std::endl;
	timemap<std::unordered_map<key_type, int, hasher_some> >(n, i);
	std::cout << std::endl;

	//std::cout << "Timing stdext::hash_map<std::string, int>" << std::endl;
	//timemap<stdext::hash_map<key_type, int> >(n, i);
	//std::cout << std::endl;

	//std::cout << "Timing std::map<std::string, int>" << std::endl;
	//timemap<std::map<key_type, int> >(n, i);
	//std::cout << std::endl;
}


#include <boost/hana/integral_constant.hpp>


//  Copyright (c) 2010 Lars Kielhorn
//  Copyright (c) 2001-2010 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config/warning_disable.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/karma.hpp>

#include <iostream>
#include <string>
#include <iterator>

namespace karma = boost::spirit::karma;

// define a new real number formatting policy
template <typename Num>
struct scientific_policy : karma::real_policies<Num>
{
    // we want the numbers always to be in scientific format
    static int floatfield(Num) { return std::ios_base::scientific; }
};

int main()
{
    // define a new generator type based on the new policy
    typedef karma::real_generator<double, scientific_policy<double> >
        science_type;
    science_type const scientific = science_type();

    std::string output;
    typedef std::back_insert_iterator<std::string> output_iterator;
    output_iterator sink(output);

    // should output: 1.0e-01, but will output: 10.0e-02
    BOOST_TEST(karma::generate(sink, scientific, 0.1) && output == "1.0e-01");

    return boost::report_errors();
}