#include "UGCPopularity.hpp"
#include "ContentElement.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/gamma_distribution.hpp>
#include "boost/random/uniform_real_distribution.hpp"
#include <boost/math/distributions/lognormal.hpp>
#include "boost/random/uniform_int.hpp"
#include <algorithm>

extern boost::mt19937 gen;

UGCPopularity::UGCPopularity(unsigned int totalRounds,
        bool perturbations) {
  this->totalRounds = totalRounds;
  this->ttpDist = new boost::math::ttpDistribution(totalRounds);
  this->perturbations = perturbations;
}

UGCPopularity::~UGCPopularity() {
  delete ttpDist;
}

// generate views for all the content elements at a given phase for this round only
unsigned int UGCPopularity::generateViews(std::list<ContentElement*> contentList,
      peakingPhase phase) {
  unsigned int bodyCount, tailCount;
  tailCount = std::floor(contentList.size() / 10);
  bodyCount = contentList.size() - tailCount;
  std::list<unsigned int> views;
  if (phase == BEFORE_PEAK) {
    for (unsigned int i = 0; i < tailCount; i++) {
      views.push_back(this->getBeforePeakTailViews());
    }
    for (unsigned int i = 0; i < bodyCount; i++) {
      views.push_back(this->getBeforePeakViews());
    }
  } else if (phase == AT_PEAK) {
    for (unsigned int i = 0; i < tailCount; i++) {
      views.push_back(this->getAtPeakTailViews());
    }
    for (unsigned int i = 0; i < bodyCount; i++) {
      views.push_back(this->getAtPeakViews());
    }
  } else { // phase == AFTER_PEAK
    for (unsigned int i = 0; i < tailCount; i++) {
      views.push_back(this->getAfterPeakTailViews());
    }
    for (unsigned int i = 0; i < bodyCount; i++) {
      views.push_back(this->getAfterPeakViews());
    }
  }
  // sort the views in descending order and assign them to the contentList
  // (which is ordered by ranking) so that relative ranking remains unaltered
  views.sort(std::greater<unsigned int>());
  std::list<unsigned int>::iterator viewsIt, mIt;

  // introduce perturbations in the number of views, by switching views between
  // videos in the same phase and at a maximum distance g
  if (this->perturbations && phase != AT_PEAK) {
    uint g = 12; // suggested value from Borghol et al.
    std::vector<std::pair<uint, uint> > winLimits;
    uint minWin, maxWin;
    uint maxViews = views.front();
    // compute valid switching windows for each content
    BOOST_FOREACH(uint v, views) {
      minWin = std::floor(v / g);
      maxWin = std::min(maxViews, v * g);
      winLimits.push_back(std::make_pair(minWin,maxWin));
    }
    // switch views a "sufficiently large number of times" (!) between contents
    // with compatible switching windows

    // NOTE: the following code is so ugly it makes me want to kill kittens.
    // try to rewrite it in a half-decent way, ok?
    boost::random::uniform_int_distribution<> uniDist(0, contentList.size()-1);
    for (uint i = 0; i < contentList.size() / 3; i++) {
      uint randC = uniDist(gen);
      viewsIt = views.begin();
      for (uint j = 0; j < randC; j++)
        viewsIt++;
      mIt = viewsIt;
      int j = 0;
      while (mIt != views.begin() && *mIt >= winLimits[randC].first) {
        j--;
        mIt--;
      }
      if (j != 0) j++;
      mIt = viewsIt;
      int k = 0;
      while (mIt != views.end() && *mIt <= winLimits[randC].second) {
        mIt++;
        k++;
      }
      if (k != 0) k--;
      if (j != 0 || k != 0) {
        boost::random::uniform_int_distribution<> switchDist(0,k-j);
        j = switchDist(gen) + j;
        if (j < 0) {
          std::swap(winLimits[randC], winLimits[randC+j]);
          mIt = viewsIt;
          while (j < 0) {
            mIt--;
            j++;
          }
          std::iter_swap(viewsIt, mIt);
        }
        else if (j > 0) {
          std::swap(winLimits[randC], winLimits[randC+j]);
          mIt = viewsIt;
          while (j > 0) {
            mIt++;
            j--;
          }
          std::iter_swap(viewsIt, mIt);
        }
      }
    }
  }
  // assign views in the intended order (sorted or permuted)
  uint totalViews = 0;
  viewsIt = views.begin();
  BOOST_FOREACH (ContentElement* content, contentList) {
    content->setViewsThisRound(*viewsIt);
    totalViews += *viewsIt;
    viewsIt++;
  }
  return totalViews;
}

// Returns the round at which a content will peak in terms of views
unsigned int UGCPopularity::generatePeakRound() {
  boost::uniform_01<> uni;
  double randVal = uni(gen);
//  std::cout << "Uniform Value: " << randVal << std::endl;
  return boost::math::quantile(*ttpDist, randVal);
}

unsigned int UGCPopularity::getBeforePeakTailViews() {
  boost::math::lognormal logDist(2.000, 2.135);
  boost::random::uniform_real_distribution<> uniDist(0.903, 1);
  double randValue = uniDist(gen);
  double retValue = boost::math::quantile(logDist, randValue);
  return static_cast<unsigned int>(0.5+retValue);
}

unsigned int UGCPopularity::getAtPeakTailViews() {
  boost::math::lognormal logDist(-3.826, 3.477);
  boost::random::uniform_real_distribution<> uniDist(0.997, 1);
  double randValue = uniDist(gen);
  double retValue = boost::math::quantile(logDist, randValue);
  return static_cast<unsigned int>(0.5+retValue);
}

unsigned int UGCPopularity::getAfterPeakTailViews() {
  boost::math::lognormal logDist(-0.356, 2.533);
  boost::random::uniform_real_distribution<> uniDist(0.931, 1);
  double randValue = uniDist(gen);
  double retValue = boost::math::quantile(logDist, randValue);
  return static_cast<unsigned int>(0.5+retValue);
}

unsigned int UGCPopularity::getBeforePeakViews() {
  const unsigned int xMin = 0, xThresh = 119;
  unsigned int retValue = 0;
  boost::uniform_01<> uniDist;
  double randVal = uniDist(gen);
  // std::cout << "Uniform Value: " << randVal << std::endl;
  boost::math::beta_distribution<> betaDist(0.191, 1.330);
  retValue = std::floor(0.5+xMin+(xThresh-xMin)*
          boost::math::quantile(betaDist, randVal));
  return retValue;
}

unsigned int UGCPopularity::getAtPeakViews() {
  const unsigned int xMin = 4, xThresh = 297;
  unsigned int retValue = 0;
  boost::uniform_01<> uniDist;
  double randVal = uniDist(gen);
  // std::cout << "Uniform Value: " << randVal << std::endl;
  boost::math::beta_distribution<> betaDist(0.543, 2.259);
  retValue = std::floor(0.5+xMin+(xThresh-xMin)*
            boost::math::quantile(betaDist, randVal));
  return retValue;
}

unsigned int UGCPopularity::getAfterPeakViews() {
  const unsigned int xMin = 0, xThresh = 30;
  unsigned int retValue = 0;
  boost::uniform_01<> uniDist;
  double randVal = uniDist(gen);
  // std::cout << "Uniform Value: " << randVal << std::endl;
  boost::math::beta_distribution<> betaDist(0.077, 0.968);
  retValue = std::floor(0.5+xMin+(xThresh-xMin)*
            boost::math::quantile(betaDist, randVal));
  return retValue;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/split_high.hpp>
#include <boost/simd/function/split_low.hpp>
#include <array>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( split_
                            , (typename A0, typename X)
                            , (bd::is_upgradable<A0>)
                            , bd::cpu_
                            , bs::pack_<bd::unspecified_<A0>, X>
                            )
  {
    using result_t = std::array<bd::upgrade_t<A0>,2>;

    BOOST_FORCEINLINE result_t operator()(A0 const& a) const BOOST_NOEXCEPT
    {
      return { split_low(a), split_high(a) };
    }
  };
} } }

#endif


#include <iostream>
#include <thread>
#include <chrono>
#include <fstream>
#include <vector>
#include <map>
#include <thread>
#include <chrono>

#include <experimental/filesystem>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/surface/vtk_smoothing/vtk_utils.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/concave_hull.h>

#include <Eigen/Geometry>

#include "utils.h"
#include "json.h"
#include "MapManager.h"

typedef pcl::PointXYZRGB PointType;
using namespace std;
using namespace pcl;
using namespace pcl::visualization;
using namespace lass;
using json = nlohmann::json;
namespace fs = std::experimental::filesystem;
using namespace Eigen;

void load_and_preserve_info_file(const char* fn, vector<Quaternionf>& qs, vector<Vector3f>& ts, vector<vector<float>>& is) {
    ifstream ifs(fn, std::ios::in | std::ios::binary);
    if (!ifs) {
        cout << "Cannot read file" << endl;
        exit(0);
    }

    uint32_t n_cameras;
    ifs.read((char*) &n_cameras, sizeof(u_int32_t));
    cout << "n_cameras = " << n_cameras << endl;
    qs.resize(n_cameras);
    ts.resize(n_cameras);
    is.resize(n_cameras, vector<float>(5));

    for (uint32_t i = 0; i < n_cameras; i++) {
        double focal_length, kappa_1, kappa_2;
        int32_t width, height;
        double *r = new double[9];
        double *t = new double[3];
        Eigen::Matrix3f rotation_matrix;

        ifs.read((char *)&focal_length, sizeof(double));
        ifs.read((char *)&kappa_1, sizeof(double));
        ifs.read((char *)&kappa_2, sizeof(double));
        ifs.read((char *)&width, sizeof(int32_t));
        ifs.read((char *)&height, sizeof(int32_t));
        ifs.read((char *)r, 9 * sizeof(double));
        ifs.read((char *)t, 3 * sizeof(double));

        auto& ii = is[i];
        ii[0] = focal_length;
        ii[1] = kappa_1;
        ii[2] = kappa_2;
        ii[3] = width;
        ii[4] = height;

        rotation_matrix <<
                r[0], r[1], r[2],
                r[3], r[4], r[5],
                r[6], r[7], r[8];

        auto& qi = qs[i];
        auto& ti = ts[i];
        qi = Quaternionf(rotation_matrix);
        qi.normalize();
        qi.y() = -qi.y();
        qi.z() = -qi.z();

        ti[0] = t[0];
        ti[1] = -t[1];
        ti[2] = -t[2];
    }
}

void load_and_preserve_list_file(const char *fn, int n_cameras, vector<string>& image_fns) {
    cout << "Start load list file" << endl;
    cout << n_cameras << endl;
    cout << fn << endl;

    ifstream ifs(fn);

    if (!ifs) {
        cout << "Cannot read file" << endl;
        exit(0);
    }
    image_fns.resize(n_cameras);
    for (int idx = 0; idx < n_cameras; idx++) {
        // string img_fn;
        ifs >> image_fns[idx];
        // image_fns[idx] = img_fn;
    }
}

void export_to_txt(const char *fn, vector<string>& image_fns, vector<Vector3f>& ts, vector<Quaternionf>& qs, vector<vector<float>>& is) {
    ofstream ofs(fn);

    const int n_cameras = image_fns.size();
    CHECK(ts.size() == n_cameras) << "ts inconsistent" << endl;
    CHECK(qs.size() == n_cameras) << "qs inconsistent" << endl;
    CHECK(is.size() == n_cameras) << "is inconsistent" << endl;

    ofs << "image_file_name, x, y, z, qw, qx, qy, qz, focal, k1, k2, width, height" << endl << endl;

    for (int idx = 0; idx < n_cameras; idx++) {
        ofs << image_fns[idx] << ' ';
        ofs << ts[idx][0] << ' ' << ts[idx][1] << ' ' << ts[idx][2] << ' ';
        ofs << qs[idx].w() << ' ' << qs[idx].x() << ' ' << qs[idx].y() << ' ' << qs[idx].z() << ' ';
        for (int i = 0; i < 5; i++) {
            ofs << is[idx][i] << ' ';
        }
        ofs << endl;
    }
}

void test_extrinsics(const string ply_fn, vector<Eigen::Vector3f>& ts, vector<Eigen::Quaternionf>& qs) {
    vector<Eigen::Matrix4f> es;
    PointCloud<PointXYZRGB>::Ptr pcd(new PointCloud<PointXYZRGB>);
    PointXYZRGB min3d, max3d;
    PointXYZ position;

    auto mm = make_unique<MapManager>();
    mm->load_ply_pcl(ply_fn);
    getMinMax3D(*mm->m_pcd, min3d, max3d);
    position.x = (min3d.x + max3d.x) / 2;
    position.y = (min3d.y + max3d.y) / 2;
    position.z = (min3d.z + max3d.z) / 2;

    mm->update_view();
    mm->m_viewer->setCameraPosition(position.x, position.y, position.z, 1, 0, 0);

    Eigen::Vector3f o, d, u;

    pcd->points.resize(ts.size() * 5);
    for (int idx = 0; idx < ts.size(); idx++) {
        Eigen::Matrix4f e;

        e.block<3, 3>(0, 0) = qs[idx].matrix();
        e.block<3, 1>(0, 3) = ts[idx];

        Eigen::Vector3f c_nvm = e.block(0, 3, 3, 1), t_e;
        Eigen::Matrix3f R_nvm = e.block(0, 0, 3, 3), R_e;

        // c_nvm(1) *= -1;
        // c_nvm(2) *= -1;
        // R_nvm(0, 1) *= -1;
        // R_nvm(0, 2) *= -1;
        // R_nvm(1, 0) *= -1;
        // R_nvm(2, 0) *= -1;

        c_nvm = -R_nvm.transpose() * c_nvm;
        d = R_nvm.transpose() * Eigen::Vector3f::UnitZ();

        auto &pt = pcd->points[idx * 5];
        pt.r = pt.g = pt.b = 200;
        pt.x = c_nvm(0);
        pt.y = c_nvm(1);
        pt.z = c_nvm(2);
        for (int s_idx = 1; s_idx < 5; s_idx++) {
            auto &pt = pcd->points[idx * 5 + s_idx];
            pt.g = pt.b = 0;
            pt.r = 200;

            pt.x = c_nvm(0) + d(0) * s_idx * 0.2;
            pt.y = c_nvm(1) + d(1) * s_idx * 0.2;
            pt.z = c_nvm(2) + d(2) * s_idx * 0.2;
        }
    }
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pcd);
    mm->m_viewer->addPointCloud(pcd, rgb, "camera poses");
    mm->m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "camera poses");
    mm->show_point_cloud();
}

void test_transfer_list(int argc, char** argv) {
    const char* info_fn = argv[1];
    const char* list_fn = argv[2];
    const char* txt_fn  = argv[3];
    const char* ply_fn  = argv[4];

    vector<Eigen::Quaternionf>  qs;
    vector<Eigen::Vector3f>     ts;
    vector<vector<float>>     is;
    vector<string>  image_fns;
    vector<int>     camera_types;

    load_and_preserve_info_file(info_fn, qs, ts, is);
    load_and_preserve_list_file(list_fn, qs.size(), image_fns);
    export_to_txt(txt_fn, image_fns, ts, qs, is);
    test_extrinsics(ply_fn, ts, qs);
}

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

    test_transfer_list(argc, argv);
}


/*
 * Copyright (C) 2019 Open Source Robotics Foundation
 *
 * 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.
 *
 */

#ifndef IGNITION_GAZEBO_SYSTEMS_MULTICOPTERVELOCITYCONTROL_PARAMETERS_HH_
#define IGNITION_GAZEBO_SYSTEMS_MULTICOPTERVELOCITYCONTROL_PARAMETERS_HH_

#include <Eigen/Geometry>
#include <vector>

#include "ignition/gazebo/config.hh"

namespace ignition
{
namespace gazebo
{
// Inline bracket to help doxygen filtering.
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
namespace systems
{
namespace multicopter_control
{
  /// \brief A struct that holds various properties of a rotor
  struct Rotor
  {
    /// \brief The angle formed on the xy plane by the vector going from the
    /// the center of mass to the rotor. The angle is measured from the +x axis
    /// of the body frame.
    // cppcheck-suppress unusedStructMember
    double angle;
    /// \brief The length of the vector going from the center of mass to the
    /// rotor.
    // cppcheck-suppress unusedStructMember
    double armLength;
    /// \brief A constant that multiplies with the square of the rotor's
    /// velocity to compute its thrust.
    // cppcheck-suppress unusedStructMember
    double forceConstant;
    /// \brief A constant the multiplies with the rotor's thrust to compute its
    /// moment.
    // cppcheck-suppress unusedStructMember
    double momentConstant;
    /// \brief Direction of rotation of the rotor. +1 is counterclockwise and -1
    /// is clockwise.
    // cppcheck-suppress unusedStructMember
    int direction;
  };

  /// \brief A collection of Rotor objects
  using RotorConfiguration = std::vector<Rotor>;

  /// \brief A struct that holds properties of the vehicle such as mass,
  /// inertia and rotor configuration. Gravity is also included even though it's
  /// not a parameter unique to the vehicle
  struct VehicleParameters
  {
    /// \brief Total mass of the vehicle
    // cppcheck-suppress unusedStructMember
    double mass;
    /// \brief Moment of inertia matrix of the vehicle
    Eigen::Matrix3d inertia;
    /// \brief Gravity vector
    Eigen::Vector3d gravity;
    /// \brief A collection of Rotor objects that specifiy various properties of
    /// the rotors in the vehicle
    RotorConfiguration rotorConfiguration;
  };

  /// \brief Noise parameters used when computing frame data. These are all
  /// assumed to be gaussian.
  struct NoiseParameters
  {
    Eigen::Vector3d linearVelocityMean;
    Eigen::Vector3d linearVelocityStdDev;
    Eigen::Vector3d angularVelocityMean;
    Eigen::Vector3d angularVelocityStdDev;
  };

}  // namespace multicopter_control
}  // namespace systems
}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
}  // namespace gazebo
}  // namespace ignition

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_SPLIT_HPP_INCLUDED

#ifdef BOOST_SIMD_HAS_AVX_SUPPORT
#include <boost/simd/swar/functions/split.hpp>
#include <boost/simd/sdk/simd/meta/retarget.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/fusion/tuple.hpp>

namespace boost { namespace simd { namespace ext
{
  //============================================================================
  // Implementation when type is single_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::split_
                                    , boost::simd::tag::avx_
                                    , (A0)(A1)
                                    , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                      ((simd_<double_<A1>,boost::simd::tag::avx_>))
                                      ((simd_<double_<A1>,boost::simd::tag::avx_>))
                                    )
  {
    typedef void result_type;

    BOOST_FORCEINLINE
    result_type operator()(const A0 & a0,  A1& a1, A1& a2) const
    {
      a1 = _mm256_cvtps_pd(_mm256_extractf128_ps(a0, 0)) ;
      a2 = _mm256_cvtps_pd(_mm256_extractf128_ps(a0, 1)) ;
    }
  };

  //============================================================================
  // Implementation when type is arithmetic_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::split_
                                      , boost::simd::tag::avx_
                                      , (A0)(A1)
                                      , (boost::mpl::and_
                                        < boost::mpl::not_
                                          < boost::is_same
                                            < A0
                                            , typename dispatch::meta::upgrade<A0>::type
                                            >
                                          >
                                        , boost::is_same
                                          < A1
                                          , typename dispatch::meta::upgrade<A0>::type
                                          >
                                        >
                                        )
                                      , ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                        ((simd_<arithmetic_<A1>,boost::simd::tag::avx_>))
                                        ((simd_<arithmetic_<A1>,boost::simd::tag::avx_>))
                                      )
  {
    typedef void result_type;
    typedef typename meta::retarget<A0,tag::sse_>::type A0_sse_;
    typedef typename meta::retarget<A1,tag::sse_>::type A1_sse_;

    BOOST_FORCEINLINE
    result_type operator()(const A0 & a0,  A1& a1, A1& a2)const
    {
      A0_sse_ a00 = _mm256_extractf128_si256(a0, 0);
      A0_sse_ a01 = _mm256_extractf128_si256(a0, 1);

      A1_sse_ ha000, ha001;
      split(a00, ha000, ha001);
      a1 = _mm256_castsi128_si256(ha000);
      a1 = _mm256_insertf128_si256(a1, ha001, 1);

      A1_sse_ ha010, ha011;
      split(a01, ha010, ha011);
      a2 = _mm256_castsi128_si256(ha010);
      a2 = _mm256_insertf128_si256(a2, ha011, 1);
    }
  };
} } }

#endif
#endif


/*
 * =====================================================================================
 *
 *       Filename:  EnvelopFinder.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  9/6/2012 4:48:20 PM
 *       Revision:  none
 *       Compiler:  msvc
 *
 *         Author:  Han Hu (hh1985@bu.edu),
 *   Organization:  Boston University
 *
 * =====================================================================================
 */

#include <algorithm>
#include <GAGPL/MISC/Param.h>
#include <GAGPL/SPECTRUM/EnvelopFinder.h>
#include <GAGPL/SPECTRUM/EnvelopReference.h>
#include <GAGPL/SPECTRUM/IsotopicDistribution.h>
#include <boost/utility.hpp>
//#include <boost/math/distributions/normal.hpp>

namespace gag
{
	EnvelopFinder::EnvelopFinder(RichList& spec)
		: spectrum(spec), param(Param::instance())
	{
		/*
		Guess the range of average formula. Usually we assume the difference between composition estimated from the base peak and the one estimated from monoisotopic peak can be safely ignored.
			No Sulfur (100D)		High Sulfur (100D)
			C: 3.7238523        1.9961864
			H: 5.4425534        2.9942796
			O: 2.8645018        3.1938983
			N: 0.2864502        0.1996186
			S: 0                0.5988559
		*/
		ave1.insert(std::make_pair("C", 3.7238523));
		ave1.insert(std::make_pair("H", 5.4425534));
		ave1.insert(std::make_pair("O", 2.8645018));
		ave1.insert(std::make_pair("N", 0.2864502));
		ave1.insert(std::make_pair("S", 0.0));

		ave2.insert(std::make_pair("C", 1.9961864));
		ave2.insert(std::make_pair("H", 2.9942796));
		ave2.insert(std::make_pair("O", 3.1938983));
		ave2.insert(std::make_pair("N", 0.1996186));
		ave2.insert(std::make_pair("S", 0.5988559));

		run();
	}

	void EnvelopFinder::run()
	{
		// Get/Set parameters.
		unsigned int precursor_charge = param.getParameter<unsigned int>("precursor_charge").first;

		// Divide the spectrum into several independent islands.
		IslandList is_list = this->getPeakIslands(spectrum);

		// Iterate over all independent islands.
		for(IslandList::iterator iter = is_list.begin(); iter != is_list.end(); iter++)
		{
			std::set<EnvelopPtr> temp_env_set;

			RichList island_pks = *iter;
			std::cout << "Found island: " << std::endl;
			island_pks.printPeakList<peak_mz>();
			// Starts from the peak with highest intensity. Assuming it is the base peak.
			RichPeakListByResolution& pks_area = island_pks.getPeakListByType<peak_resolution>();

			RichPeakListByResolution::iterator iter_area = pks_area.begin();
			RichPeakListByResolution::iterator end_iter = pks_area.end();
			// The last peak cannot be considered as a base peak.
			end_iter--;

			/* Step 1. Envelop identification */
			for(; iter_area != end_iter; iter_area++)
			{
				// Iterate over all possible charge states.
				// Notice that the sign of the charge is not used here.
				for(unsigned int z=precursor_charge; z>=1; z--)
				{
					// All candidate envelops should be stored for records.
					this->findNextEnvelops(island_pks, iter_area, z, temp_env_set);
				}

			}

			std::cout << "Before optimizing, output all the envelop information..." << std::endl;
			BOOST_FOREACH(EnvelopPtr env, temp_env_set)
				env_ref.printEnvelopInformation(env);

			std::cout << std::endl;

			// Adjust the fitting scores and splitting scores for envelop set.
			this->optimizeEnvelopSet(temp_env_set);

		}
	}

	IslandList EnvelopFinder::getPeakIslands(RichList& pks)
	{
		double island_space = param.getParameter<double>("island_space").first;

		IslandList is_list;

		RichList* temp_spec = new RichList();

		RichPeakListByMZ& pks_mz = pks.getPeakListByType<peak_mz>();

		for(RichPeakListByMZ::const_iterator iter = pks_mz.begin(); iter != pks_mz.end(); iter++)
		{
			if(iter == pks_mz.begin()) {
				temp_spec->addPeak(*iter);
				continue;
			}

			if((*iter)->mz - (*boost::prior(iter))->mz < island_space) {
				temp_spec->addPeak(*iter);
			} else { //
				is_list.push_back(*temp_spec);

				// Delete the container. Be careful of this section.
				delete temp_spec;

				temp_spec = new RichList();
				temp_spec->addPeak(*iter);
			}
		}
		is_list.push_back(*temp_spec);
		delete temp_spec;

		return is_list;
	}

	void EnvelopFinder::findNextEnvelops(RichList& island, RichPeakListByResolution::iterator iter_area, unsigned int charge, std::set<EnvelopPtr>& env_set)
	{
		// 1. Find candidate envelops which extend from current peak.

		// Project the area iterator into corresponding mz iterator.
		RichPeakListByMZ::iterator iter_mz = island.getPeakContainer().project<peak_mz>(iter_area);

		std::cout << "Identify base peak: " << (*iter_mz)->mz << " Charge: " << charge << std::endl;

		int mode = param.getParameter<int>("mode").first;
		double pre_mz = param.getParameter<double>("precursor_mz").first;
		int pre_z = param.getParameter<int>("precursor_charge").first;

		// Convert m/z value to corresponding mass.
		double pre_mass = calculateMass(pre_mz, pre_z * mode);

		// If the calculated mass is larger than precursor mass, there is no need to continue identification.
		double mass = calculateMass((*iter_mz)->mz, charge * mode);

		if(mass > pre_mass)
			return;

		std::pair<PeakSet,size_t> pk_set = this->extendPeakSet(island, iter_mz, charge);

		if(pk_set.first.getSize()==1)
			return;

    // Container for temporary storage of envelops.
		std::vector<EnvelopPtr> env_store;
		for(size_t i=0; i < pk_set.second; i++) {
			EnvelopPtr env = createEnvelop(charge, pk_set.first.getBoundary());
			env_store.push_back(env);
		}

		// 2. TBD: Estimate the validity of the pk_set.
		// e.g. the overall shifts and the continuity of the shifts.

		// 2.a If the base peak has been reported as a distal peak of other envelops.
		// 2.b If envelops which share the base peak have the same charge
		// 2.c No event report for the base peak.
		// 2.d no contribution of new peaks.
		// 2.e fitting score is worse than expected.

		// Get the maximum number of envelops.

		// Dynamically convert pk_set into vector of envelops.
		ProtoEnv& peak_information = pk_set.first.getPeakInformation();

		ProtoEnv::iterator pk_iter = peak_information.begin();
		// Iterate over all shift, decide if all envelop will share.
		for(; pk_iter != peak_information.end(); )
		{
			int count = peak_information.count(pk_iter->first);
			std::pair<ProtoEnv::iterator, ProtoEnv::iterator> shift_pair = peak_information.equal_range(pk_iter->first);
			ProtoEnv::iterator shift_iter = shift_pair.first;
			ProtoEnv::iterator last_iter = shift_pair.second;
			last_iter--;

			for(size_t i=0; i<env_store.size(); i++)
			{
				// Clean the definition of the internal structure.
				int shift = shift_iter->first;
				RichPeakPtr pk = shift_iter->second.first;
				InfoPeakPtr info = shift_iter->second.second;
				// Register the (peak, env) pair, shift and peak info into the envelop reference table.
				InfoPeakPtr info_copy(new InfoPeak(*info));
				std::cout << "Register peak " << pk->mz << "for envelop " << env_store[i]->id << std::endl;
				env_ref.addDictionaryReference(pk, env_store.at(i), shift, info_copy);

				// Non_unique_key.
				if(shift_iter != last_iter)
					shift_iter++;

			}

			pk_iter = shift_pair.second;
		}

		// Update envelop lambda information for each envelop.
		this->updateEnvelopParameter(env_store);

		// Store the envelops.
		env_set.insert(env_store.begin(), env_store.end());
	}

std::pair<PeakSet, size_t> EnvelopFinder::extendPeakSet(RichList& island, RichPeakListByMZ::iterator iter_mz, unsigned int charge)
	{

		// Candidate base peak. Notice that this might not be the monoisotopic peak for molecules with high mass values.
		RichPeakPtr expr_base_pk = *iter_mz;
		int mode = param.getParameter<int>("mode").first;
		double mass = calculateMass(expr_base_pk->mz, charge * mode);
		EnvelopBoundary bound = this->createBoundary(mass, charge);

		// The reason of creating an object called envelop is because during the extension of peakset towards different directions, there might be multiple matching for given shift.
		PeakSet pk_set(bound);

		InfoPeakPtr pk_infor = boost::make_shared<InfoPeak>(expr_base_pk->resolution, 0.0, 0.5, 0.0);

		pk_set.addPeak(0, expr_base_pk, pk_infor);

		RichPeakListByMZ& pks_mz = island.getPeakListByType<peak_mz>();

		// Shift towards right along the mz axis.
		size_t max_env = this->extendPeak(pk_set, pks_mz, iter_mz, charge, 1);
		// TBD: Shift towards left along the mz axis.
		//this->extendPeak(pk_set, pks_mz, iter_mz, charge, -1);

		return std::make_pair(pk_set,max_env);
	}

	EnvelopBoundary EnvelopFinder::createBoundary(const double mass, unsigned int charge)
	{
		// 1. Estimate the rounded version of compositions.
		double coef = mass / 100.0;
		Composition compo1, compo2;

		AveragineFormulae::iterator iter1 = ave1.begin();
		for(; iter1 != ave1.end(); iter1++)
			compo1.addElement(iter1->first, (int)floor(iter1->second * coef + 0.5));

		AveragineFormulae::iterator iter2 = ave2.begin();
		for(; iter2 != ave2.end(); iter2++)
			compo2.addElement(iter2->first, (int)floor(iter2->second * coef + 0.5));

		int mode = param.getParameter<int>("mode").first;

		int z = mode > 0 ? charge : (-1)*charge;
		// 2. Get the range of the isotopic distribution
		IsotopicDistribution iso_up(compo1);
		AggregatedIsotopicVariants peakset1 = iso_up.getAggregatedIsotopicVariants(z);
		IsotopicDistribution iso_down(compo2);
		AggregatedIsotopicVariants peakset2 = iso_down.getAggregatedIsotopicVariants(z);

		return EnvelopBoundary(peakset1, peakset2);
	}

	size_t EnvelopFinder::extendPeak(PeakSet& pk_set, RichPeakListByMZ& pks_mz,RichPeakListByMZ::iterator iter_mz, unsigned int charge, int direction)
	{
		//int dist = direction > 0 ? std::distance(iter_mz, pks_mz.end())-1 : std::distance(pks_mz.begin(), iter_mz);

		unsigned int shift = 1;
		//int prev_dist = 0;

		// Experimental candidate base peak.
		RichPeakPtr expr_base_pk = *iter_mz;

		int mode = param.getParameter<int>("mode").first;
		// For confidence estimation, using internal_accuracy.
		double internal_accuracy = param.getParameter<double>("internal_accuracy").first;

		// For peak finding and lambda estimation, using external_accuracy.
		double external_accuracy = param.getParameter<double>("external_accuracy").first;

		// Calculate the mass of the candidate base peak.
		// Notice that there will be mechanism where there is electron capture.
		double mass = calculateMass(expr_base_pk->mz, charge * mode);

		// Create theoretical boundary. The boundary has been adjusted to fit the charge.
		//EnvelopBoundary env_bound = this->createBoundary(mass, charge);
		EnvelopBoundary& env_bound = pk_set.getBoundary();

		//std::cout << "Up Boundary: " << std::endl;
		//env_bound.first.printPeakList<peak_mz>();
		//std::cout << "Down Boundary: " << std::endl;
		//env_bound.second.printPeakList<peak_mz>();

		// Get a copy of the base peak iterator for further move operation.
		//RichPeakListByMZ::iterator iter = iter_mz;
		int stop_flag = 1;

		// Dynamically convert pk_set into vector of envelops.
		std::pair<unsigned int, size_t> shift_count(0,1);
		size_t max_env = 0;

		//std::advance(iter_mz, direction);
		if(direction < 0 && iter_mz == pks_mz.begin())
			return 0;
		std::advance(iter_mz, direction);
		if(direction > 0 && iter_mz == pks_mz.end())
			return 0;

		// TBD: the program should efficiently location the position of most likely peak. instead of sequentially search for it.
		while(1)
		{

			std::cout << "Shift: " << shift << std::endl;

			// Get the boundary peaks at current shift.
			PeakPtr theo_pk1 = env_bound.getUpperBound().getPeakByShift<peak_intensity>(direction * shift);
			PeakPtr theo_pk2 = env_bound.getLowerBound().getPeakByShift<peak_intensity>(direction * shift);

			// No extension any more.
			if(theo_pk1->mz == 0.0 && theo_pk2->mz == 0.0)
				break;

			RichPeakPtr current_pk = *iter_mz;
			std::cout << "Examine peak: " << current_pk->mz << std::endl;

			// Experimental distance from current peak to base peak.
			// Notice that the value might be negative.
			double expr_dist = abs(current_pk->mz - expr_base_pk->mz);

			// Theoretical distance from current peak to base peak. Adjusted for charged peak.
			double theo_dist1 = abs(env_bound.getUpperBound().getMassDifferenceByShift<peak_intensity>(0, shift));
			double theo_dist2 = abs(env_bound.getLowerBound().getMassDifferenceByShift<peak_intensity>(0, shift));

			double max_dist = theo_dist1;
			double min_dist = theo_dist2;

			if(max_dist < min_dist) {
				swap(min_dist, max_dist);
			}

			double error1 = 1e6 * (expr_dist - min_dist)/expr_base_pk->mz;
			double error2 = 1e6 * (expr_dist - max_dist)/expr_base_pk->mz;

			if(error1 < -1 * external_accuracy) {
				// Before the shift region: keep moving the iterator.
				// std::advance(iter_mz, direction); continue;
			} else if(error2 > external_accuracy) {
				// Beyond the shift region: update shift.
				if(stop_flag == 0) {
					shift++; stop_flag = 1; continue;
				} else {
					// break means no missing peak is allowed in the middle. This might be controlled by some parameter.
					stop_flag = 1;
					break;
				}
			} else {
				// A matching peak. Notice the effect of charge state.
				std::cout << "A matching peak!" << std::endl;

				if(shift == shift_count.first) {
					shift_count.second++;
				} else {
					// Reset shift_count.
					shift_count = std::make_pair(shift, 1);
				}

				// Update maximum number of envelops.
				if(shift_count.second > max_env)
					max_env = shift_count.second;

				stop_flag = 0;
				// Estimate the confidence of the mz in terms of estimating lambda.
				double diff = max_dist - min_dist;

				// Error window is used for confidence estimation.
				double err_win = internal_accuracy * expr_base_pk->mz * 1e-6;
				double prob = diff/(diff + 2*err_win);

				// Indicate the sign of the error window.
				int err_sign = (theo_dist1 < theo_dist2 ? -1 : 1);

				//: lambda_mz should always be in (0, 1). If lambda is closed to 1, the pattern is closed to no-sulfate boundary, and 0 for high-sulfate boundary.
				double lambda_mz = (expr_dist - theo_dist2+err_sign*err_win)/(theo_dist1 - theo_dist2 + 2*err_sign*err_win);

				// Round the abnormal lambda_mz to the boundary.
				if(lambda_mz > 1) {
					lambda_mz = 1.0;
					prob = 1.0;
				} else if(lambda_mz < 0) {
					lambda_mz = 0.0;
					prob = 1.0;
				}

				// TBD: estimate lambda_abd (within 5%) and delta_lambda.
				// Notice: lambda_abd can be < 0 or > 1.
				double lambda_abd = (current_pk->resolution/expr_base_pk->resolution - theo_pk2->intensity)/(theo_pk1->intensity - theo_pk2->intensity);

				InfoPeakPtr pk_infor = boost::make_shared<InfoPeak>(current_pk->resolution, lambda_mz, lambda_abd, prob);
				// Notice: The abundance information should be initialized during the establish of global lambda value.
				//pk_infor.adjusted_abundance = env_bound.getTheoreticalPeak(lambda_abd, shift).intensity;

				pk_set.addPeak(shift, current_pk, pk_infor);

			}
			if(direction < 0 && iter_mz == pks_mz.begin())
				break;
			std::advance(iter_mz, direction);
			if(direction > 0 && iter_mz == pks_mz.end())
				break;
		}
		return max_env;
	}

	void EnvelopFinder::updateEnvelopParameter(EnvelopPtr env)
	{
		EnvEntry base_entry = env_ref.getEntryByShift(env, 0);

		// 1.a Find lambda value for each of the peak.
		std::vector<EnvEntry> peak_entries = env_ref.getEntryByEnvelop(env);

		double current_prob = 0.0;
		BOOST_FOREACH(EnvEntry& entry, peak_entries)
		{
			if(entry.info->prob > current_prob) {
				current_prob = entry.info->prob;
				env->lambda_mz = entry.info->lambda_mz;
			}

		}
		//// 1.b Normalize the confidence value.
		//double sum_confidence = 0.0;
		//for(std::vector<EnvEntry>::iterator iter = peak_entries.begin();
		//	iter != peak_entries.end(); iter++)
		//	sum_confidence += pow(iter->info->prob,2);

		//double lambda_mz = 0.0;
		//for(std::vector<EnvEntry>::iterator iter = peak_entries.begin();
		//	iter != peak_entries.end(); iter++) {
		//		// Pass base peak.
		//		if(iter->getShift() == 0)
		//			continue;

		//		lambda_mz += pow(iter->info->prob,2)/sum_confidence *  iter->info->lambda_mz;
		//}
		//env->lambda_mz = lambda_mz;

		// 2. Update theoretical abundance from the lambda.
		for(std::vector<EnvEntry>::iterator iter = peak_entries.begin();
			iter != peak_entries.end(); iter++) {
			// The adjusted abundance is initialized as the exp data.
			iter->info->adjusted_abundance = iter->pk->area;
		}
	}

	void EnvelopFinder::updateEnvelopParameter( std::vector<EnvelopPtr> env_list )
	{
		BOOST_FOREACH(EnvelopPtr env, env_list) {
			this->updateEnvelopParameter(env);

			//this->updateSplittingPotential(env);
		}
	}

	void EnvelopFinder::optimizeEnvelopSet(std::set<EnvelopPtr>& env_set, double last_score)
	{
		/* New version */
		this->updateSplittingPotential(env_set);
		this->updateFittingScore(env_set);

		// Get all the peaks.
		RichList pk_list = env_ref.getRichPeakList(env_set);

		RichPeakListByMZ& pks_mz = pk_list.getPeakListByType<peak_mz>();

		for(RichPeakListByMZ::iterator iter = pks_mz.begin(); iter != pks_mz.end(); iter++)
		{
			// Split the peak based on fitting score & estimated abundance.
			// Notice that the identification of FP will also happen here.
			this->splitPeak(*iter);
		}
		// Calculate the overall fitting scores.
		double next_score = this->updateFittingScore(env_set);
		std::cout << "Old score: " << last_score << "\t" << "New score: " << next_score << std::endl;
		if(abs(next_score - last_score)/last_score < 1e-5)
			return;
		else
			this->optimizeEnvelopSet(env_set, next_score);
		/* Old version */

		//std::set<RichPeak> processed_peak_set;
		//std::set<RichPeak>::iterator peak_iter;

		//// Generate fitting score for each envelop.
		//// Estimate scaling factor for each envelop.
		//this->updateSplittingPotential(env_pool);
		//this->updateFittingScore(env_pool);

		//sort(env_pool.begin(), env_pool.end(), Envelop::scoreLarger);

		//// Iterate over all envelops with intensity from high to low.
		//BOOST_FOREACH(EnvelopPtr& env, env_pool)
		//{
		//	// If the base peak has been processed.
		//	EnvEntry base_entry = env_ref.getBasePeakForEnvelop(env);
		//
		//	peak_iter = processed_peak_set.find(base_entry->pk);
		//	if(peak_iter != processed_peak_set.end())
		//		continue;

		//	// Split the base peak.
		//	bool status = this->splitPeak(base_entry->pk);
		//	if(status) {
		//		this->updateSplittingPotential(env);
		//		this->updateFittingScore(env);
		//	}

		//	// Get all belonging peaks.
		//	std::vector<EnvEntry> entry_vec = env_ref.getEntryByEnvelop(env);

		//	// Iterate over all peaks with intensity from high to low.
		//	BOOST_FOREACH(EnvEntry& entry, entry_vec)
		//	{
		//		if(entry->getShift()==0)
		//			continue;
		//		// Split the peak based on 1. splitting potential and 2. scaling factor.
		//		this->splitPeak(entry->pk);
		//		// Update the records.
		//		processed_peak_set.insert(entry->pk);
		//	}

		//}
	}

	void EnvelopFinder::updateSplittingPotential(EnvelopPtr env)
	{

		//double scaling_factor = env->scaling_factor;

		// Entry for base peak.
		EnvEntry& base_entry = env_ref.getEntryByShift(env, 0);
		std::cout << "The base peak is: " << base_entry.pk->mz << std::endl;

		// Get all peaks included by the envelop.
		std::vector<EnvEntry> entry_vec = env_ref.getEntryByEnvelop(env);

		// Update the scaling factor. The value for base peak should be 1.
		BOOST_FOREACH(EnvEntry& entry, entry_vec)
		{
			double expected_abundance = env_ref.getTheoreticalAbundance(entry);
			// Adjusted abunance / Theoretical abundance.
			double scaling_factor = entry.info->adjusted_abundance/expected_abundance;
			if(scaling_factor < env->scaling_factor)
				env->scaling_factor = scaling_factor;
		}

		// Update the adjusted abundance.
		// 1. Update base entry.
		base_entry.info->adjusted_abundance *= env->scaling_factor;
		// 2. Update all the rest of the peaks.
		BOOST_FOREACH(EnvEntry& entry, entry_vec)
		{
			if(entry.getShift()==0)
				continue;

			entry.info->adjusted_abundance = env_ref.getTheoreticalAbundance(entry);
		}

		//// Iterate over all theoretical peaks (except base peak), calculate their relative shift.
		//double total_share = 0.0;
		//std::map<int, PeakPtr> pk_map = env->getTheoreticalPeaks();
		//for(std::map<int, PeakPtr>::iterator iter = pk_map.begin();
		//	iter != pk_map.end(); iter++)
		//{
		//	total_share += iter->second->intensity;
		//}
		//
		//double base_adjusted_abd = 0.0;
		//for(std::map<int, PeakPtr>::iterator iter = pk_map.begin();
		//	iter != pk_map.end(); iter++)
		//{
		//	int shift = iter->first; PeakPtr pk = iter->second;

		//	// Get entry corresponding to the shift. Notice the problem if shift does not exist for the envelop..
		//	EnvEntry entry = env_ref.getEntryByShift(env, shift);

		//	// If EnvEntry is empty. Which means the experimental peak corresponds to the shift doesn't show up.
		//	if(entry.isEmpty()) continue;

		//	entry.info->relative_share = pk->intensity / total_share;

		//	double expected_abundance = env_ref.getTheoreticalAbundance(entry);

		//	if(expected_abundance > entry.info->adjusted_abundance) {
		//		// Potential overlapping event for base peak. Scale the base peak.
		//		base_adjusted_abd += base_entry.info->adjusted_abundance * (entry.info->adjusted_abundance / expected_abundance) * entry.info->relative_share;
		//	}
		//	// Update adjusted abundance.
		//	entry.info->adjusted_abundance = expected_abundance;
		//}

		//// Update base peak information.
		//base_entry.info->adjusted_abundance = base_adjusted_abd;
	}

	void EnvelopFinder::updateSplittingPotential( std::vector<EnvEntry> entry_vec )
	{
		BOOST_FOREACH(EnvEntry entry, entry_vec)
			this->updateSplittingPotential(entry.env);
	}
	void EnvelopFinder::updateSplittingPotential(std::set<EnvelopPtr>& env_set)
	{
		BOOST_FOREACH(EnvelopPtr env, env_set)
			this->updateSplittingPotential(env);
	}

	double EnvelopFinder::updateFittingScore(EnvelopPtr env)
	{
		std::vector<EnvEntry> entry_vec = env_ref.getEntryByEnvelop(env);
		//RichPeakPtr base_pk = env_ref.getBasePeakForEnvelop(env);
		EnvEntry base_entry = env_ref.getEntryByShift(env, 0);

		double fitting_score = 0.0;
		BOOST_FOREACH(EnvEntry& entry, entry_vec)
		{
			// For base peak, intensity_shift and mz_shift is always 1.
			if(entry.getShift()==0) {
				fitting_score += sqrt(base_entry.info->adjusted_abundance);
				continue;
			}

			double expected_abundance = env_ref.getTheoreticalAbundance(entry);
			// The fitting score comes from 3 parts:
			// 1. the shift of intensity: abs(E-T)/T.
			double intensity_shift = 1.0 - abs(expected_abundance - entry.info->adjusted_abundance)/expected_abundance;
			if(intensity_shift < 0)
				intensity_shift = 0.0;
			//std::cout << "Intensity shift: " << intensity_shift << std::endl;

			// 2. the relative shift of mz.
			double internal_accuracy = param.getParameter<double>("internal_accuracy").first;

			double mz_shift = 1.0 - abs(entry.pk->mz - base_entry.pk->mz - entry.env->getTheoreticalMZDistance(0, entry.getShift()))/(entry.env->getWindowSizeByShift(entry.getShift()));
			//std::cout << "MZ shift: " << mz_shift << std::endl;

			// 3. the square root of expected_abundance.
			double weight = sqrt(expected_abundance);
			//std::cout << "Weight: " << weight << std::endl;
			fitting_score += weight * intensity_shift * mz_shift;
		}
		//std::cout << "Fitting score: " << fitting_score << std::endl;
		env->fitting_score = fitting_score;
		return fitting_score;
	}

	double EnvelopFinder::updateFittingScore( std::vector<EnvEntry> entry_vec)
	{
		double sum_score = 0.0;
		BOOST_FOREACH(EnvEntry& entry, entry_vec)
			sum_score += this->updateFittingScore(entry.env);
		return sum_score;
	}

	double EnvelopFinder::updateFittingScore(std::set<EnvelopPtr>& env_set)
	{
		double sum_score = 0.0;
		BOOST_FOREACH(EnvelopPtr env, env_set)
			sum_score += this->updateFittingScore(env);
		return sum_score;
	}

	void EnvelopFinder::adjustLambdaFromProfile( EnvelopPtr env )
	{
		double last_score = this->updateFittingScore(env);
		// The value of lambda_bound has to be in between 0.0 and 1.0
		double lambda_bound = 0.0;

		while(1)
		{
			// Adjust lambda. Notice the direction.
			env->lambda_mz = (lambda_bound + env->lambda_mz)/2;

			// Update splitting score.
			this->updateSplittingPotential(env);

			double new_score = this->updateFittingScore(env);

			double score_diff = new_score - last_score;

			// Converge.
			if(abs(score_diff) < 1e-6)
				break;

			if(score_diff > 0.0) {
					env->lambda_mz = (lambda_bound + env->lambda_mz)/2;
			} else {
					// Change the bound.
					lambda_bound = 1.0-lambda_bound;
					env->lambda_mz = (lambda_bound + env->lambda_mz)/2;
			}

		}

	}

	void EnvelopFinder::splitPeak(RichPeakPtr pk)
	{
		// Get all connecting envelops.
		std::vector<EnvEntry> entry_vec = env_ref.getEntryByPeak(pk);

		if(entry_vec.size() == 1) // No need to split for single envelop.
			return;
		else if(entry_vec.size() == 0)
			throw std::runtime_error("Undefined peak in the dictionary");
		else
			;

		double splitting_score = 0.0;

		BOOST_FOREACH(EnvEntry& entry, entry_vec)
		{
			// splitting_score += entry.info->adjusted_abundance;
			splitting_score += sqrt(entry.env->fitting_score * entry.info->adjusted_abundance);
		}

		// Split the peak.
		BOOST_FOREACH(EnvEntry& entry, entry_vec)
		{
			double scale_coef = sqrt(entry.env->fitting_score * entry.info->adjusted_abundance)/ splitting_score;

			// Update the estimated abundance.
			entry.info->adjusted_abundance = pk->resolution * scale_coef;

			env_ref.printEnvelopInformation(entry.env);
		}

	}

	void EnvelopFinder::printEnvelop( EnvelopPtr env )
	{
		std::cout << "Envelop ID: " << env->id << std::endl;
		std::cout << "Charge: " << env->charge_state << std::endl;
		std::cout << "Lambda: " << env->lambda_mz << std::endl;
		std::cout << "Score: " << env->fitting_score << std::endl;
		std::cout << std::endl;
		// Peak information.
		std::cout << "Covered peaks:" << std::endl;
		std::cout << "Shift\tMZ\tEXP_ABD\tFIT_ABD" << std::endl;

		// Get all peaks.
		std::vector<EnvEntry> pk_entries = env_ref.getEntryByEnvelop(env);
		BOOST_FOREACH(EnvEntry& pk_entry, pk_entries)
		{
			std::cout << pk_entry.getShift() << "\t" << pk_entry.pk->mz << "\t"
				<< pk_entry.pk->resolution << "\t" << pk_entry.info->adjusted_abundance
				<< std::endl;
		}
		std::cout << std::endl;
	}

	//void EnvelopFinder::optimizeEnvelopSet(RichList& pk_list)
	//{
	//	// 1. Iterate over peaks with m/z from low to high
	//	RichPeakListByMZ& pks_by_mz = pk_list.getPeakListSortedByType<peak_mz>();

	//	RichPeakListByMZ::iterator iter_mz = pks_by_mz.begin();

	//	// Envelop set and corresponding fitting score.
	//	EnvelopSetCollection last_frontier;

	//	// TBD: how to decide the cluster group will end.
	//	for(; iter_mz != pks_by_mz.end(); iter_mz++)
	//	{
	//		// 2. For each peak, find all connecting envelops.
	//		std::vector<EnvEntry> entry_vec = env_ref.getEntryByPeak(*iter_mz);

	//		// Empty information. Consider storing the information with a separate container.
	//		if(entry_vec.size() == 0)
	//			continue;

	//		std::vector<std::set<EnvelopPtr> > env_set_collection;
	//		for(size_t i = 0; i < entry_vec.size()-1; i++)
	//			for(size_t j=i; j<entry_vec.size(); j++)
	//			{
	//				std::set<EnvelopPtr> temp_env;
	//				temp_env.insert(entry_vec[i]->env);

	//				if(i != j) // Not just pairs.
	//					temp_env.insert(entry_vec[j]->env);

	//				env_set_collection.insert(temp_env);
	//			}

	//		// New frontier.
	//		if(last_frontier.size() == 0)
	//		{
	//			std::vector<std::set<EnvelopPtr> >::iterator iter = env_set_collection.begin();
	//			for(; iter != env_set_collection.end(); iter++)
	//			{
	//				double temp_score = this->calculateFittingScore(*iter);
	//				last_frontier.insert(std::make_pair(temp_score, *iter));
	//			}
	//			continue;
	//		}

	//		EnvelopSetCollection next_frontier;
	//		// Update current frontier.
	//		for(EnvelopSetCollection::iterator fron_iter = last_frontier.begin(); fron_iter != last_frontier.end(); fron_iter++)
	//		{
	//			BOOST_FOREACH(std::set<EnvelopPtr>& env_set, env_set_collection)
	//			{
	//				std::set<EnvelopPtr> temp_set(fron_iter->second);
	//				// Append the set.
	//				set_union(temp_set.begin(), temp_set.end(),
	//					env_set.begin(), env_set.end(),
	//					inserter(temp_set, temp_set.begin()));

	//				// Compare the fitting score between std::vector<EnvelopPtr> with and without new current envelops.
	//				double score1 = fron_iter->first;
	//				double score2 = this->calculateFittingScore(new_set);

	//				if(score2 > score1)
	//					next_frontier.insert(std::make_pair(score2, temp_set));
	//				else
	//					next_frontier.insert(std::make_pair(score1, fron_iter->second));
	//			}
	//		}

	//		last_frontier = next_frontier;
	//
	//	}

	//	// Select the set with maximum score.
	//	std::set<EnvelopPtr>& winner_set = last_frontier.rbegin()->second;
	//
	//	env_pool.insert(env_pool.end(), winner_set.begin(), winner_set.end());
	//
	//}

	//double EnvelopFinder::calculateFittingScore( std::set<EnvelopPtr> env_set )
	//{
	//	/* Estimate the abundance based on current set selection. */
	//

	//	double total_score = 0.0;
	//	BOOST_FOREACH(EnvelopPtr& env, env_set)
	//	{
	//		total_score += this->calculateFittingScore(env);
	//	}
	//	return total_score;
	//}

	//double EnvelopFinder::calculateFittingScore(EnvelopPtr env)
	//{
	//	std::vector<EnvEntry> entry_vec = env_ref.getEntryByEnvelop(env);
	//
	//	if(entry_vec.size()==0)
	//		return 0.0;

	//	// Get base entry.
	//	EnvEntry base_entry = env_ref.getEntryByShift(env, 0);

	//	double fitting_score = 0.0;
	//	BOOST_FOREACH(EnvEntry& entry, entry_vec)
	//	{
	//		// For base peak, intensity_shift and mz_shift is always 1.
	//		if(entry.getShift()==0) {
	//			fitting_score += sqrt(base_entry.info->adjusted_abundance);
	//			continue;
	//		}

	//		double expected_abundance = env_ref.getTheoreticalAbundance(entry);
	//
	//		// The fitting score comes from 3 parts:
	//		// 1. the shift of intensity: abs(E-T)/T.
	//		double intensity_shift = 1.0 - abs(expected_abundance - entry.info->adjusted_abundance)/expected_abundance;
	//		if(intensity_shift < 0)
	//			intensity_shift = 0.0;

	//		// 2. the relative shift of mz. Here the accuracy depends on the position of the peak in the envelop.
	//		double mz_shift = 1.0 - abs(entry.pk->mz - base_entry.pk->mz - entry.env->getTheoreticalMZDistance(0, entry.getShift()))/(entry.env->getWindowSizeByShift(entry.getShift()));

	//		// 3. the square root of expected_abundance.
	//		double weight = sqrt(expected_abundance);

	//		fitting_score += weight * intensity_shift * mz_shift;
	//	}

	//	env->fitting_score = fitting_score;
	//	return fitting_score;
	//}
}


#ifndef STAT_MAP_HPP_
#define STAT_MAP_HPP_

#include <numeric>
#include <boost/multi_array.hpp>
#include <sferes/stat/stat.hpp>

namespace sferes
{
  namespace stat
  {
    SFERES_STAT(Map, Stat)
    {
    public:
      typedef boost::shared_ptr<Phen> phen_t;
      typedef boost::multi_array<phen_t, 2> array_t;
      typedef boost::array<float, 2> point_t;

      Map() : _xs(0), _ys(0) {}
      template<typename E>
        void refresh(const E& ea)
      {
	_archive.clear();
	_xs = ea.archive().shape()[0];
	_ys = ea.archive().shape()[1];
	assert(_xs == Params::ea::res_x);
	assert(_ys == Params::ea::res_y);

	for (size_t i = 0; i < _xs; ++i)
	  for (size_t j = 0; j < _ys; ++j)
	    {
	      phen_t p = ea.archive()[i][j];
	      _archive.push_back(p);
	    }

	if (ea.gen() % Params::pop::dump_period == 0)
	  {
	    _write_archive(ea.archive(), std::string("archive_"), ea);
#ifdef MAP_WRITE_PARENTS
	    _write_parents(ea.archive(), ea.parents(), std::string("parents_"), ea);
#endif
	  }
      }
      void show(std::ostream& os, size_t k)
      {
	std::cerr << "loading "<< k / _ys << "," << k % _ys << std::endl;
	if (_archive[k])
	  {
	    _archive[k]->develop();
	    _archive[k]->show(os);
	    _archive[k]->fit().set_mode(fit::mode::view);
	    _archive[k]->fit().eval(*_archive[k]);
	  }
	else
	  std::cerr << "Warning, no point here" << std::endl;
      }
      template<class Archive>
        void serialize(Archive& ar, const unsigned int version)
      {
	ar & BOOST_SERIALIZATION_NVP(_archive);
	ar & BOOST_SERIALIZATION_NVP(_xs);
	ar & BOOST_SERIALIZATION_NVP(_ys);
      }
    protected:
      std::vector<phen_t> _archive;
      int _xs, _ys;

      template<typename EA>
      void _write_parents(const array_t& array,
			  const array_t& p_array,
			  const std::string& prefix,
			  const EA& ea) const
      {
	std::cout << "writing..." << prefix << ea.gen() << std::endl;
	std::string fname =  ea.res_dir() + "/"
	  + prefix
	  + boost::lexical_cast<
	    std::string>(ea.gen())
	  + std::string(".dat");
	std::ofstream ofs(fname.c_str());
	for (size_t i = 0; i < _xs; ++i)
	  for (size_t j = 0; j < _ys; ++j)
	    if (array[i][j] && p_array[i][j])
	      {
		point_t p = _get_point(p_array[i][j]);
		size_t x = round(p[0] * _xs);
		size_t y = round(p[1] * _ys);
		ofs << i / (float) _xs
		    << " " << j / (float) _ys
		    << " " << p_array[i][j]->fit().value()
		    << " " << x / (float) _xs
		    << " " << y / (float) _ys
		    << " " << array[i][j]->fit().value()
		    << std::endl;
	      }
      }

      template<typename EA>
      void _write_archive(const array_t& array,
			  const std::string& prefix,
			  const EA& ea) const
      {
	std::cout << "writing..." << prefix << ea.gen() << std::endl;
	std::string fname = ea.res_dir() + "/"
	  + prefix
	  + boost::lexical_cast<
	    std::string>(ea.gen())
	  + std::string(".dat");

	std::ofstream ofs(fname.c_str());
	for (size_t i = 0; i < _xs; ++i)
	  for (size_t j = 0; j < _ys; ++j)
	    if (array[i][j])
	      {
		ofs << i / (float) _xs
		    << " " << j / (float) _ys
		    << " " << array[i][j]->fit().value()
		    << std::endl;
	      }
      }

    };
  }
}

#endif


#include "rovi_utils/rovi_utils.h"

#include <thread>
#include <pthread.h>
#include <boost/filesystem.hpp>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_kdl.h>
#include <eigen_conversions/eigen_msg.h>
#include <kdl/trajectory_composite.hpp>

#include <moveit/planning_interface/planning_interface.h>

#include <gazebo_msgs/ModelStates.h>
#include <gazebo_msgs/LinkStates.h>
#include <geometric_shapes/shape_operations.h>

#include <ur5_dynamics/ur5_dynamics.h>

geometry_msgs::Pose
rovi_utils::make_pose(const std::array<double, 3>& pos, const Eigen::Quaternion<double>& ori)
{
	geometry_msgs::Pose pose;

	pose.position.x = pos[0];
	pose.position.y = pos[1];
	pose.position.z = pos[2];

	pose.orientation.w = ori.w();
	pose.orientation.x = ori.x();
	pose.orientation.y = ori.y();
	pose.orientation.z = ori.z();

	return pose;
}

geometry_msgs::Pose
rovi_utils::make_pose(const std::array<double, 6>& pose)
{
	return rovi_utils::make_pose
	(
		{ pose[0], pose[1], pose[2] },
		{ pose[3], pose[4], pose[5] }
	);
}

geometry_msgs::Pose
rovi_utils::make_pose(const std::array<double, 3>& pos, const std::array<double, 3>& rpy)
{
	geometry_msgs::Pose pose;

	pose.position.x = pos[0];
	pose.position.y = pos[1];
	pose.position.z = pos[2];

	Eigen::Quaterniond quat;

	quat = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *
		   Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) *
		   Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()) ;

	pose.orientation.w = quat.w();
	pose.orientation.x = quat.x();
	pose.orientation.y = quat.y();
	pose.orientation.z = quat.z();

	return pose;
}

moveit_msgs::CollisionObject
rovi_utils::make_mesh_cobj(const std::string& name, const std::string& planning_frame, const std::array<double, 3>& pos, const std::array<double, 4>& ori)
// rovi_utils::make_mesh_cobj(const std::string& model_name, const std::string& obj_name, const std::string& planning_frame, const std::array<double, 3>& pos, const std::array<double, 4>& ori)
{
	// 3D model of <name> object is expected to be located at
	// package://rovi_gazebo/models/name/name.dae
	static const std::string PATH_PACKAGE = "package://rovi_gazebo/models";
	// static const std::string PATH_PACKAGE = ros::package::getPath("rovi_gazebo") + "/models";
	ROS_WARN_STREAM_ONCE("make_mesh_cobj() only searches '" << PATH_PACKAGE << "' path for .dae models.");

	// create mesh
	// https://answers.ros.org/question/246467/moveit-attach-object-error/

	// remove any digits from object name to get model name (bottle3 -> bottle)
	auto model_name = name;
	model_name.erase(std::remove_if(model_name.begin(), model_name.end(), &isdigit), model_name.end());

	shape_msgs::Mesh mesh;
	shapes::Mesh* mesh_ptr;
	shapes::ShapeMsg shape_msg;

	mesh_ptr = shapes::createMeshFromResource("package://rovi_gazebo/models/" + model_name + "/" + model_name + ".dae");
	// mesh_ptr = shapes::createMeshFromResource("file://" + PATH_PACKAGE + "/" + model_name + "/" + model_name + ".dae");
	shapes::constructMsgFromShape(mesh_ptr, shape_msg);
	mesh = boost::get<shape_msgs::Mesh>(shape_msg);

	// construct collision object

	moveit_msgs::CollisionObject co;

	co.id = name;
	co.meshes.resize(1);
	co.mesh_poses.resize(1);

	co.meshes[0] = mesh;
	co.header.frame_id = planning_frame;

	co.mesh_poses[0].position.x = pos[0];
	co.mesh_poses[0].position.y = pos[1];
	co.mesh_poses[0].position.z = pos[2];

	co.mesh_poses[0].orientation.w = ori[0];
	co.mesh_poses[0].orientation.x = ori[1];
	co.mesh_poses[0].orientation.y = ori[2];
	co.mesh_poses[0].orientation.z = ori[3];

	// co.pose.position.x = pos[0];
	// co.pose.position.y = pos[1];
	// co.pose.position.z = pos[2];

	// co.pose.orientation.w = ori[0];
	// co.pose.orientation.x = ori[1];
	// co.pose.orientation.y = ori[2];
	// co.pose.orientation.z = ori[3];

	// co.meshes.push_back(mesh);
	// co.mesh_poses.push_back(co.mesh_poses[0]);
	co.operation = co.ADD;

	return co;
}

void
rovi_utils::move_base(moveit::core::RobotState& state, const std::array<double, 3>& offset, const std::string& virtual_joint_name)
{
	// RobotState has a floating virtual joint, which can be set.
	// http://docs.ros.org/en/melodic/api/moveit_core/html/classmoveit_1_1core_1_1RobotState.html#ad08c92a61d43013714ec3894cd67a297

	// ROS_ERROR_STREAM(state.getRobotModel()->getRootJointName());
	// ROS_ERROR_STREAM(state.getRobotModel()->getRootJoint()->getTypeName());

	// the position of the virtual joint is set by calling
	// setVariablePositions(const std::map<std::string, double>& variable_map)

	ROS_DEBUG_STREAM("Moving base (joint: " << virtual_joint_name <<") to: { " << offset[0] << ", " << offset[2] << ", " << offset[2] << "}");

	state.setVariablePositions({
		{ virtual_joint_name + "/trans_x", offset[0] }, // state.setVariablePosition(0, offset[0]);
		{ virtual_joint_name + "/trans_y", offset[1] }, // state.setVariablePosition(1, offset[1]);
		{ virtual_joint_name + "/trans_z", offset[2] } // state.setVariablePosition(2, offset[2]);
	});
}

void
rovi_utils::move_base(moveit::core::RobotState& state, const geometry_msgs::Pose& offset, const std::string& virtual_joint_name)
{
	move_base(state, { offset.position.x, offset.position.y, offset.position.z }, virtual_joint_name);
}

template<typename T>
void
rovi_utils::export_traj(T& traj, const std::string& filename, const double resolution)
{

	// MATLAB code:
	// data = readmatrix("traj_lin.csv");
	// plot3(data(:, 4), data(:, 8), data(:, 12))

	// get filename stem and extension
	const auto extension = boost::filesystem::path(filename).extension().string();
	const auto stem      = boost::filesystem::path(filename).stem().string();

	// KDL::Trajectory_Composite (Cartesian space)
	if constexpr (std::is_same<T, KDL::Trajectory_Composite*>::value)
	{

		if (resolution <= 0)
			throw std::runtime_error("Resolution must be positive.");

		std::ofstream fs(filename, std::ofstream::out);

		for (double t = 0.0; t < traj->Duration(); t += resolution)
		{
			// KDL Frame
			const auto& frame = traj->Pos(t);
			const auto& twist = traj->Vel(t);

			// convert KDL Frame to Eigen
			static auto mat = Eigen::Affine3d();
			tf::transformKDLToEigen(frame, mat);

			// create flattened vector from Eigen (row major)
			auto T_ = mat.matrix();
			T_.transposeInPlace();
			Eigen::VectorXd v(Eigen::Map<Eigen::VectorXd>(T_.data(), T_.size()));

			for (size_t i = 0; i < v.size(); ++i)
				fs << v(i) << ((i != v.size() - 1) ? ", " : "");

			fs << "\n";
		}

		fs.close();
	}

	// robot_trajectory::RobotTrajectory (Cartesian space, waypoints)
	if constexpr (std::is_same<T, robot_trajectory::RobotTrajectory>::value)
	{
		auto robot_state = std::make_shared<moveit::core::RobotState>(*traj.getFirstWayPointPtr());
		ROS_WARN_STREAM("Exporting trajectory using waypoints; resolution (timestep) is ignored.");

		std::ofstream fs(filename, std::ofstream::out);

		for (size_t i = 0; i < traj.getWayPointCount(); ++i)
		{
			// auto robot_state = traj.getWayPoint(i);
			auto mat = traj.getWayPoint(i).getFrameTransform("ee_tcp");

			// create flattened vector from Eigen (row major)
			auto T_ = mat.matrix();
			T_.transposeInPlace();
			Eigen::VectorXd v(Eigen::Map<Eigen::VectorXd>(T_.data(), T_.size()));

			for (size_t i = 0; i < v.size(); ++i)
				fs << v(i) << ((i != v.size() - 1) ? ", " : "");

			fs << "\n";
		}

		fs.close();
	}

	// std::array<KDL::Trajectory_Composite*, 6> (Joint space)
	if constexpr (std::is_same<T, std::array<KDL::Trajectory_Composite*, 6>>::value)
	{
		if (resolution <= 0)
			throw std::runtime_error("Resolution must be positive.");

		std::ofstream fs_q(stem + "_q" + extension, std::ofstream::out);
		std::ofstream fs_qdot(stem + "_qdot" + extension, std::ofstream::out);

		const auto num_joints = traj.size();
		const auto max_dur = (*std::max_element(traj.begin(), traj.end(), [&](auto& a, auto& b) {
			return a->Duration() < b->Duration();
		}))->Duration();

		for (double t = 0.0; t < max_dur; t += resolution)
		{
			std::string str_q    = "";
			std::string str_qdot = "";

			for (size_t i = 0; i < num_joints; ++i)
			{
				double q = traj[i]->Pos(t).p.data[0];
				double qdot = traj[i]->Vel(t).vel.data[0];

				str_q    +=    (str_q.empty() ? "" : ", ") + std::to_string(q);
				str_qdot += (str_qdot.empty() ? "" : ", ") + std::to_string(qdot);
			}

			fs_q    << str_q    << "\n";
			fs_qdot << str_qdot << "\n";
		}

		fs_q.close();
		fs_qdot.close();
	}
}

std::vector<geometry_msgs::Pose>
rovi_utils::waypoints_from_traj(const robot_trajectory::RobotTrajectory& traj)
{
	if (not traj.getWayPointCount())
		throw std::runtime_error("Trajectory is empty.");

	std::vector<geometry_msgs::Pose> waypoints;

	for (size_t i = 0; i < traj.getWayPointCount(); ++i)
	{
		// const auto mat = traj.getWayPoint(i).getFrameTransform("ur5_link6");
		const auto mat = traj.getWayPoint(i).getFrameTransform("ee_tcp");
		const auto t   = mat.translation() - traj.getWayPoint(i).getFrameTransform("ur5_link0").translation();
		const auto r   = mat.rotation();

		// create pose from translation and rotation; add to vector
		waypoints.emplace_back(make_pose( { t[0], t[1], t[2] }, Eigen::Quaternion<double>(r)));
	}

	return waypoints;
}

std::vector<sensor_msgs::JointState>
rovi_utils::joint_states_from_traj(const robot_trajectory::RobotTrajectory& traj)
{
	if (not traj.getWayPointCount())
		throw std::runtime_error("Trajectory is empty.");

	std::vector<sensor_msgs::JointState> joint_states;

	// static std::ofstream fs("joint_states_from_traj.csv", std::ofstream::out);

	for (size_t i = 0; i < traj.getWayPointCount(); ++i)
	{
		std::vector<double> q, qdot, qddot;
		sensor_msgs::JointState joint_state;

		// get joint values, define joint statee and add to vector of states
		traj.getWayPoint(i).copyJointGroupPositions("ur5_arm", q);
		joint_state.position = q;

		traj.getWayPoint(i).copyJointGroupVelocities("ur5_arm", qdot);
		joint_state.velocity = qdot;

		traj.getWayPoint(i).copyJointGroupVelocities("ur5_arm", qddot);
		joint_state.effort = qddot;

		joint_states.push_back(joint_state);

		// fs << joint_state.position[0] << ", " << joint_state.position[1] << ", " <<  joint_state.position[2] << ", " << joint_state.position[3] << ", " << joint_state.position[4] << ", " << joint_state.position[5] << ", " << std::endl;
	}

	// fs.close();

	return joint_states;
}

// rovi_utils::export_traj(const T& traj, const std::string&& filename, const double resolution)
template void rovi_utils::export_traj<KDL::Trajectory_Composite*>(KDL::Trajectory_Composite*& traj, const std::string& filename, const double resolution);
template void rovi_utils::export_traj<robot_trajectory::RobotTrajectory>(robot_trajectory::RobotTrajectory& traj, const std::string& filename, const double resolution);
template void rovi_utils::export_traj<std::array<KDL::Trajectory_Composite*, 6>>(std::array<KDL::Trajectory_Composite*, 6>& traj, const std::string& filename, const double resolution);


/** kmeans.cc
    Jeremy Barnes, 16 December 2014
    Copyright (c) 2014 mldb.ai inc.  All rights reserved.

    This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.
    Implementation of an KMEANS algorithm for embedding of a dataset.
*/

#include "mldb/utils/distribution_simd.h"
#include "kmeans_interface.h"
#include "mldb/builtin/matrix.h"
#include "mldb/core/mldb_engine.h"
#include "mldb/core/dataset.h"
#include "mldb/utils/distribution.h"
#include <boost/multi_array.hpp>
#include "mldb/base/scope.h"
#include "mldb/base/parallel.h"
#include "mldb/utils/pair_utils.h"
#include "mldb/arch/timers.h"
#include "mldb/arch/simd_vector.h"
#include "mldb/utils/vector_utils.h"
#include "mldb/types/basic_value_descriptions.h"
#include "mldb/plugins/jml/kmeans.h"
#include "mldb/sql/sql_expression.h"
#include "mldb/builtin/sql_config_validator.h"
#include "mldb/core/analytics.h"
#include "mldb/types/any_impl.h"
#include "mldb/types/optional_description.h"
#include "mldb/vfs/fs_utils.h"
#include "mldb/vfs/filter_streams.h"

using namespace std;

namespace MLDB {

DEFINE_STRUCTURE_DESCRIPTION(KmeansConfig);

KmeansConfigDescription::
KmeansConfigDescription()
{
    Optional<PolyConfigT<Dataset> > optional;
    optional.emplace(PolyConfigT<Dataset>().
                     withType(KmeansConfig::defaultOutputDatasetType));

    addField("trainingData", &KmeansConfig::trainingData,
             "Specification of the data for input to the k-means procedure.  This should be "
             "organized as an embedding, with each selected row containing the same "
             "set of columns with numeric values to be used as coordinates.  The select statement "
             "does not support groupby and having clauses.");
    addField("outputDataset", &KmeansConfig::output,
             "Dataset for cluster assignment.  This dataset will contain the same "
             "row names as the input dataset, but the coordinates will be replaced "
             "by a single column giving the cluster number that the row was assigned to.",
              optional);
    addField("centroidsDataset", &KmeansConfig::centroids,
             "Dataset in which the centroids will be recorded.  This dataset will "
             "have the same coordinates (columns) as those selected from the input "
             "dataset, but will have one row per cluster, providing the centroid of "
             "the cluster.",
             optional);
    addField("numInputDimensions", &KmeansConfig::numInputDimensions,
             "Number of dimensions from the input to use (-1 = all).  This limits "
             "the number of columns used.  Columns will be ordered alphabetically "
             "and the lowest ones kept.",
             -1);
    addField("numClusters", &KmeansConfig::numClusters,
             "Number of clusters to create.  This will provide the total number of "
             "centroids created.  There must be at least as many rows selected as "
             "clusters.", 10);
    addField("maxIterations", &KmeansConfig::maxIterations,
             "Maximum number of iterations to perform.  If no convergeance is "
             "reached within this number of iterations, the current clustering "
             "will be returned.", 100);
    addField("metric", &KmeansConfig::metric,
             "Metric space in which the k-means distances will be calculated. "
             "Normally this will be Cosine for an orthonormal basis, and "
             "Euclidian for another basis",
             METRIC_COSINE);
    addField("modelFileUrl", &KmeansConfig::modelFileUrl,
             "URL where the model file (with extension '.kms') should be saved. "
             "This file can be loaded by the ![](%%doclink kmeans function). "
             "This parameter is optional unless the `functionName` parameter is used.");
    addField("functionName", &KmeansConfig::functionName,
             "If specified, an instance of the ![](%%doclink kmeans function) of this name will be created using "
             "the trained model. Note that to use this parameter, the `modelFileUrl` must "
             "also be provided.");
    addParent<ProcedureConfig>();

    onPostValidate = chain(validateQuery(&KmeansConfig::trainingData,
                                         MustContainFrom(),
                                         NoGroupByHaving()),
                           validateFunction<KmeansConfig>());
}

// TODO: see http://www.eecs.tufts.edu/~dsculley/papers/fastkmeans.pdf

namespace {

ML::KMeansMetric * makeMetric(MetricSpace metric)
{
    switch (metric) {
    case METRIC_EUCLIDEAN:
        return new ML::KMeansEuclideanMetric();
        break;
    case METRIC_COSINE:
        return new ML::KMeansCosineMetric();
        break;
    default:
        throw MLDB::Exception("Unknown kmeans metric type");
    }
}

} // file scope

/*****************************************************************************/
/* KMEANS PROCEDURE                                                           */
/*****************************************************************************/

KmeansProcedure::
KmeansProcedure(MldbEngine * owner,
               PolyConfig config,
               const std::function<bool (const Json::Value &)> & onProgress)
    : Procedure(owner)
{
    kmeansConfig = config.params.convert<KmeansConfig>();
}

Any
KmeansProcedure::
getStatus() const
{
    return Any();
}

RunOutput
KmeansProcedure::
run(const ProcedureRunConfig & run,
      const std::function<bool (const Json::Value &)> & onProgress) const
{
    auto runProcConf = applyRunConfOverProcConf(kmeansConfig, run);

    // an empty url is allowed but other invalid urls are not
    if(!runProcConf.modelFileUrl.empty() && !runProcConf.modelFileUrl.valid()) {
        throw AnnotatedException(400, "modelFileUrl \"" +
                                  runProcConf.modelFileUrl.toUtf8String()
                                  + "\" is not valid");
    }

    if (!runProcConf.modelFileUrl.empty()) {
        checkWritability(runProcConf.modelFileUrl.toDecodedString(), "modelFileUrl");
    }

    auto onProgress2 = [&] (const Json::Value & progress)
        {
            Json::Value value;
            value["dataset"] = progress;
            return onProgress(value);
        };

    SqlExpressionMldbScope context(engine);

    ConvertProgressToJson convertProgressToJson(onProgress);
    auto embeddingOutput = getEmbedding(*runProcConf.trainingData.stm,
                                        context,
                                        runProcConf.numInputDimensions,
                                        convertProgressToJson);

    std::vector<std::tuple<RowHash, RowPath, std::vector<double>,
                           std::vector<ExpressionValue> > > & rows
        = embeddingOutput.first;
    std::vector<KnownColumn> & vars = embeddingOutput.second;

    std::vector<ColumnPath> columnNames;
    for (auto & v: vars) {
        columnNames.push_back(v.columnName);
    }

    std::vector<distribution<float> > vecs;

    for (unsigned i = 0;  i < rows.size();  ++i) {
        vecs.emplace_back(distribution<float>(std::get<2>(rows[i]).begin(),
                                                  std::get<2>(rows[i]).end()));
    }

    if (vecs.size() == 0)
        throw AnnotatedException(400, "Kmeans training requires at least 1 datapoint. "
                                  "Make sure your dataset is not empty and that your WHERE expression "
                                  "does not filter all the rows");

    ML::KMeans kmeans;
    kmeans.metric.reset(makeMetric(runProcConf.metric));

    vector<int> inCluster;

    int numClusters = runProcConf.numClusters;
    int numIterations = runProcConf.maxIterations;

    kmeans.train(vecs, inCluster, numClusters, numIterations);

    bool saved = false;
    if (!runProcConf.modelFileUrl.empty()) {
        try {
            makeUriDirectory(runProcConf.modelFileUrl.toDecodedString());
            Json::Value md;
            md["algorithm"] = "MLDB k-Means model";
            md["version"] = 1;
            md["columnNames"] = jsonEncode(columnNames);

            filter_ostream stream(runProcConf.modelFileUrl);
            stream << md.toString();
            MLDB::DB::Store_Writer writer(stream);
            kmeans.serialize(writer);
            saved = true;
        }
        catch (const std::exception & exc) {
            throw AnnotatedException(400, "Error saving kmeans centroids at location'" +
                                      runProcConf.modelFileUrl.toString() + "': " +
                                      exc.what());
        }
    }

    if (runProcConf.output.get()) {

        PolyConfigT<Dataset> outputDataset = *runProcConf.output;
        if (outputDataset.type.empty())
            outputDataset.type = KmeansConfig::defaultOutputDatasetType;

        auto output = createDataset(engine, outputDataset, onProgress2, true /*overwrite*/);

        Date applyDate = Date::now();

        for (unsigned i = 0;  i < rows.size();  ++i) {
            std::vector<std::tuple<ColumnPath, CellValue, Date> > cols;
            cols.emplace_back(ColumnPath("cluster"), inCluster[i], applyDate);
            output->recordRow(std::get<1>(rows[i]), cols);
        }

        output->commit();
    }

    if (runProcConf.centroids.get()) {

        PolyConfigT<Dataset> centroidsDataset = *runProcConf.centroids;
        if (centroidsDataset.type.empty())
            centroidsDataset.type = KmeansConfig::defaultOutputDatasetType;

        auto centroids = createDataset(engine, centroidsDataset, onProgress2, true /*overwrite*/);

        Date applyDate = Date::now();

        for (unsigned i = 0;  i < kmeans.clusters.size();  ++i) {
            auto & cluster = kmeans.clusters[i];

            std::vector<std::tuple<ColumnPath, CellValue, Date> > cols;

            for (unsigned j = 0;  j < cluster.centroid.size();  ++j) {
                cols.emplace_back(columnNames[j], cluster.centroid[j], applyDate);
            }

            centroids->recordRow(RowPath(MLDB::format("%i", i)), cols);
        }

        centroids->commit();
    }

    if(!runProcConf.functionName.empty()) {
        if (saved) {
            KmeansFunctionConfig funcConf;
            funcConf.modelFileUrl = runProcConf.modelFileUrl;

            PolyConfig kmeansFuncPC;
            kmeansFuncPC.type = "kmeans";
            kmeansFuncPC.id = runProcConf.functionName;
            kmeansFuncPC.params = funcConf;

            createFunction(engine, kmeansFuncPC, onProgress, true);
        } else {
            throw AnnotatedException(400, "Can't create kmeans function '" +
                                      runProcConf.functionName.rawString() +
                                      "'. Have you provided a valid modelFileUrl?",
                                      "modelFileUrl", runProcConf.modelFileUrl.toString());
        }
    }

    return Any();
}

DEFINE_STRUCTURE_DESCRIPTION(KmeansFunctionConfig);

KmeansFunctionConfigDescription::
KmeansFunctionConfigDescription()
{
    addField("modelFileUrl", &KmeansFunctionConfig::modelFileUrl,
             "URL of the model file (with extension '.kms') to load. "
             "This file is created by the ![](%%doclink kmeans.train procedure).");

    onPostValidate = [] (KmeansFunctionConfig * cfg,
                         JsonParsingContext & context) {
        // this includes empty url
        if(!cfg->modelFileUrl.valid()) {
            throw MLDB::Exception("modelFileUrl \"" + cfg->modelFileUrl.toString()
                                + "\" is not valid");
        }
    };
}

/*****************************************************************************/
/* KMEANS FUNCTION                                                           */
/*****************************************************************************/

DEFINE_STRUCTURE_DESCRIPTION(KmeansFunctionArgs);

KmeansFunctionArgsDescription::
KmeansFunctionArgsDescription()
{
    addField("embedding", &KmeansFunctionArgs::embedding,
             "Embedding values for the k-means function.  The column names in "
             "this embedding must match those used in the original kmeans "
             "dataset.");
}

DEFINE_STRUCTURE_DESCRIPTION(KmeansExpressionValue);

KmeansExpressionValueDescription::KmeansExpressionValueDescription()
{
    addField("cluster", &KmeansExpressionValue::cluster,
             "Index of the row in the `centroids` dataset whose columns describe "
             "the point which is closest to the input according to the `metric` "
             "specified in training.");
}

struct KmeansFunction::Impl {
    ML::KMeans kmeans;
    std::vector<ColumnPath> columnNames;

    Impl(const Url & modelFileUrl)
    {
        filter_istream stream(modelFileUrl);
        std::string firstLine;
        std::getline(stream, firstLine);
        Json::Value md = Json::parse(firstLine);
        if (md["algorithm"] != "MLDB k-Means model") {
            throw AnnotatedException(400, "Model file is not a k-means model");
        }
        if (md["version"].asInt() != 1) {
            throw AnnotatedException(400, "k-Means model version is wrong");
        }
        columnNames = jsonDecode<std::vector<ColumnPath> >(md["columnNames"]);

        MLDB::DB::Store_Reader store(stream);
        kmeans.reconstitute(store);
    }
};

KmeansFunction::
KmeansFunction(MldbEngine * owner,
               PolyConfig config,
               const std::function<bool (const Json::Value &)> & onProgress)
    : BaseT(owner, config)
{
    functionConfig = config.params.convert<KmeansFunctionConfig>();

    impl.reset(new Impl(functionConfig.modelFileUrl));

    dimension = impl->kmeans.clusters[0].centroid.size();
}

KmeansExpressionValue
KmeansFunction::
call(KmeansFunctionArgs input) const
{
    Date ts = input.embedding.getEffectiveTimestamp();

    return {ExpressionValue
            (impl->kmeans.assign
             (input.embedding.getEmbedding(impl->columnNames.data(),
                                           impl->columnNames.size())
              .cast<float>()),
             ts)};
}

namespace {

RegisterProcedureType<KmeansProcedure, KmeansConfig>
regKmeans(builtinPackage(),
          "Simple clustering algorithm based on cluster centroids in embedding space",
          "procedures/KmeansProcedure.md.html");

RegisterFunctionType<KmeansFunction, KmeansFunctionConfig>
regKmeansFunction(builtinPackage(),
                  "kmeans",
                  "Apply a k-means clustering to new data",
                  "functions/Kmeans.md.html");

} // file scope

} // namespace MLDB


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_CPP_INT_CORE_HPP
#define BOOST_MP_CPP_INT_CORE_HPP

#include <boost/integer.hpp>
#include <boost/integer_traits.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/static_assert.hpp>
#include <boost/assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {

            namespace detail {

                //
                // These traits calculate the largest type in the list
                // [unsigned] boost::long_long_type, long, int, which has the specified number
                // of bits.  Note that intN_t and boost::int_t<N> find the first
                // member of the above list, not the last.  We want the last in the
                // list to ensure that mixed arithmetic operations are as efficient
                // as possible.
                //
                template<unsigned N>
                struct largest_signed_type {
                    typedef typename boost::mpl::if_c<
                        1 + std::numeric_limits<boost::long_long_type>::digits == N, boost::long_long_type,
                        typename boost::mpl::if_c<
                            1 + std::numeric_limits<long>::digits == N, long,
                            typename boost::mpl::if_c<1 + std::numeric_limits<int>::digits == N, int,
                                                      typename boost::int_t<N>::exact>::type>::type>::type type;
                };

                template<unsigned N>
                struct largest_unsigned_type {
                    typedef typename boost::mpl::if_c<
                        std::numeric_limits<boost::ulong_long_type>::digits == N, boost::ulong_long_type,
                        typename boost::mpl::if_c<
                            std::numeric_limits<unsigned long>::digits == N, unsigned long,
                            typename boost::mpl::if_c<std::numeric_limits<unsigned int>::digits == N, unsigned int,
                                                      typename boost::uint_t<N>::exact>::type>::type>::type type;
                };

            }    // namespace detail

#if defined(BOOST_HAS_INT128)

            typedef detail::largest_unsigned_type<64>::type limb_type;
            typedef detail::largest_signed_type<64>::type signed_limb_type;
            typedef boost::uint128_type double_limb_type;
            typedef boost::int128_type signed_double_limb_type;
            static const limb_type max_block_10 = 1000000000000000000uLL;
            static const limb_type digits_per_block_10 = 18;

            inline BOOST_MP_CXX14_CONSTEXPR limb_type block_multiplier(unsigned count) {
#ifdef BOOST_NO_CXX14_CONSTEXPR
                static
#else
                constexpr
#endif
                    const limb_type values[digits_per_block_10] = {10,
                                                                   100,
                                                                   1000,
                                                                   10000,
                                                                   100000,
                                                                   1000000,
                                                                   10000000,
                                                                   100000000,
                                                                   1000000000,
                                                                   10000000000,
                                                                   100000000000,
                                                                   1000000000000,
                                                                   10000000000000,
                                                                   100000000000000,
                                                                   1000000000000000,
                                                                   10000000000000000,
                                                                   100000000000000000,
                                                                   1000000000000000000};
                BOOST_ASSERT(count < digits_per_block_10);
                return values[count];
            }

// Can't do formatted IO on an __int128
#define BOOST_MP_NO_DOUBLE_LIMB_TYPE_IO

            // Need to specialise integer_traits for __int128 as it's not a normal native type:
        }    // namespace multiprecision
    }
}

namespace boost {
    template<>
    class integer_traits<nil::crypto3::multiprecision::double_limb_type>
        : public std::numeric_limits<nil::crypto3::multiprecision::double_limb_type>,
          public detail::integer_traits_base<nil::crypto3::multiprecision::double_limb_type, 0,
                                             ~static_cast<nil::crypto3::multiprecision::double_limb_type>(0)> { };
    template<>
    class integer_traits<nil::crypto3::multiprecision::signed_double_limb_type>
        : public std::numeric_limits<nil::crypto3::multiprecision::signed_double_limb_type>,
          public detail::integer_traits_base<
              nil::crypto3::multiprecision::signed_double_limb_type,
              static_cast<nil::crypto3::multiprecision::signed_double_limb_type>(
                  (static_cast<nil::crypto3::multiprecision::double_limb_type>(1) << 127)),
              static_cast<nil::crypto3::multiprecision::signed_double_limb_type>(
                  ((~static_cast<nil::crypto3::multiprecision::double_limb_type>(0)) >> 1))> { };
}    // namespace boost

namespace nil {
    namespace crypto3 {
        namespace multiprecision {

#else

            typedef detail::largest_unsigned_type<32>::type limb_type;
            typedef detail::largest_signed_type<32>::type signed_limb_type;
            typedef detail::largest_unsigned_type<64>::type double_limb_type;
            typedef detail::largest_signed_type<64>::type signed_double_limb_type;
            static const limb_type max_block_10 = 1000000000;
            static const limb_type digits_per_block_10 = 9;

            inline limb_type block_multiplier(unsigned count) {
#ifdef BOOST_NO_CXX14_CONSTEXPR
                static
#else
                constexpr
#endif
                    const limb_type values[digits_per_block_10] = {10,      100,      1000,      10000,     100000,
                                                                   1000000, 10000000, 100000000, 1000000000};
                BOOST_ASSERT(count < digits_per_block_10);
                return values[count];
            }

#endif

            static const unsigned bits_per_limb = sizeof(limb_type) * CHAR_BIT;

            template<class T>
            inline BOOST_MP_CXX14_CONSTEXPR void minmax(const T& a, const T& b, T& aa, T& bb) {
                if (a < b) {
                    aa = a;
                    bb = b;
                } else {
                    aa = b;
                    bb = a;
                }
            }

            enum cpp_integer_type {
                signed_magnitude = 1,
                unsigned_magnitude = 0,
                signed_packed = 3,
                unsigned_packed = 2
            };

            enum cpp_int_check_type { checked = 1, unchecked = 0 };

        }    // namespace multiprecision
    }        // namespace crypto3
}    // namespace nil

//
// Figure out whether to support user-defined-literals or not:
//
#if !defined(BOOST_NO_CXX11_VARIADIC_TEMPLATES) && !defined(BOOST_NO_CXX11_USER_DEFINED_LITERALS) && \
    !defined(BOOST_NO_CXX11_CONSTEXPR)
#define BOOST_MP_USER_DEFINED_LITERALS
#endif

#endif    // BOOST_MP_CPP_INT_CORE_HPP


#ifndef FEATURE_EXTRACTION_H
#define FEATURE_EXTRACTION_H
// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the dlib C++
    Library.  In it, we will show how to do face recognition.  This example uses the
    pretrained dlib_face_recognition_resnet_model_v1 model which is freely available from
    the dlib web site.  This model has a 99.38% accuracy on the standard LFW face
    recognition benchmark, which is comparable to other state-of-the-art methods for face
    recognition as of February 2017.

    In this example, we will use dlib to do face clustering.  Included in the examples
    folder is an image, bald_guys.jpg, which contains a bunch of photos of action movie
    stars Vin Diesel, The Rock, Jason Statham, and Bruce Willis.   We will use dlib to
    automatically find their faces in the image and then to automatically determine how
    many people there are (4 in this case) as well as which faces belong to each person.

    Finally, this example uses a network with the loss_metric loss.  Therefore, if you want
    to learn how to train your own models, or to get a general introduction to this loss
    layer, you should read the dnn_metric_learning_ex.cpp and
    dnn_metric_learning_on_images_ex.cpp examples.
*/

#include "opencv4/opencv2/opencv.hpp"
#include <dlib/string.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/dnn.h>

#include <dlib/opencv.h>

using namespace dlib;
using namespace std;

// ----------------------------------------------------------------------------------------

// The next bit of code defines a ResNet network.  It's basically copied
// and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
// layer with loss_metric and made the network somewhat smaller.  Go read the introductory
// dlib DNN examples to learn what all this stuff means.
//
// Also, the dnn_metric_learning_on_images_ex.cpp example shows how to train this network.
// The dlib_face_recognition_resnet_model_v1 model used by this example was trained using
// essentially the code shown in dnn_metric_learning_on_images_ex.cpp except the
// mini-batches were made larger (35x15 instead of 5x5), the iterations without progress
// was set to 10000, and the training dataset consisted of about 3 million images instead of
// 55.  Also, the input layer was locked to images of size 150.
template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

// ----------------------------------------------------------------------------------------

std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img
);

// ----------------------------------------------------------------------------------------
class FeatureExtraction {
public:

	static void TrainAndSerializeDataset(std::vector<cv::Mat> image, std::vector<string> labels) try
	{
		// The first thing we are going to do is load all our models.  First, since we need to
		// find faces in the image we will need a face detector:
		frontal_face_detector detector = get_frontal_face_detector();
		// We will also use a face landmarking model to align faces to a standard pose:  (see face_landmark_detection_ex.cpp for an introduction)
		shape_predictor sp;
		deserialize("shape_predictor_5_face_landmarks.dat") >> sp;
		// And finally we load the DNN responsible for face recognition.
		anet_type net;
		deserialize("dlib_face_recognition_resnet_model_v1.dat") >> net;

		std::vector<dlib::cv_image<bgr_pixel>> imgs;
		std::cout << "[FTREXTRC]: Converting all images to dlib pixels" << std::endl;
		for(auto i : image){
			dlib::cv_image<bgr_pixel> tmp(i);
			imgs.push_back(tmp);
		}

		// Run the face detector on the image of our action heroes, and for each face extract a
		// copy that has been normalized to 150x150 pixels in size and appropriately rotated
		// and centered.
		std::cout << "[FTREXTRC]: Looping through all faces and creating matrices" << std::endl;
		std::vector<matrix<rgb_pixel>> faces;
		int i = 0;
		for (auto img : imgs){
			i++;
			std::cout << "[FTREXTRC]: Processing new image; " << i << " of images" << imgs.size() << std::endl;
			for (auto face : detector(img))
			{
				auto shape = sp(img, face);
				matrix<rgb_pixel> face_chip;
				extract_image_chip(img, get_face_chip_details(shape,150,0.25), face_chip);
				faces.push_back(move(face_chip));
			}
		}

		if (faces.size() == 0)
		{
			std::cout << "No faces found in image!" << endl;
			return;
		}

		// This call asks the DNN to convert each face image in faces into a 128D vector.
		// In this 128D vector space, images from the same person will be close to each other
		// but vectors from different people will be far apart.  So we can use these vectors to
		// identify if a pair of images are from the same person or from different people.

		// Add more elements to our saved encodings
		std::cout << "[FTREXTRC]: Serializing 128D vectors" << std::endl;
		std::vector<matrix<float,0,1>> face_descriptors = net(faces);
		serialize("faceDesc.dat") << face_descriptors;

		std::cout << "[FTREXTRC]: Serializing labels" << std::endl;
		serialize("faceDescLabels.dat") << labels;
		std::cout << "[FTREXTRC]: Feature extraction finished" << std::endl;
	}
	catch (std::exception& e)
	{
		cout << e.what() << endl;
	}

}; // class

#endif // incl saveguard