#include "openvslam/data/keyframe.h"
#include "openvslam/data/landmark.h"
#include "openvslam/data/map_database.h"
#include "openvslam/optimize/graph_optimizer.h"
#include "openvslam/optimize/internal/sim3/shot_vertex.h"
#include "openvslam/optimize/internal/sim3/graph_opt_edge.h"
#include "openvslam/util/converter.h"

#include <Eigen/StdVector>
#include <g2o/core/solver.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

namespace openvslam {
namespace optimize {

graph_optimizer::graph_optimizer(data::map_database* map_db, const bool fix_scale)
    : map_db_(map_db), fix_scale_(fix_scale) {}

void graph_optimizer::optimize(const std::shared_ptr<data::keyframe>& loop_keyfrm, const std::shared_ptr<data::keyframe>& curr_keyfrm,
                               const module::keyframe_Sim3_pairs_t& non_corrected_Sim3s,
                               const module::keyframe_Sim3_pairs_t& pre_corrected_Sim3s,
                               const std::map<std::shared_ptr<data::keyframe>, std::set<std::shared_ptr<data::keyframe>>>& loop_connections) const {
    // 1. Construct an optimizer

    auto linear_solver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_7_3::PoseMatrixType>>();
    auto block_solver = g2o::make_unique<g2o::BlockSolver_7_3>(std::move(linear_solver));
    auto algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);

    // 2. Add vertices

    const auto all_keyfrms = map_db_->get_all_keyframes();
    const auto all_lms = map_db_->get_all_landmarks();

    const unsigned int max_keyfrm_id = map_db_->get_max_keyframe_id();

    // Transform the pre-modified poses of all the keyframes to Sim3, and save them
    std::vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> Sim3s_cw(max_keyfrm_id + 1);
    // Save the added vertices
    std::vector<internal::sim3::shot_vertex*> vertices(max_keyfrm_id + 1);

    constexpr int min_weight = 100;

    for (auto keyfrm : all_keyfrms) {
        if (keyfrm->will_be_erased()) {
            continue;
        }
        auto keyfrm_vtx = new internal::sim3::shot_vertex();

        const auto id = keyfrm->id_;

        // BEFORE optimization, check if the poses have been already modified
        const auto iter = pre_corrected_Sim3s.find(keyfrm);
        if (iter != pre_corrected_Sim3s.end()) {
            // BEFORE optimization, set the already-modified poses for verices
            Sim3s_cw.at(id) = iter->second;
            keyfrm_vtx->setEstimate(iter->second);
        }
        else {
            // Transform an unmodified pose to Sim3, and set it for a vertex
            const Mat33_t rot_cw = keyfrm->get_rotation();
            const Vec3_t trans_cw = keyfrm->get_translation();
            const g2o::Sim3 Sim3_cw(rot_cw, trans_cw, 1.0);

            Sim3s_cw.at(id) = Sim3_cw;
            keyfrm_vtx->setEstimate(Sim3_cw);
        }

        // Fix the loop keyframe
        if (*keyfrm == *loop_keyfrm) {
            keyfrm_vtx->setFixed(true);
        }

        // Set the vertex to the optimizer
        keyfrm_vtx->setId(id);
        keyfrm_vtx->fix_scale_ = fix_scale_;

        optimizer.addVertex(keyfrm_vtx);
        vertices.at(id) = keyfrm_vtx;
    }

    // 3. Add edges

    // Save keyframe pairs which the edge is inserted between
    std::set<std::pair<unsigned int, unsigned int>> inserted_edge_pairs;

    // Function to add a constraint edge
    const auto insert_edge =
        [&optimizer, &vertices, &inserted_edge_pairs](unsigned int id1, unsigned int id2, const g2o::Sim3& Sim3_21) {
            auto edge = new internal::sim3::graph_opt_edge();
            edge->setVertex(0, vertices.at(id1));
            edge->setVertex(1, vertices.at(id2));
            edge->setMeasurement(Sim3_21);

            edge->information() = MatRC_t<7, 7>::Identity();

            optimizer.addEdge(edge);
            inserted_edge_pairs.insert(std::make_pair(std::min(id1, id2), std::max(id1, id2)));
        };

    // Add loop edges only over the weight threshold
    for (const auto& loop_connection : loop_connections) {
        auto keyfrm = loop_connection.first;
        const auto& connected_keyfrms = loop_connection.second;

        const auto id1 = keyfrm->id_;
        const g2o::Sim3& Sim3_1w = Sim3s_cw.at(id1);
        const g2o::Sim3 Sim3_w1 = Sim3_1w.inverse();

        for (auto connected_keyfrm : connected_keyfrms) {
            const auto id2 = connected_keyfrm->id_;

            // Except the current vs loop edges,
            // Add the loop edges only over the weight threshold
            if (!(id1 == curr_keyfrm->id_ && id2 == loop_keyfrm->id_)
                && keyfrm->graph_node_->get_weight(connected_keyfrm) < min_weight) {
                continue;
            }

            // Compute the relative camera pose
            const g2o::Sim3& Sim3_2w = Sim3s_cw.at(id2);
            const g2o::Sim3 Sim3_21 = Sim3_2w * Sim3_w1;

            // Add a constraint edge
            insert_edge(id1, id2, Sim3_21);
        }
    }

    // Add non-loop-connected edges
    for (auto keyfrm : all_keyfrms) {
        // Select one pose of the keyframe pair
        const auto id1 = keyfrm->id_;

        // Use only non-modified poses in the covisibility information
        // (Both camera poses should be non-modified in order to compute the relative pose correctly)
        const auto iter1 = non_corrected_Sim3s.find(keyfrm);
        const g2o::Sim3 Sim3_w1 = ((iter1 != non_corrected_Sim3s.end()) ? iter1->second : Sim3s_cw.at(id1)).inverse();

        auto parent_node = keyfrm->graph_node_->get_spanning_parent();
        if (parent_node) {
            const auto id2 = parent_node->id_;

            // Avoid duplication
            if (id1 <= id2) {
                continue;
            }

            // Use only non-modified poses in the covisibility information
            // (Both camera poses should be nop-modified in order to compute the relative pose correctly)
            const auto iter2 = non_corrected_Sim3s.find(parent_node);
            const g2o::Sim3& Sim3_2w = (iter2 != non_corrected_Sim3s.end()) ? iter2->second : Sim3s_cw.at(id2);

            // Compute the relative camera pose
            const g2o::Sim3 Sim3_21 = Sim3_2w * Sim3_w1;

            // Add a constraint edge
            insert_edge(id1, id2, Sim3_21);
        }

        // Add all the loop edges with any weight
        const auto loop_edges = keyfrm->graph_node_->get_loop_edges();
        for (auto connected_keyfrm : loop_edges) {
            const auto id2 = connected_keyfrm->id_;

            // Avoid duplication
            if (id1 <= id2) {
                continue;
            }

            // Use only non-modified poses in the covisibility information
            // (Both camera poses should be nop-modified in order to compute the relative pose correctly)
            const auto iter2 = non_corrected_Sim3s.find(connected_keyfrm);
            const g2o::Sim3& Sim3_2w = (iter2 != non_corrected_Sim3s.end()) ? iter2->second : Sim3s_cw.at(id2);

            // Compute the relative camera pose
            const g2o::Sim3 Sim3_21 = Sim3_2w * Sim3_w1;

            // Add a constraint edge
            insert_edge(id1, id2, Sim3_21);
        }

        // Add the covisibility information over the weight threshold
        const auto connected_keyfrms = keyfrm->graph_node_->get_covisibilities_over_weight(min_weight);
        for (auto connected_keyfrm : connected_keyfrms) {
            // null check
            if (!connected_keyfrm || !parent_node) {
                continue;
            }
            // Exclude parent-child edges because they've been already inserted
            if (*connected_keyfrm == *parent_node
                || keyfrm->graph_node_->has_spanning_child(connected_keyfrm)) {
                continue;
            }
            // Exclude any edges associated to the loop because they've been already inserted
            if (static_cast<bool>(loop_edges.count(connected_keyfrm))) {
                continue;
            }

            if (connected_keyfrm->will_be_erased()) {
                continue;
            }

            const auto id2 = connected_keyfrm->id_;

            // Avoid duplication
            if (id1 <= id2) {
                continue;
            }
            if (static_cast<bool>(inserted_edge_pairs.count(std::make_pair(std::min(id1, id2), std::max(id1, id2))))) {
                continue;
            }

            // Use only non-modified poses in the covisibility information
            // (Both camera poses should be nop-modified in order to compute the relative pose correctly)
            const auto iter2 = non_corrected_Sim3s.find(connected_keyfrm);
            const g2o::Sim3& Sim3_2w = (iter2 != non_corrected_Sim3s.end()) ? iter2->second : Sim3s_cw.at(id2);

            // Compute the relative camera pose
            const g2o::Sim3 Sim3_21 = Sim3_2w * Sim3_w1;

            // Add a constraint edge
            insert_edge(id1, id2, Sim3_21);
        }
    }

    // 4. Perform a pose graph optimization

    optimizer.initializeOptimization();
    optimizer.optimize(50);

    // 5. Update the camera poses and point-cloud

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        // For modification of a point-cloud, save the post-modified poses of all the keyframes
        std::vector<g2o::Sim3, Eigen::aligned_allocator<g2o::Sim3>> corrected_Sim3s_wc(max_keyfrm_id + 1);

        for (auto keyfrm : all_keyfrms) {
            const auto id = keyfrm->id_;

            auto keyfrm_vtx = static_cast<internal::sim3::shot_vertex*>(optimizer.vertex(id));

            const g2o::Sim3& corrected_Sim3_cw = keyfrm_vtx->estimate();
            const float s = corrected_Sim3_cw.scale();
            const Mat33_t rot_cw = corrected_Sim3_cw.rotation().toRotationMatrix();
            const Vec3_t trans_cw = corrected_Sim3_cw.translation() / s;

            const Mat44_t cam_pose_cw = util::converter::to_eigen_cam_pose(rot_cw, trans_cw);
            keyfrm->set_cam_pose(cam_pose_cw);

            corrected_Sim3s_wc.at(id) = corrected_Sim3_cw.inverse();
        }

        // Update the point-cloud
        for (const auto& lm : all_lms) {
            if (lm->will_be_erased()) {
                continue;
            }

            const auto id = (lm->loop_fusion_identifier_ == curr_keyfrm->id_)
                                ? lm->ref_keyfrm_id_in_loop_fusion_
                                : lm->get_ref_keyframe()->id_;

            const g2o::Sim3& Sim3_cw = Sim3s_cw.at(id);
            const g2o::Sim3& corrected_Sim3_wc = corrected_Sim3s_wc.at(id);

            const Vec3_t pos_w = lm->get_pos_in_world();
            const Vec3_t corrected_pos_w = corrected_Sim3_wc.map(Sim3_cw.map(pos_w));

            lm->set_pos_in_world(corrected_pos_w);
            lm->update_normal_and_depth();
        }
    }
}

} // namespace optimize
} // namespace openvslam


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REC_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/rec.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/if_one_else_zero.hpp>
#include <boost/simd/include/functions/simd/if_else_allbits.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( rec_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<uint_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else_allbits(a0, if_one_else_zero(a0 == One<A0>()));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( rec_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<int_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else(a0, if_else_zero(abs(a0) == One<A0>(), a0), Valmax<A0>());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( rec_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return One<result_type>()/a0;
    }
  };
} } }

#endif


/**
 * @author Manuel Guenther <manuel.guenther@idiap.ch>
 * @date Thu Jun  5 17:47:55 CEST 2014
 *
 * @brief The C++ implementations of the Gabor jet similarities and disparity computation
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.ip.gabor/Similarity.h>
#include <boost/assign.hpp>

static const std::map<bob::ip::gabor::Similarity::SimilarityType, std::string> type_map = boost::assign::map_list_of
  (bob::ip::gabor::Similarity::SCALAR_PRODUCT, "ScalarProduct")
  (bob::ip::gabor::Similarity::CANBERRA, "Canberra")
  (bob::ip::gabor::Similarity::ABS_PHASE, "AbsPhase")
  (bob::ip::gabor::Similarity::DISPARITY, "Disparity")
  (bob::ip::gabor::Similarity::PHASE_DIFF, "PhaseDiff")
  (bob::ip::gabor::Similarity::PHASE_DIFF_PLUS_CANBERRA, "PhaseDiffPlusCanberra")
  ;

const std::string& bob::ip::gabor::Similarity::type_to_name(bob::ip::gabor::Similarity::SimilarityType type){
  return type_map.find(type)->second;
}

bob::ip::gabor::Similarity::SimilarityType bob::ip::gabor::Similarity::name_to_type(const std::string& type){
  for (auto it = type_map.begin(); it != type_map.end(); ++it)
    if (it->second == type)
      return it->first;
  throw std::runtime_error("The given similarity name '" + type + "' does not name an appropriate similarity function type.");
}

bob::ip::gabor::Similarity::Similarity(SimilarityType type, boost::shared_ptr<Transform> gwt)
:
  m_type(type),
  m_gwt(gwt),
  m_disparity(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN())
{
  // initialize, when required
  if (m_type >= DISPARITY){
    if (!m_gwt)
      throw std::runtime_error("The given similarity function type '" + type_to_name(m_type) + "' required to specify the Gabor wavelet transform!");
    init();
  }
}

bob::ip::gabor::Similarity::Similarity(bob::io::base::HDF5File& file)
{
  // load configuration from file
  load(file);
}

static double sqr(double x){return x*x;}

void bob::ip::gabor::Similarity::init(){
  m_confidences.resize(m_gwt->numberOfWavelets());
  m_confidences = 0.;
  m_phase_differences.resize(m_gwt->numberOfWavelets());
  m_phase_differences = 0.;
}

double bob::ip::gabor::Similarity::similarity(const Jet& jet1, const Jet& jet2) const{
  // compute the disparity, if required
  if (m_type < DISPARITY){
    switch (m_type){
      case SCALAR_PRODUCT:
        // normalized scalar product (we assume normalized Gabor jets here!)
        return blitz::dot(jet1.abs(), jet2.abs());
      case CANBERRA:{
        // Canberra similarity
        double sim = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        int size = jet1.length();
        for (int j = 0; j < size; ++j){
          sim += 1. - std::abs(a1(j) - a2(j)) / (a1(j) + a2(j));
        }
        return sim / size;
      }
      case ABS_PHASE:{
        // similarity with absloute values and cosine of phase differences
        double sim = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        const auto& p1 = jet1.phase(),& p2 = jet2.phase();
        int size = jet1.length();
        for (int j = 0; j < size; ++j){
          sim += a1(j) * a2(j) * cos(p1(j) - p2(j));
        }
        return sim;
      }
      default:
        throw std::runtime_error("This should not have happened. Please assure that newly generated Gabor jet similarity functions are actually implemented!");
    }

  } else {
    // here only the disparity-related functions should be computed
    // compute disparity
    disparity(jet1, jet2);

    const std::vector<blitz::TinyVector<double,2> >& kernels = m_gwt->waveletFrequencies();

    switch (m_type){
      case DISPARITY:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        for (int j = 0; j < m_confidences.extent(0); ++j){
          sum += m_confidences(j) * cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
        }
        return sum;
      } // DISPARITY

      case PHASE_DIFF:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        for (int j = 0; j < m_phase_differences.extent(0); ++j){
          sum += cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
        }
        return sum / jet1.length();
      } // PHASE_DIFF

      case PHASE_DIFF_PLUS_CANBERRA:{
        // compute the similarity using the estimated disparity
        double sum = 0.;
        const auto& a1 = jet1.abs(),& a2 = jet2.abs();
        for (int j = 0; j < m_phase_differences.extent(0); ++j){
          // add disparity term
          sum += cos(m_phase_differences(j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
          // add Canberra term
          sum += 1. - std::abs(a1(j) - a2(j)) / (a1(j) + a2(j));
        }
        return sum / (2. * jet1.length());
      }

      default:
        // this should never happen
        throw std::runtime_error("This should not have happened. Please check the implementation of the similarity() functions.");
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////  Disparity estimation  /////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
blitz::TinyVector<double,2> bob::ip::gabor::Similarity::disparity(const Jet& jet1, const Jet& jet2) const{

  // Here, only the disparity based similarity functions are executed
  bob::core::array::assertCZeroBaseContiguous(jet1.jet());
  bob::core::array::assertCZeroBaseContiguous(jet2.jet());
  bob::core::array::assertSameShape(jet1.jet(),jet2.jet());

  // compute confidence vectors
  compute_confidences(jet1, jet2);

  // now, compute the disparity
  compute_disparity();

  // return the disparity
  return m_disparity;
}

static double adjustPhase(double phase){
  return phase - (2.*M_PI)*round(phase / (2.*M_PI));
}

void bob::ip::gabor::Similarity::shift_phase(const Jet& jet, const Jet& reference, Jet& shifted) const{
  bob::core::array::assertSameShape(jet.jet(),reference.jet());
  bob::core::array::assertSameShape(jet.jet(),shifted.jet());

  // compute disparity between jet and reference jet
  disparity(jet, reference);

  // compute phase shift for each jet entry based on disparity vector
  const std::vector<blitz::TinyVector<double,2>>& kernels = m_gwt->waveletFrequencies();
  auto& data = shifted.jet();
  // copy data from original jet
  data = jet.jet();
  // shift phases according to the computed disparity
  for (int j = 0; j < m_phase_differences.extent(0); ++j){
    data(1,j) = adjustPhase(data(1,j) - m_disparity[0] * kernels[j][0] - m_disparity[1] * kernels[j][1]);
  }
}

void bob::ip::gabor::Similarity::compute_confidences(const Jet& jet1, const Jet& jet2) const{
  if (m_type < DISPARITY){
    throw std::runtime_error("The disparity computation is not supported for similarity type " + type());
  }
  if (jet1.length() != m_confidences.extent(0)){
    throw std::runtime_error((boost::format("The size of the Gabor jet (%d) and the number of wavelets in the Gabor wavelet transform (%d) differ!") % jet1.length() % m_confidences.extent(0)).str());
  }
  // first, fill confidence and phase difference vectors
  const auto& a1 = jet1.abs(),& a2 = jet2.abs(),& p1 = jet1.phase(),& p2 = jet2.phase();
  for (int j = 0; j < m_confidences.extent(0); ++j){
    m_confidences(j) = a1(j) * a2(j);
    m_phase_differences(j) = adjustPhase(p1(j) - p2(j));
  }
}

void bob::ip::gabor::Similarity::compute_disparity() const{
  // approximate the disparity from the phase differences
  double gamma_x_x = 0., gamma_x_y = 0., gamma_y_y = 0., phi_x = 0., phi_y = 0.;
  // initialize the disparity with 0
  m_disparity = 0.;

  const std::vector<blitz::TinyVector<double,2> >& kernels = m_gwt->waveletFrequencies();
  // iterate backwards through the vector to start with the lowest frequency wavelets
  for (int j = m_confidences.extent(0)-1, level = m_gwt->numberOfScales()-1; level >= 0; --level){
    for (int direction = m_gwt->numberOfDirections()-1; direction >= 0; --direction, --j){
      double
          kjx = kernels[j][1],
          kjy = kernels[j][0],
          conf = m_confidences(j),
          diff = m_phase_differences(j);

      // totalize gamma matrix
      gamma_x_x += kjx * kjx * conf;
      gamma_x_y += kjx * kjy * conf;
      gamma_y_y += kjy * kjy * conf;

      // totalize phi vector
      // estimate the number of cycles that we are off
      double nL = round((diff - m_disparity[1] * kjx - m_disparity[0] * kjy) / (2.*M_PI));
      // totalize corrected phi vector elements
      phi_x += (diff - nL * 2. * M_PI) * conf * kjx;
      phi_y += (diff - nL * 2. * M_PI) * conf * kjy;
    } // for direction

    // re-calculate disparity as d=\Gamma^{-1}\Phi of the (low frequency) wavelet scales that we used up to now
    double gamma_det = gamma_x_x * gamma_y_y - sqr(gamma_x_y);
    m_disparity[1] = (gamma_y_y * phi_x - gamma_x_y * phi_y) / gamma_det;
    m_disparity[0] = (gamma_x_x * phi_y - gamma_x_y * phi_x) / gamma_det;
  } // for level
}

void bob::ip::gabor::Similarity::save(bob::io::base::HDF5File& file) const{

  file.set("Type", type_to_name(m_type));
  if (m_type >= DISPARITY){
    file.createGroup("Transform");
    file.cd("Transform");
    m_gwt->save(file);
    file.cd("..");
  }
}

void bob::ip::gabor::Similarity::load(bob::io::base::HDF5File& file){
  // read value
  m_type = name_to_type(file.read<std::string>("Type"));

  if (m_type >= DISPARITY){
    file.cd("Transform");
    m_gwt.reset(new Transform(file));
    file.cd("..");

    init();
  }
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
#include"taskspaceonline3.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<geometry_msgs/Wrench.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include<termios.h>
#include<gazebo_msgs/LinkStates.h>
#include<sensor_msgs/JointState.h>
#include"pidcontroller.h"

using namespace  std;
using namespace  Eigen;

bool left_first=true;//right support in first step
bool simulation=false;
double T_begin;
double Tds=.75;
double Tss=.75;
double Tc;
int N_stride=300;

double saturate(double a, double min, double max){
    if(a<min){return min;ROS_INFO("subceeding!");}
    else if(a>max){return max;ROS_INFO("exceeding!");}
    else{return a;}
}

bool FileExists(QString path) {
    QFileInfo check_file(path);
    // check if file exists and if yes: Is it really a file and no directory?
    if (check_file.exists() && check_file.isFile()) {
        return true;
    } else {
        return false;
    }
}

QList <QByteArray> GetContent(QByteArray content)
{
    QList <QByteArray> result;
    int index=0;
    while (index<content.length()) {

        index=content.indexOf(":",index);
        if (index<0)
        {
            break;
        }

        QByteArray temp=content.mid(index,content.length());
        QByteArray line=temp.split('\n')[0];
        QList <QByteArray> values=line.split(':');
        if (values.length()>1){
            result.append(values[1]);
        }

        index++;
    }
    return result;
}

MatrixXd ExtractionOfMatrix(QByteArray data)
{
    MatrixXd mat;
    QByteArray insideBrackets=data.split(']')[0];
    insideBrackets= insideBrackets.split('[')[1];
    QList <QByteArray> rows=insideBrackets.split(';');
    QList <QByteArray> columns=rows[0].split(',');

    mat.resize(rows.length(),columns.length());

    //////initial mat values
    for (int i = 0; i < rows.length(); i++) {
        QList <QByteArray> currentCols=rows[i].split(',');
        for (int j = 0; j < currentCols.length(); j++) {
            mat(i,j) = currentCols[j].toDouble();

        }
    }

    return mat;
}

void numplot(double num,double min,double max){
    //⬛

    QString str;
    int l=100;
    int n=int((num-min)/(max-min)*l);
    if (num<min){n=0;}
    if (num>max){n=100;}
    str+=QString::number(min);
    str+="|";
    if (n<=l/2){
        for (int i = 0; i < n; ++i) {
            str+=" ";
        }
        for (int i = 0; i < l/2-n; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
    }
    else {
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
        for (int i = 0; i < n-l/2; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l-n; ++i) {
            str+=" ";
        }

    }

    str+="|";
    str+=QString::number(max);
    str+="=>";str+=QString::number(num);
    qDebug()<<str;
    qDebug()<<"";

}

void matrix_view(MatrixXd M){

    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {
            str+=QString::number(M(i,j));
            str+="   ";
        }
        qDebug()<<str;
    }
    qDebug()<<"";
}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;
    qDebug()<<"";
}

ros::Publisher pub1; ros::Publisher pub2; ros::Publisher pub3; ros::Publisher pub4;
ros::Publisher pub5; ros::Publisher pub6; ros::Publisher pub7; ros::Publisher pub8;
ros::Publisher pub9; ros::Publisher pub10; ros::Publisher pub11; ros::Publisher pub12;
ros::Publisher pub13; ros::Publisher pub14; ros::Publisher pub15; ros::Publisher pub16;
ros::Publisher pub17; ros::Publisher pub18; ros::Publisher pub19; ros::Publisher pub20;
ros::Publisher pub21; ros::Publisher pub22; ros::Publisher pub23; ros::Publisher pub24;
ros::Publisher pub25; ros::Publisher pub26; ros::Publisher pub27; ros::Publisher pub28;
ros::Publisher pub29; ros::Publisher pub30; ros::Publisher pub31;

void  SendGazebo(vector<double> ctrl){

    std_msgs::Float64 data;
    data.data=ctrl[1];
    pub1.publish(data);
    data.data=ctrl[2];
    pub2.publish(data);
    data.data=ctrl[3];
    pub3.publish(data);
    data.data=ctrl[4];
    pub4.publish(data);
    data.data=ctrl[5];
    pub5.publish(data);
    data.data=ctrl[6];
    pub6.publish(data);
    data.data=ctrl[7];
    pub7.publish(data);
    data.data=ctrl[8];
    pub8.publish(data);
    data.data=ctrl[9];
    pub9.publish(data);
    data.data=ctrl[10];
    pub10.publish(data);
    data.data=ctrl[11];
    pub11.publish(data);
    data.data=ctrl[12];
    pub12.publish(data);
    data.data=0;
    pub13.publish(data);
    data.data=0;
    pub14.publish(data);
    data.data=0;
    pub15.publish(data);
    data.data=0-10*M_PI/180;
    pub16.publish(data);
    data.data=0;
    pub17.publish(data);
    data.data=0;
    pub18.publish(data);
    data.data=0;
    pub19.publish(data);
    data.data=0;
    pub20.publish(data);
    data.data=0;
    pub21.publish(data);
    data.data=0;
    pub22.publish(data);
    data.data=0+10*M_PI/180;
    pub23.publish(data);
    data.data=0;
    pub24.publish(data);
    data.data=0;
    pub25.publish(data);
    data.data=0;
    pub26.publish(data);
    data.data=0;
    pub27.publish(data);
    data.data=0;
    pub28.publish(data);

}

void  SendGazebo(QList<LinkM> links,MatrixXd RollModifieds, double PitchModifieds, double theta_r, double phi_r, double theta_l, double phi_l){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;

    data.data=links[1].JointAngle;    pub1.publish(data);
    data.data=links[2].JointAngle+RollModifieds(0,0);    pub2.publish(data);
    data.data=links[3].JointAngle+PitchModifieds;    pub3.publish(data);
    data.data=links[4].JointAngle;    pub4.publish(data);
    // data.data=links[5].JointAngle+theta_r;    pub5.publish(data);
    data.data=saturate(links[5].JointAngle,-M_PI/5.4,M_PI/4)+theta_r;    pub5.publish(data);
    data.data=links[6].JointAngle+phi_r;    pub6.publish(data);
    data.data=links[7].JointAngle;    pub7.publish(data);
    data.data=links[8].JointAngle+RollModifieds(1,0);    pub8.publish(data);
    data.data=links[9].JointAngle+PitchModifieds;    pub9.publish(data);
    data.data=links[10].JointAngle;    pub10.publish(data);
    //data.data=links[11].JointAngle+theta_l;    pub11.publish(data);
    data.data=saturate(links[11].JointAngle,-M_PI/5.4,M_PI/4)+theta_l;    pub11.publish(data);
    data.data=links[12].JointAngle+phi_l;    pub12.publish(data);
    data.data=links[13].JointAngle;    pub13.publish(data);
    data.data=links[14].JointAngle;    pub14.publish(data);
    data.data=links[15].JointAngle;    pub15.publish(data);
    data.data=links[16].JointAngle;    pub16.publish(data);
    data.data=links[17].JointAngle;    pub17.publish(data);
    data.data=links[18].JointAngle;    pub18.publish(data);
    data.data=links[19].JointAngle;    pub19.publish(data);
    data.data=links[20].JointAngle;    pub20.publish(data);
    data.data=links[21].JointAngle;    pub21.publish(data);
    data.data=links[22].JointAngle;    pub22.publish(data);
    data.data=links[23].JointAngle;    pub23.publish(data);
    data.data=links[24].JointAngle;    pub24.publish(data);
    data.data=links[25].JointAngle;    pub25.publish(data);
    data.data=links[26].JointAngle;    pub26.publish(data);
    data.data=links[27].JointAngle;    pub27.publish(data);
    data.data=links[28].JointAngle;    pub28.publish(data);
    data.data=0;    pub29.publish(data);
    data.data=0;    pub30.publish(data);
    data.data=0;    pub31.publish(data);
}

void  SendGazebo_reverse(QList<LinkM> links,MatrixXd RollModifieds, double PitchModifieds, double theta_r, double phi_r, double theta_l, double phi_l){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;

    data.data=-links[7].JointAngle;    pub1.publish(data);
    data.data=-links[8].JointAngle+RollModifieds(0,0);    pub2.publish(data);
    data.data=links[9].JointAngle+PitchModifieds;    pub3.publish(data);
    data.data=links[10].JointAngle;    pub4.publish(data);
    // data.data=links[5].JointAngle+theta_r;    pub5.publish(data);
    data.data=saturate(links[11].JointAngle,-M_PI/5.4,M_PI/4)+theta_r;    pub5.publish(data);
    data.data=-links[12].JointAngle+phi_r;    pub6.publish(data);
    data.data=-links[1].JointAngle;    pub7.publish(data);
    data.data=-links[2].JointAngle+RollModifieds(1,0);    pub8.publish(data);
    data.data=links[3].JointAngle+PitchModifieds;    pub9.publish(data);
    data.data=links[4].JointAngle;    pub10.publish(data);
    //data.data=links[11].JointAngle+theta_l;    pub11.publish(data);
    data.data=saturate(links[5].JointAngle,-M_PI/5.4,M_PI/4)+theta_l;    pub11.publish(data);
    data.data=-links[6].JointAngle+phi_l;    pub12.publish(data);

    data.data=links[13].JointAngle;  pub13.publish(data);
    data.data=links[14].JointAngle;  pub14.publish(data);

    data.data=links[15].JointAngle; pub22.publish(data);
    data.data=-links[16].JointAngle; pub23.publish(data);
    data.data=-links[17].JointAngle; pub24.publish(data);
    data.data=links[18].JointAngle; pub25.publish(data);
    data.data=-links[19].JointAngle; pub26.publish(data);
    data.data=-links[20].JointAngle; pub27.publish(data);
    data.data=links[21].JointAngle; pub28.publish(data);

    data.data=links[22].JointAngle; pub15.publish(data);
    data.data=-links[23].JointAngle; pub16.publish(data);
    data.data=-links[24].JointAngle; pub17.publish(data);
    data.data=links[25].JointAngle; pub18.publish(data);
    data.data=-links[26].JointAngle; pub19.publish(data);
    data.data=-links[27].JointAngle; pub20.publish(data);
    data.data=links[28].JointAngle; pub21.publish(data);

    data.data=0;    pub29.publish(data);
    data.data=0;    pub30.publish(data);
    data.data=0;    pub31.publish(data);
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

MinimumJerkInterpolation CoefGen;

//data of left foot sensor
int a,b,c,d,e,f,g,h;

//angles of ankle adaptation
double teta_motor_L,teta_motor_R;//pitch
double phi_motor_L,phi_motor_R;//roll
double PitchModified;

int qc_offset[12];
bool qc_initial_bool;

//
int bump_pushed[8];
int bump_notpushed[8];
bool bump_initialize;
void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=8) {
        qDebug("the size of sensor data is in wrong");
        return;
    }
    if(bump_initialize){
        for (int i = 0; i < 8; ++i) {
            bump_pushed[i]=msg.data[i];
        }
        bump_initialize=false;
    }
    //ROS_INFO("I heard: [%d  %d %d %d %d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3],(int)msg.data[4],(int)msg.data[5],(int)msg.data[6],(int)msg.data[7]);
    int temp[8];

    bump_pushed[0]=1095;bump_pushed[1]= 849;bump_pushed[2]=3124;bump_pushed[3]=3003;
    bump_pushed[4]=3126;bump_pushed[5]=2925;bump_pushed[6]=1204;bump_pushed[7]=923;

    bump_notpushed[0]=1012;bump_notpushed[1]= 930;bump_notpushed[2]=3037;bump_notpushed[3]=3098;
    bump_notpushed[4]=3042;bump_notpushed[5]=3009;bump_notpushed[6]=1123;bump_notpushed[7]=1014;
    temp[0]=msg.data[0]-bump_notpushed[0];
    temp[1]=-1*(msg.data[1]-bump_notpushed[1]);
    temp[2]=msg.data[2]-bump_notpushed[2];
    temp[3]=-1*(msg.data[3]-bump_notpushed[3]);

    //normalizing data of sensors
    a=temp[0]*(100.0/(bump_pushed[0]-bump_notpushed[0]));
    b=temp[1]*(100.0/(bump_notpushed[1]-bump_pushed[1]));
    c=temp[2]*(100.0/(bump_pushed[2]-bump_notpushed[2]));
    d=temp[3]*(100.0/(bump_notpushed[3]-bump_pushed[3]));

    //ROS_INFO("I heard a b c d: [%d  %d %d %d]", a,b,c,d);

    temp[4]=msg.data[4]-bump_notpushed[4];
    temp[5]=-1*(msg.data[5]-bump_notpushed[5]);
    temp[6]=msg.data[6]-bump_notpushed[6];
    temp[7]=-1*(msg.data[7]-bump_notpushed[7]);

    //normalizing data of sensors
    e=temp[4]*(100.0/(bump_pushed[4]-bump_notpushed[4]));
    f=temp[5]*(100.0/(bump_notpushed[5]-bump_pushed[5]));
    g=temp[6]*(100.0/(bump_pushed[6]-bump_notpushed[6]));
    h=temp[7]*(100.0/(bump_notpushed[7]-bump_pushed[7]));

    // ROS_INFO("I heard e f g h: [%d  %d %d %d]", e,f,g,h);

    //deleting data with negative sign
    if (a<0){a=0;} if (b<0){b=0;} if (c<0){c=0;} if (d<0){d=0;}
    if (e<0){e=0;} if (f<0){f=0;} if (g<0){g=0;} if (h<0){h=0;}
}

void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        qc_initial_bool=false;

        ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);}
}

double Fzl,Fzr,Mxl,Mxr;
void FT_left_feedback(const geometry_msgs::Wrench &msg){
    Fzl=msg.force.z;
    Mxl=msg.torque.y;
}

void FT_right_feedback(const geometry_msgs::Wrench &msg){
    Fzr=msg.force.z;
    Mxr=msg.torque.x;
}

MatrixXd quater2rot(double w,double x,double y, double z){
    MatrixXd R(3,3);
    R<<w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y,
            2*x*y+2*w*z,w*w-x*x+y*y-z*z,2*y*z-2*w*x,
            2*x*z-2*w*y,2*y*z+2*w*x,w*w-x*x-y*y+z*z;
    return R;
}
double roll_imu=0;
int N=100;
double roll_imu_qeue[100];
double a_y=0;double a_z=0;
void IMU_feedback(const sensor_msgs::Imu &msg){
//   double w,x,y,z;
//   w=msg.orientation.w;
//   x=msg.orientation.x;
//   y=msg.orientation.y;
//   z=msg.orientation.z;
//   MatrixXd R(3,3);
//   R=quater2rot(w,x,y,z);
  // matrix_view(R);
//   roll_imu=acos(R(2,1));
   a_y=msg.linear_acceleration.y;
   a_z=msg.linear_acceleration.z;
   roll_imu_qeue[0]=atan2(a_y,a_z)/8;
   for (int i = 1; i < N; ++i) {
       roll_imu_qeue[i]=roll_imu_qeue[i-1];
   }
   roll_imu=0;
   for (int i = 0; i < N; ++i) {
       roll_imu+=roll_imu_qeue[i]/100;
   }

}

void ankle_states(const gazebo_msgs::LinkStates& msg){
    double x_left, x_right,y_left, y_right,z_left, z_right;
    Vector3d vec_A_E;
    Vector3d vec_B_F;
    Vector3d vec_C_G;
    Vector3d vec_D_H;

    vec_A_E<<-.085,
            -.08,
            -.11;
    vec_B_F<<.135,
            -.08,
            -.11;
    vec_C_G<<.135,
            .08,
            -.11;
    vec_D_H<<-.085,
            .08,
            -.11;

    x_left=msg.pose[7].position.x;
    x_right=msg.pose[13].position.x;
    y_left=msg.pose[7].position.y;
    y_right=msg.pose[13].position.y;
    z_left=msg.pose[7].position.z;
    z_right=msg.pose[13].position.z;

    MatrixXd R_left(3,3);
    MatrixXd R_right(3,3);
    R_left=quater2rot(msg.pose[7].orientation.w,msg.pose[7].orientation.x,msg.pose[7].orientation.y,msg.pose[7].orientation.z);
    R_right=quater2rot(msg.pose[13].orientation.w,msg.pose[13].orientation.x,msg.pose[13].orientation.y,msg.pose[13].orientation.z);
    Vector3d temp;
    temp=R_left*vec_A_E;    //A=temp(2)+z_left;
    a=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_B_F;    //B=temp(2)+z_left;
    b=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_C_G;    //C=temp(2)+z_left;
    c=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_D_H;    //D=temp(2)+z_left;
    d=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_right*vec_A_E;    //E=temp(2)+z_right;
    e=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_B_F;    //F=temp(2)+z_right;
    f=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_C_G;    //G=temp(2)+z_right;
    g=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_D_H;    //H=temp(2)+z_right;
    h=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    //ROS_INFO("z_left=%f  A=%d,B=%d,C=%d,D=%d\tz_right=%f  E=%d,F=%d,G=%d,H=%d",z_left,a,b,c,d,z_right,e,f,g,h);
    //    ROS_INFO("x_left=%f,x_right=%f,y_left=%f,y_right=%fz_left=%f,z_right=%f",x_left,x_right,y_left,y_right,z_left,z_right);
}

int main(int argc, char **argv)
{
    for (int i = 0; i < N; ++i) {
        roll_imu_qeue[i]=0;
    }

    QString address;

  //  address += "/home/milad/humanoid/surena4/src/trajectory_generation/src/OfflineWalkData/offlinepub.txt";
  //  address += "/home/milad/humanoid/surena4/src/trajectory_generation/src/OfflineWalkData/JointDataTestRadian2.txt";
     address += "/home/cast/humanoid/surena4/src/trajectory_generation/src/OfflineWalkData/JointData";
     address +="Tc1.5Td0.75";
//address += "TestRadian5.txt";

    if (!FileExists(address))
    {
        qWarning()<<"Invalid Robot data Path:"<<address;

    }
    QFile file(address);
    file.open(QFile::ReadWrite);
    QByteArray content;
    content = file.readAll();file.close();

    QList<QByteArray> Position= GetContent(content);
    int N =Position.count();
    MatrixXd positionmat[N];
    for (int i = 0; i < N; ++i) {
        positionmat[i]=ExtractionOfMatrix(Position[i]);
        //  matrix_view(positionmat[i]);

    }

    qc_initial_bool=!simulation;
    bump_initialize=false;

    vector<double> cntrl(13);
    QCgenerator QC;
    for (int i = 0; i < 12; ++i) {
        qc_offset[i]=0;
    }

    teta_motor_L=0;
    teta_motor_R=0;
    phi_motor_L=0;
    phi_motor_R=0;

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;
    int counter=0;
    double time=0;

    ros::init(argc, argv, "myNode");
    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
    ros::Publisher  contact_flag  = nh.advertise<std_msgs::Int32MultiArray>("contact_flag_timing",100);
    ros::Publisher  trajectory_data_pub  = nh.advertise<std_msgs::Float32MultiArray>("my_trajectory_data",100);
    std_msgs::Float32MultiArray trajectory_data;

    ros::Subscriber sub = nh.subscribe("/surena/bump_sensor_state", 1000, receiveFootSensor);
    ros::Subscriber ft_left = nh.subscribe("/surena/ft_l_state",1000,FT_left_feedback);
    ros::Subscriber ft_right = nh.subscribe("/surena/ft_r_state",1000,FT_right_feedback);
    ros::Subscriber imu = nh.subscribe("/surena/imu_state",1000,IMU_feedback);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
    if(simulation){//gazebo publishers
        pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
        pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
        pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
        pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
        pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
        pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
        pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
        pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
        pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
        pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
        pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
        pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
        pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
        pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
        pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
        pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
        pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
        pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
        pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
        pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
        pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
        pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
        pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
        pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
        pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
        pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
        pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
        pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);
        pub29 = nh.advertise<std_msgs::Float64>("rrbot/joint29_position_controller/command",1000);
        pub30 = nh.advertise<std_msgs::Float64>("rrbot/joint30_position_controller/command",1000);
        pub31 = nh.advertise<std_msgs::Float64>("rrbot/joint31_position_controller/command",1000);
    }
    ros::Subscriber ankleStates = nh.subscribe("/gazebo/link_states", 10, ankle_states);

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    MatrixXd P;

    ROS_INFO("press any key to start!");
    getch();
    ROS_INFO("started!");
Tc=Tds+Tss;
T_begin=6+Tds/2;
    while (ros::ok())
    {

        //  for robot test musbe uncommented

        if (qc_initial_bool) {
            ROS_INFO_ONCE("qc is initializing!");
            ros::spinOnce();
            continue;
        }

        cntrl[0]=0.0;
        for (int i = 1; i <=12 ; ++i) {
           // cntrl[i]= positionmat[count](0,i-1);
            cntrl[i]=0;
        }
        //matrix_view(positionmat[count]);

       cntrl[2]+=roll_imu;
       cntrl[8]+=roll_imu;
       cntrl[6]-=roll_imu;
       cntrl[12]-=roll_imu/10;

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        msg.data.clear();

        if(left_first){
            for(int  i = 0;i < 12;i++)
            {
                msg.data.push_back(qref[i]+qc_offset[i]);
            }}
        else{
            msg.data.push_back(-qref[5]+qc_offset[0]);
            msg.data.push_back(-qref[4]+qc_offset[1]);
            msg.data.push_back(-qref[6]+qc_offset[2]);
            msg.data.push_back(-qref[7]+qc_offset[3]);
            msg.data.push_back(-qref[1]+qc_offset[4]);
            msg.data.push_back(-qref[0]+qc_offset[5]);
            msg.data.push_back(-qref[2]+qc_offset[6]);
            msg.data.push_back(-qref[3]+qc_offset[7]);
            msg.data.push_back(-qref[11]+qc_offset[8]);
            msg.data.push_back(-qref[10]+qc_offset[9]);
            msg.data.push_back(-qref[9]+qc_offset[10]);
            msg.data.push_back(-qref[8]+qc_offset[11]);
        }

        for(int  i = 12;i < 28;i++)
        {
            msg.data.push_back(0);
        }
        if(!simulation){chatter_pub.publish(msg);}

        if (simulation){
            SendGazebo(cntrl);
            //            if(left_first){SendGazebo(links,0*RollModified,0*PitchModified,teta_motor_R,phi_motor_R,teta_motor_L,phi_motor_L);}
            //            else{SendGazebo_reverse(links,0*RollModified,0*PitchModified,teta_motor_R,phi_motor_R,teta_motor_L,phi_motor_L);}
        }

        trajectory_data.data.clear();
        trajectory_data.data.push_back(cntrl[1]);
        trajectory_data.data.push_back(cntrl[2]);
        trajectory_data.data.push_back(cntrl[3]);
        trajectory_data.data.push_back(cntrl[4]);
        trajectory_data.data.push_back(cntrl[5]);
        trajectory_data.data.push_back(cntrl[6]);
        trajectory_data.data.push_back(cntrl[7]);
        trajectory_data.data.push_back(cntrl[8]);
        trajectory_data.data.push_back(cntrl[9]);
        trajectory_data.data.push_back(cntrl[10]);
        trajectory_data.data.push_back(cntrl[11]);
        trajectory_data.data.push_back(cntrl[12]);
        trajectory_data_pub.publish(trajectory_data);

        if(count%20==0){ //use to print once in n steps
numplot(roll_imu,-M_PI/2,M_PI/2);
          //numplot(a_y,-5,5);
        }

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
        if(counter<N_stride){
        if(count==int((2*Tc+T_begin)*200)){
            count=int(T_begin*200);
            counter++;
        }}
        if(count>=N){break;}
        time+=.005;
       // qDebug()<<time<<"\t"<<counter<<"\t"<<count;
    }
    return 0;
}


/**
 * Copyright 2020 Monchack Audio
 *
 * 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.
 */

#include <iostream>

#include <Windows.h>

// reomve comment out below to use Boost
//#include <boost/multiprecision/cpp_dec_float.hpp>

// Tap size; change this number if necessary. Must be an odd number
#define TAP_SIZE 16383

// remove comment out below to enable high precision mode
//#define HIGH_PRECISION 1

#define DATA_UNIT_SIZE (1024 * 1024)

// 16(15+1)bit  X  scale: 48(47+1)bit =  63(62+1)bit -> 32bit (31bit shift)
#define COEFF_SCALE 47
#define SCALE_SHIFT 31

#if !defined(BOOST_VERSION)

void createHannCoeff(int tapNum, long long* dest, double* dest2)
{
	int coeffNum = (tapNum + 1) / 2;
	double* coeff1 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double* coeff2 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double* coeff3 = (double*)::GlobalAlloc(GPTR, sizeof(double) * coeffNum);
	double pi = 3.141592653589793;

	coeff1[0] = 2.0f * (22050.0f / 352800.0f);
	for (int i = 1; i < coeffNum; ++i)
	{
		double x = i * 2.0f * pi * (22050.0f / 352800.0f);
		coeff1[i] = sin(x) / (pi * i);
	}

	for (int i = 0; i < coeffNum; ++i)
	{
		double x = 2.0f * pi * i / (double)(tapNum - 1);
		coeff2[i] = 0.5f + 0.5f * cos(x);
	}
	coeff2[coeffNum - 1] = 0;

	long long scale = 1LL << (COEFF_SCALE + 3);

	for (int i = 0; i < coeffNum; ++i)
	{
		coeff3[i] = coeff1[i] * coeff2[i] * scale;
	}

	dest[coeffNum - 1] = (long long)round(coeff3[0]);
	dest2[coeffNum - 1] = (double)(coeff3[0] - round(coeff3[0]));
	for (int i = 1; i < coeffNum; ++i)
	{
		dest[coeffNum - 1 + i] = (long long)round(coeff3[i]);
		dest[coeffNum - 1 - i] = (long long)round(coeff3[i]);
		dest2[coeffNum - 1 + i] = (double)(coeff3[i] - round(coeff3[i]));
		dest2[coeffNum - 1 - i] = (double)(coeff3[i] - round(coeff3[i]));
	}
	::GlobalFree(coeff1);
	::GlobalFree(coeff2);
	::GlobalFree(coeff3);
}

#else

using namespace boost::multiprecision;
using boost::math::constants::pi;

void createHannCoeff(int tapNum, long long* dest, double* dest2)
{
	int coeffNum = (tapNum + 1) / 2;
	cpp_dec_float_100* coeff1 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);
	cpp_dec_float_100* coeff2 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);
	cpp_dec_float_100* coeff3 = (cpp_dec_float_100*)::GlobalAlloc(GPTR, sizeof(cpp_dec_float_100) * coeffNum);

	cpp_dec_float_100 piq = pi<cpp_dec_float_100>();

	coeff1[0] = cpp_dec_float_100(2) * 22050 / 352800;
	for (int i = 1; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = cpp_dec_float_100(i) * 2 * piq * 22050 / 352800;
		coeff1[i] = boost::multiprecision::sin(x) / (piq * i);
	}

	for (int i = 0; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = cpp_dec_float_100(2) * piq * i / (tapNum - 1);
		coeff2[i] = cpp_dec_float_100("0.5") + cpp_dec_float_100("0.5") * boost::multiprecision::cos(x);
	}
	coeff2[coeffNum - 1] = 0;

	long long scale = 1LL << (COEFF_SCALE + 3);

	for (int i = 0; i < coeffNum; ++i)
	{
		coeff3[i] = coeff1[i] * coeff2[i] * scale;
		//coeff3[i] = boost::multiprecision::round(coeff1[i] * coeff2[i] * scale);
	}

	dest[coeffNum - 1] = (long long)boost::multiprecision::round(coeff3[0]);
	dest2[coeffNum - 1] = (double)(coeff3[0] - boost::multiprecision::round(coeff3[0]));
	for (int i = 1; i < coeffNum; ++i)
	{
		cpp_dec_float_100 x = boost::multiprecision::round(coeff3[i]);
		dest[coeffNum - 1 + i] = (long long)x;
		dest[coeffNum - 1 - i] = (long long)x;
		dest2[coeffNum - 1 + i] = (double)(coeff3[i] - x);
		dest2[coeffNum - 1 - i] = (double)(coeff3[i] - x);
	}
	::GlobalFree(coeff1);
	::GlobalFree(coeff2);
	::GlobalFree(coeff3);
}

#endif

static void writeRaw32bitPCM(long long left, long long right, int* buffer)
{
	int shift = SCALE_SHIFT;

	int add = 1 << (shift - 1);
	left += add;
	right += add;

	if (left >= 4611686018427387904) left = 4611686018427387904 - 1; // over 63bit : limitted to under [1 << 62]   62bit + 1bit
	if (right >= 4611686018427387904) right = 4611686018427387904 - 1;

	if (left < -4611686018427387904) left = -4611686018427387904;
	if (right < -4611686018427387904) right = -4611686018427387904;

	left = left >> shift;
	right = right >> shift;

	buffer[0] = (int)left;
	buffer[1] = (int)right;
}

int  oversample(short* src, unsigned int length, long long* coeff, double* coeff2, int tapNum, int* dest, unsigned int option)
{
	int half_size = (tapNum - 1) / 2;
	if (option == 0) option = 0xffff;

	for (unsigned int i = 0; i < length; ++i)
	{
		short *srcLeft = src;
		short *srcRight = src + 1;
		long long tmpLeft, tmpRight;
		double tmpLeft2, tmpRight2;

		if (option & 0x0001)
		{
			// 1st
			tmpLeft = *srcLeft * coeff[half_size];
			tmpRight = *srcRight * coeff[half_size];
			writeRaw32bitPCM(tmpLeft, tmpRight, dest);
		}

		if (option & 0x0002)
		{
			// 2nd
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			// src[1] * coeff[ 7]  +  src[ 2] * coeff[15]  +  src[ 3] * coeff[ 23]  + ...
			// src[0] * coeff[-1]  +  src[-1] * coeff[-9]  +  src[-2] * coeff[-17]  + ...
			for (int j = 1; (j * 8 - 1) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 1];
				tmpRight += (long long)*(srcRight + j * 2) *coeff[half_size + j * 8 - 1];

				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 1];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 1];
				#endif
			}
			for (int j = 0; (j * 8 + 1) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 1];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 1];

				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 1];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 1];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 2);
		}

		if (option & 0x0004)
		{
			// 3rd
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			// src[1] * coeff[ 6]  +  src[ 2] * coeff[ 14]  +  src[ 3] * coeff[ 22]  + ...
			// src[0] * coeff[-2]  +  src[-1] * coeff[-10]  +  src[-2] * coeff[-18]  + ...
			for (int j = 1; (j * 8 - 2) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 2];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 2];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 2];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 2];
				#endif
			}
			for (int j = 0; (j * 8 + 2) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 2];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 2];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 2];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 2];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 4);
		}

		if (option & 0x0008)
		{
			// 4th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 3) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 3];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 3];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 3];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 3];
				#endif
			}
			for (int j = 0; (j * 8 + 3) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 3];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 3];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 3];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 3];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 6);
		}

		if (option & 0x0010)
		{
			//5th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 4) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 4];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 4];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 4];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 4];
				#endif
			}
			for (int j = 0; (j * 8 + 4) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 4];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 4];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 4];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 4];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 8);
		}

		if (option & 0x0020)
		{
			//6th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 5) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 5];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 5];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 5];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 5];
				#endif
			}
			for (int j = 0; (j * 8 + 5) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 5];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 5];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 5];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 5];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 10);
		}

		if (option & 0x0040)
		{
			//7th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 6) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 6];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 6];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 6];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 6];
				#endif
			}
			for (int j = 0; (j * 8 + 6) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 6];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 6];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 6];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 6];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 12);
		}

		if (option & 0x0080)
		{
			//8th
			tmpLeft = 0;
			tmpRight = 0;
			tmpLeft2 = 0.0;
			tmpRight2 = 0.0;
			for (int j = 1; (j * 8 - 7) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft + j * 2) * coeff[half_size + j * 8 - 7];
				tmpRight += (long long)*(srcRight + j * 2) * coeff[half_size + j * 8 - 7];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft + j * 2) * coeff2[half_size + j * 8 - 7];
				tmpRight2 += (double)*(srcRight + j * 2) *coeff2[half_size + j * 8 - 7];
				#endif
			}
			for (int j = 0; (j * 8 + 7) <= half_size; ++j)
			{
				tmpLeft += (long long)*(srcLeft - j * 2) * coeff[half_size - j * 8 - 7];
				tmpRight += (long long)*(srcRight - j * 2) * coeff[half_size - j * 8 - 7];
				#if defined(HIGH_PRECISION)
				tmpLeft2 += (double)*(srcLeft - j * 2) * coeff2[half_size - j * 8 - 7];
				tmpRight2 += (double)*(srcRight - j * 2) *coeff2[half_size - j * 8 - 7];
				#endif
			}
			tmpLeft += (long long)tmpLeft2;
			tmpRight += (long long)tmpRight2;
			writeRaw32bitPCM(tmpLeft, tmpRight, dest + 14);
		}

		src += 2;
		dest += 8 * 2;
	}

	return 0;
}

struct oversample_info
{
	short* src;
	unsigned int length;
	long long* coeff;
	double* coeff2;
	int tapNum;
	int* dest;
	unsigned int option;
};

DWORD WINAPI ThreadFunc(LPVOID arg)
{
	struct oversample_info* info = (struct oversample_info*)arg;
	oversample(info->src, info->length, info->coeff, info->coeff2, info->tapNum, info->dest, info->option);
	return 0;
}

unsigned int searchFmtDataChunk(wchar_t* fileName, WAVEFORMATEX* wf, DWORD* offset, DWORD* size)
{
	HANDLE fileHandle;
	fileHandle = CreateFileW(fileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
	if (fileHandle == INVALID_HANDLE_VALUE)
	{
		return 0;
	}

	DWORD header[2];
	DWORD readSize;
	WORD  wav[8];
	DWORD riffSize, pos = 0;
	DWORD dataOffset, dataSize;
	::ReadFile(fileHandle, header, 8, &readSize, NULL);
	bool fmtFound = false, dataFound = false;

	if (readSize != 8)
	{
		CloseHandle(fileHandle);
		return 0;
	}

	if (header[0] != 0x46464952)
	{
		// not "RIFF"
		CloseHandle(fileHandle);
		return 0;
	}
	riffSize = header[1];

	::ReadFile(fileHandle, header, 4, &readSize, NULL);
	if (readSize != 4)
	{
		CloseHandle(fileHandle);
		return 0;
	}
	if (header[0] != 0x45564157)
	{
		// not "WAVE"
		CloseHandle(fileHandle);
		return 0;
	}
	pos += 4;

	while (pos < riffSize)
	{
		::ReadFile(fileHandle, header, 8, &readSize, NULL);
		if (readSize != 8)
		{
			break;
		}
		pos += 8;

		if (header[0] == 0x20746d66)
		{
			// "fmt "
			if (header[1] >= 16)
			{
				::ReadFile(fileHandle, wav, 16, &readSize, NULL);
				if (readSize != 16)
				{
					break;
				}
				fmtFound = true;
				if (header[1] > 16)
				{
					::SetFilePointer(fileHandle, header[1] - 16, 0, FILE_CURRENT);
				}
				pos += header[1];
			}
			else
			{
				::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
				pos += header[1];
			}
		}
		else if (header[0] == 0x61746164)
		{
			// "data"
			dataFound = true;
			dataOffset = ::SetFilePointer(fileHandle, 0, 0, FILE_CURRENT);
			dataSize = header[1];
			::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
			pos += header[1];
		}
		else
		{
			::SetFilePointer(fileHandle, header[1], 0, FILE_CURRENT);
			pos += header[1];
		}
		if (GetLastError() != NO_ERROR)
		{
			break;
		}
	}
	CloseHandle(fileHandle);

	if (dataFound && fmtFound)
	{
		*offset = dataOffset;
		*size = dataSize;
		wf->wFormatTag = wav[0]; //  1:LPCM   3:IEEE float
		wf->nChannels = wav[1]; //  1:Mono  2:Stereo
		wf->nSamplesPerSec = *(DWORD*)(wav + 2);  // 44100, 48000, 176400, 19200, 352800, 384000...
		wf->nAvgBytesPerSec = *(DWORD*)(wav + 4);
		wf->nBlockAlign = wav[6]; // 4@16bit/2ch,  6@24bit/2ch,   8@32bit/2ch
		wf->wBitsPerSample = wav[7]; // 16bit, 24bit, 32bit
		wf->cbSize = 0;
		return 1;
	}
	return 0;
}

DWORD readWavFile(wchar_t* fileName, void* readMem, DWORD readPos, DWORD readLength)
{
	HANDLE fileHandle;
	DWORD wavDataOffset, wavDataSize, readSize = 0;
	WAVEFORMATEX wf;

	if (!searchFmtDataChunk(fileName, &wf, &wavDataOffset, &wavDataSize))
	{
		return 0;
	}

	fileHandle = CreateFileW(fileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
	if (fileHandle == INVALID_HANDLE_VALUE)
	{
		return 0;
	}

	if (::SetFilePointer(fileHandle, wavDataOffset + readPos, 0, FILE_BEGIN) == INVALID_SET_FILE_POINTER)
	{
		if (GetLastError() != NO_ERROR)
		{
			// fail
			return 0;
		}
	}
	::ReadFile(fileHandle, readMem, readLength, &readSize, NULL);
	::CloseHandle(fileHandle);

	return readSize;
}

static int writePCM352_32_header(HANDLE fileHandle, unsigned long dataSize)
{
	WAVEFORMATEX wf;
	wf.wFormatTag = 0x01;
	wf.nChannels = 2;
	wf.nSamplesPerSec = 352800;
	wf.nAvgBytesPerSec = 352800 * 8; // 352800 * 4byte(32bit) * 2ch
	wf.nBlockAlign = 8; // 8bytes (32bit, 2ch) per sample
	wf.wBitsPerSample = 32;
	wf.cbSize = 0; // ignored. not written.

	DWORD writtenSize = 0;
	WriteFile(fileHandle, "RIFF", 4, &writtenSize, NULL);
	DWORD size = (dataSize + 44) - 8;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);
	WriteFile(fileHandle, "WAVE", 4, &writtenSize, NULL);
	WriteFile(fileHandle, "fmt ", 4, &writtenSize, NULL);
	size = 16;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);
	WriteFile(fileHandle, &wf, size, &writtenSize, NULL);
	WriteFile(fileHandle, "data", 4, &writtenSize, NULL);
	size = (DWORD)dataSize;
	WriteFile(fileHandle, &size, 4, &writtenSize, NULL);

	return 0;
}

int wmain(int argc, wchar_t *argv[], wchar_t *envp[])
{
	DWORD wavDataOffset, wavDataSize, writtenSize, length, readSize = 0;
	WAVEFORMATEX wf;
	wchar_t* fileName;
	wchar_t* destFileName;

	if (argc < 2) return 0;
	fileName = argv[1];
	destFileName = argv[2];

	ULONGLONG elapsedTime = GetTickCount64();

	if (!searchFmtDataChunk(fileName, &wf, &wavDataOffset, &wavDataSize))
	{
		return 0;
	}
	int part = wavDataSize / DATA_UNIT_SIZE;
	if ((wavDataSize %  DATA_UNIT_SIZE) != 0) part += 1;

	void* mem1 = ::GlobalAlloc(GPTR, DATA_UNIT_SIZE * 3);
	void* mem2 = (char*)mem1 + DATA_UNIT_SIZE;
	void* mem3 = (char*)mem2 + DATA_UNIT_SIZE;

	void* memOut = ::GlobalAlloc(GPTR, DATA_UNIT_SIZE * 8 * 2);

	HANDLE fileOut = CreateFileW(destFileName, GENERIC_WRITE, 0, NULL, CREATE_ALWAYS /*CREATE_NEW*/, FILE_ATTRIBUTE_NORMAL, NULL);
	writePCM352_32_header(fileOut, wavDataSize * 8 * 2);

	long long* firCoeff = (long long*)::GlobalAlloc(GPTR, sizeof(long long) * TAP_SIZE);
	double* firCoeff2 = (double*)::GlobalAlloc(GPTR, sizeof(double) * TAP_SIZE);
	createHannCoeff(TAP_SIZE, firCoeff, firCoeff2);

	for (int i = 0; i <= part; ++i)
	{
		::SetThreadExecutionState(ES_SYSTEM_REQUIRED);

		length = readSize;
		::CopyMemory(mem1, mem2, DATA_UNIT_SIZE);
		::CopyMemory(mem2, mem3, DATA_UNIT_SIZE);
		::SecureZeroMemory(mem3, DATA_UNIT_SIZE);
		if (i != part) readSize = readWavFile(fileName, mem3, DATA_UNIT_SIZE * i, DATA_UNIT_SIZE);
		if (i == 0) continue;

		struct oversample_info info[8];
		info[0].src = (short* )mem2;
		info[0].length = length / 4;
		info[0].coeff = firCoeff;
		info[0].coeff2 = firCoeff2;
		info[0].tapNum = TAP_SIZE;
		info[0].dest = (int* )memOut;
		info[0].option = 0;

		// Single thread
		ThreadFunc((LPVOID)&info[0]);

		// Multi thread (use code below instead of above)
		/*
		HANDLE thread[8];
		DWORD threadId[8];
		for (int j = 0; j < 8; ++j)
		{
			info[j] = info[0];
			info[j].option = 1 << j;
			thread[j] = CreateThread(NULL, 0, ThreadFunc, (LPVOID)&info[j], 0, &threadId[j]);
		}
		::WaitForMultipleObjects(8, thread, TRUE, INFINITE);
		*/

		::WriteFile(fileOut, memOut, length * 8 * 2, &writtenSize, NULL);
		std::cout << "WavOverSampling: Progress  " << (i * 100) / part << " %\r";
	}
	elapsedTime = GetTickCount64() - elapsedTime;
	std::cout << "\nWavOverSampling: Completed.   " << (elapsedTime/1000) << "." << (elapsedTime % 1000) <<  " sec  \n";

	::FlushFileBuffers(fileOut);
	::CloseHandle(fileOut);

	::GlobalFree(mem1);
	::GlobalFree(memOut);
	::GlobalFree(firCoeff);
	::GlobalFree(firCoeff2);
	return 0;
}


// Boost
#include <cstdlib>
#include <string>
#include <boost/lexical_cast.hpp>

// inside a function or method...
std::string s = "12345";
int i = boost::lexical_cast<int>(s) + 1;
s = boost::lexical_cast<std::string>(i);


#include <fstream>

#include "Tauola/Tauola.h"
#include "Tauola/TauolaHepMCEvent.h"

#include "HepMC/GenEvent.h"
#include "HepMC/GenParticle.h"

//pythia header files

#include "tauola_print_parameters.h"
using namespace std;
using namespace Tauolapp;

#include <fstream>
#include <boost/algorithm/string.hpp>
#include "Eigen/Core"
#include "opts.h"
#include "tools.h"

std::tuple<Eigen::Vector4d, Eigen::Vector4d, Eigen::Vector4d> mkParticles(Eigen::VectorXd const & DIN) {
    Eigen::Vector4d nu, tau, w;
    tau << DIN[1], DIN[2], DIN[3], DIN[4];
    nu << DIN[0], 0, 0, DIN[0];
    //w =  tau - nu;
    w = tau + nu;
    return std::make_tuple(nu,tau,w);

}

inline HepMC::FourVector eigenToHepMC(Eigen::Vector4d const & vin) {
    HepMC::FourVector temp(vin[1], vin[2], vin[3], vin[0]);
    return temp;
}

int main(int argc, char * argv[]) {
    int mode(0);
    int nmax(-1);
    string fin = "";
    string foutname = "testout.csv";
    using namespace opts;
    Options ops(argc, argv);
    bool norad     = ops >> Present("norad", "switch off photon radiation");
    bool nospincorr  = ops >> Present("nospincorr", "switch off spin correlation radiation");
    bool verbose     = ops >> Present('v', "verbose", "verbose output");
    bool tauplus     = ops >> Present('p', "tauplus", "Treat taus as tau+");
    ops >> Option('m', "mode",   mode,   "Tauolo decay mode");
    ops >> Option('n', "nmax",   nmax,   "Max number of events to generate");
    ops >> Option('f', "ftau",   fin,   "Max number of events to generate");
    ops >> Option('o', "fout",   foutname,   "Max number of events to generate");
    if (ops >> Present('h', "help", "Show help"))
    {
        std::cout << "Usage: " << argv[0] << " [OPTIONS]\n";
        std::cout << ops;
        return 1;
    }

    if (fin ==foutname) {
      std::cerr << "Do not overwrite input file, exiting\n";
      exit(1);
    }

    std::cout<< "Reading taus from " << fin << "\n";
    std::cout<< "Running Tauola mode " << mode << "\n";
    if (norad) std::cerr << "Photon radiation turned OFF\n";
    else std::cerr << "Photon radiation turned on\n";
    auto const data = readCSV(fin.c_str());
    auto [eventids, taus] = convert(data);

    int nevents = taus.rows();
    if ( (nmax>0) &&  (nmax < nevents)) {
      nevents = nmax;
    }
    std::cout << "Processing " << nevents << " tau events\n";

    // Enforce a single decay mode
    Tauola::setSameParticleDecayMode(mode);
    Tauola::setOppositeParticleDecayMode(mode);
    Tauola::setRadiation(!norad);
    Tauola::setEtaK0sPi(1,1,1);

    Tauola::initialize();
    Tauola::spin_correlation.setAll(!nospincorr);

    if (verbose) tauola_print_parameters(); // Prints TAUOLA  parameters (residing inside its library): e.g. to test user interface

    //const double tau_mass = parmas_.amtau;

    std::fstream fout;
    // Open the file in truncate mode if first line else in Append Mode
    fout.open(foutname, std::ios::out | (std::ios::trunc));

    fout << "EVENTID,PDGID,E,PX,PY,PZ\n";

    for (int iEvent = 0; iEvent < nevents; ++iEvent) {
        HepMC::GenEvent * event = new HepMC::GenEvent();

        //std::cerr <<  taus.row(iEvent) << "\n";
        auto [nu, tau, w] = mkParticles(taus.row(iEvent));
        //abort();

        auto mom_nu  = eigenToHepMC(nu);
        auto mom_tau = eigenToHepMC(tau);
        auto mom_w   = eigenToHepMC(w);
        HepMC::GenParticle * p_tau;
        HepMC::GenParticle * p_nu   = new HepMC::GenParticle(mom_nu,  16, 4); // Incident neutrino
        if (tauplus)
        {
          p_tau  = new HepMC::GenParticle(mom_tau, -15, 1); // Positively charged Tau
        }
        else
        {
          p_tau  = new HepMC::GenParticle(mom_tau,  15, 1); // Negatively charged Tau
        }
        HepMC::GenParticle * p_w    = new HepMC::GenParticle(mom_w,   24, 4); // The W

        HepMC::GenVertex * vertex = new HepMC::GenVertex();

        //vertex->add_particle_in(p_nu);
        //vertex->add_particle_in(p_w);
        //vertex->add_particle_out(p_tau);

        vertex->add_particle_out(p_nu);
        vertex->add_particle_in(p_w);
        vertex->add_particle_out(p_tau);

        event->add_vertex(vertex);
        if (verbose) {
          cout << "BEFORE:"<<endl;
          event->print();
        }
        TauolaHepMCEvent * t_event = new TauolaHepMCEvent(event);
        t_event->decayTaus();

        for (auto v = event->vertices_begin(); v != event->vertices_end(); ++v ) {
          HepMC::GenVertex::particles_out_const_iterator p1;
          for (auto p1 = (*v)->particles_out_const_begin();p1 != (*v)->particles_out_const_end(); ++p1 ) {
            auto pout=**p1;
            if (pout.status() !=1) continue; // Skip all non-final particles
            fout << eventids[iEvent] << "," << pout.pdg_id()
              << "," << pout.momentum().e()
              << "," << pout.momentum().px()
              << "," << pout.momentum().py()
              << "," << pout.momentum().pz() << "\n";
          }
        }
        if (verbose) {
          cout << "AFTER:"<<endl;
          event->print();
        }

        delete event;
        delete t_event;
    }
    fout.close();

    return 0;
}


//==================================================================================================
/**
  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_EXPM1_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_EXPM1_HPP_INCLUDED

#include <boost/simd/constant/logeps.hpp>
#include <boost/simd/detail/constant/maxlog.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/arch/common/detail/generic/expm1_kernel.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

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

  BOOST_DISPATCH_OVERLOAD_IF( expm1_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>,X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0) const BOOST_NOEXCEPT
    {
      using sA0 = bd::scalar_of_t<A0>;
      return if_else(is_less(a0, Logeps<A0>()),
                     Mone<A0>(),
                     if_else(is_greater(a0, Maxlog<A0>()),
                             Inf<A0>(),
                              detail::expm1_kernel<A0, sA0>::expm1(a0)
                            )
                    );
    }
  };
} } }

#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Alexandra Zerck $
// $Authors: Eva Lange $
// --------------------------------------------------------------------------

#include <cmath>
#include <boost/math/special_functions/acosh.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <OpenMS/TRANSFORMATIONS/RAW2PEAK/OptimizePick.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>

namespace OpenMS
{
  namespace OptimizationFunctions
  {

    // Print the computed signal
    void printSignal(const gsl_vector * x, void * param, float resolution)
    {

      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(param)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(param)->peaks;
      std::cout << "Printing Signal" << std::endl;
      if (resolution == 1.)
      {
        // iterate over all points of the signal
        for (size_t current_point = 0; current_point < positions.size(); current_point++)
        {
          double computed_signal     = 0.;
          double current_position    = positions[current_point];

          // iterate over all peaks
          for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
          {
            // Store the current parameters for this peak
            double p_height          = gsl_vector_get(x, 4 * current_peak);
            double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
            double p_width           = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                       : gsl_vector_get(x, 4 * current_peak + 2);

            // is it a Lorentz or a Sech - Peak?
            if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
            {
              computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
            }
            else // It's a Sech - Peak
            {
              computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
            }
          }
          std::cerr << positions[current_point] << " " << computed_signal << std::endl;
        }
      }
      else
      {
        // Compute step width
        float sw = (positions[1] - positions[0]) / resolution;
        for (int i = 0; i < positions.size() * resolution; i++)
        {
          double computed_signal     = 0.;
          double current_position    = positions[0] + i * sw;

          // iterate over all peaks
          for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
          {
            // Store the current parameters for this peak
            double p_height          = gsl_vector_get(x, 4 * current_peak);
            double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
            double p_width           = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                       : gsl_vector_get(x, 4 * current_peak + 2);

            // is it a Lorentz or a Sech - Peak?
            if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
            {
              computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
            }
            else // It's a Sech - Peak
            {
              computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
            }
          }

          std::cerr.precision(writtenDigits<DoubleReal>(0.0));

          std::cerr << positions[0] + i * sw << " " << computed_signal << std::endl;
        }
      }
    }

    // Evaluation of the target function for nonlinear optimization.
    int residual(const gsl_vector * x, void * params, gsl_vector * f)
    {
      // According to the gsl conventions, x contains the parameters to be optimized.
      // In our case, this means that we store for each peak four consecutive values:
      //  - its height
      //  - its left width
      //  - its right width
      //  - its position
      //
      // Params might contain any additional parameters. We handle these using class members
      // instead.
      // The vector f is supposed to contain the result when we return from this function.
      // Note: GSL wants the values for each data point i as one component of the results vector
      std::vector<DoubleReal> & signal = static_cast<OptimizePick::Data *>(params)->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactors & penalties = static_cast<OptimizePick::Data *>(params)->penalties;
      // iterate over all points of the signal
      for (size_t current_point = 0; current_point < positions.size(); current_point++)
      {
        double computed_signal     = 0.;
        double current_position    = positions[current_point];
        double experimental_signal = signal[current_point];

        // iterate over all peaks
        for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          // Store the current parameters for this peak
          double p_height        = gsl_vector_get(x, 4 * current_peak);
          double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
          double p_width         = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                   : gsl_vector_get(x, 4 * current_peak + 2);

          // is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
          }
          else // It's a Sech - Peak
          {
            computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
          }
        }
        gsl_vector_set(f, current_point, computed_signal - experimental_signal);
      }

      double penalty = 0.;
//      struct PenaltyFactors* penalties = (struct PenaltyFactors *)params;
      double penalty_pos    = penalties.pos;
      double penalty_lwidth = penalties.lWidth;
      double penalty_rwidth = penalties.rWidth;

      // iterate over all peaks again to compute the penalties
      for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        double old_position = peaks[current_peak].mz_position;
        double old_width_l  = peaks[current_peak].left_width;
        double old_width_r  = peaks[current_peak].right_width;
        double p_position   = gsl_vector_get(x, 4 * current_peak + 3);
        double p_width_l        = gsl_vector_get(x, 4 * current_peak + 1);
        double p_width_r    = gsl_vector_get(x, 4 * current_peak + 2);

        //penalty += pow(p_position - old_position, 2) + pow(p_width_l - old_width_l, 2) + pow(p_width_r - old_width_r, 2);
        penalty +=      penalty_pos    * pow(p_position - old_position, 2)
                   + penalty_lwidth * pow(p_width_l - old_width_l, 2)
                   + penalty_rwidth * pow(p_width_r - old_width_r, 2);
      }

      gsl_vector_set(f, positions.size(), 100 * penalty);

      return GSL_SUCCESS;
    }

    /** Compute the Jacobian of the residual, where each row of the matrix corresponds to a
     *  point in the data.
     */
    int jacobian(const gsl_vector * x, void * params, gsl_matrix * J)
    {
      // For the conventions on x and params c.f. the commentary in residual()
      //
      // The matrix J is supposed to contain the result when we return from this function.
      // Note: GSL expects the Jacobian as follows:
      // - each row corresponds to one data point
      // - each column corresponds to one parameter
      // std::vector<DoubleReal>& signal = static_cast<OptimizePick::Data*> (params) ->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePick::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePick::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactors & penalties = static_cast<OptimizePick::Data *>(params)->penalties;
      // iterate over all points of the signal
      for (size_t current_point = 0; current_point < positions.size(); current_point++)
      {
        double current_position    = positions[current_point];

        // iterate over all peaks
        for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          // Store the current parameters for this peak
          double p_height        = gsl_vector_get(x, 4 * current_peak);
          double p_position    = gsl_vector_get(x, 4 * current_peak + 3);
          double p_width         = (current_position <= p_position) ? gsl_vector_get(x, 4 * current_peak + 1)
                                   : gsl_vector_get(x, 4 * current_peak + 2);

          // is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            double diff      = current_position - p_position;
            double denom_inv = 1. / (1. + pow(p_width * diff, 2));

            double ddl_left  = (current_position <= p_position)
                               ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                 0;

            double ddl_right = (current_position  > p_position)
                               ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                 0;

            double ddx0          = -2 * p_height * pow(p_width, 2) * diff * pow(denom_inv, 2);

            gsl_matrix_set(J, current_point, 4 * current_peak, denom_inv);
            gsl_matrix_set(J, current_point, 4 * current_peak + 1, ddl_left);
            gsl_matrix_set(J, current_point, 4 * current_peak + 2, ddl_right);
            gsl_matrix_set(J, current_point, 4 * current_peak + 3, ddx0);
          }
          else // It's a Sech - Peak
          {
            double diff      = current_position - p_position;
            double denom_inv = 1. / cosh(p_width * diff);

            // The remaining computations are not stable if denom_inv == 0. In that case, we are far away from the peak
            // and can assume that all derivatives vanish
            double sinh_term = (fabs(denom_inv) < 1e-6) ? 0.0 : sinh(p_width * diff);
            double ddl_left  = (current_position <= p_position)
                               ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                 0;
            double ddl_right = (current_position  > p_position)
                               ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                 0;
            double ddx0      = 2 * p_height * p_width * sinh_term * pow(denom_inv, 3);

            gsl_matrix_set(J, current_point, 4 * current_peak, pow(denom_inv, 2));
            gsl_matrix_set(J, current_point, 4 * current_peak + 1, ddl_left);
            gsl_matrix_set(J, current_point, 4 * current_peak + 2, ddl_right);
            gsl_matrix_set(J, current_point, 4 * current_peak + 3, ddx0);
          }
        }
      }

      // Now iterate over all peaks again to compute the penalties.
//   struct PenaltyFactors* penalties = (struct PenaltyFactors *)params;
      for (size_t current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        double p_width_left = gsl_vector_get(x, 4 * current_peak + 1);
        double p_width_right = gsl_vector_get(x, 4 * current_peak + 2);
        double p_position   = gsl_vector_get(x, 4 * current_peak + 3);

        double old_width_left  = peaks[current_peak].left_width;
        double old_width_right = peaks[current_peak].right_width;
        double old_position    = peaks[current_peak].mz_position;

        double penalty_l = 2. * penalties.lWidth * (p_width_left - old_width_left);
        double penalty_r = 2. * penalties.rWidth * (p_width_right - old_width_right);
        double penalty_p = 0;
        if (fabs(p_position - old_position) < 0.2)
        {
          penalty_p = 2. * penalties.pos * (p_position - old_position);
        }

        gsl_matrix_set(J, positions.size(), 4 * current_peak, 0.);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 1, 100 * penalty_l);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 2, 100 * penalty_r);
        gsl_matrix_set(J, positions.size(), 4 * current_peak + 3, 100 * penalty_p);
      }

      return GSL_SUCCESS;
    }

    // Driver function for the evaluation of function and jacobian.
    int evaluate(const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix * J)
    {
      residual(x, params, f);
      jacobian(x, params, J);

      return GSL_SUCCESS;
    }

  }

  OptimizePick::OptimizePick(const struct OptimizationFunctions::PenaltyFactors & penalties,
                             const int max_iteration,
                             const double eps_abs,
                             const double eps_rel)
  {

    penalties_ = penalties;

    max_iteration_ = max_iteration;
    eps_abs_ = eps_abs;
    eps_rel_ = eps_rel;

#ifdef DEBUG_PEAK_PICKING
    std::cout << "max iteration " << max_iteration_
              << "\n eps abs " << eps_abs
              << "\n eps rel " << eps_rel
              << "\n penalty factor pos " << penalties.pos
              << "\n penalty factor left width " << penalties.lWidth
              << "\n penalty factor right width " << penalties.rWidth
              << std::endl;
#endif

  }

  OptimizePick::~OptimizePick()
  {
  }

  void OptimizePick::optimize(std::vector<PeakShape> & peaks, Data & data)
  {
    if (peaks.empty())
      return;

    size_t global_peak_number = 0;
    data.peaks.assign(peaks.begin(), peaks.end());

    gsl_vector * start_value = gsl_vector_alloc(4 * data.peaks.size());
    // We have to initialize the parameters for the optimization
    for (size_t i = 0; i < data.peaks.size(); i++)
    {
      PeakShape current_peak = data.peaks[i];
      double h  = current_peak.height;
      double wl = current_peak.left_width;
      double wr = current_peak.right_width;
      double p  = current_peak.mz_position;
      if (boost::math::isnan(wl))
      {
        data.peaks[i].left_width = 1;
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        data.peaks[i].right_width = 1;
        wr = 1.;
      }

      gsl_vector_set(start_value, 4 * i, h);
      gsl_vector_set(start_value, 4 * i + 1, wl);
      gsl_vector_set(start_value, 4 * i + 2, wr);
      gsl_vector_set(start_value, 4 * i + 3, p);
    }

    // The gsl algorithms require us to provide function pointers for the evaluation of
    // the target function.
    gsl_multifit_function_fdf fit_function;
    fit_function.f      = OptimizationFunctions::residual;
    fit_function.df       = OptimizationFunctions::jacobian;
    fit_function.fdf        = OptimizationFunctions::evaluate;
    fit_function.n          = std::max(data.positions.size() + 1, 4 * data.peaks.size());
    fit_function.p          = 4 * data.peaks.size();
//    fit_function.params = &penalties_;
    data.penalties = penalties_;
    fit_function.params = &data;

    const gsl_multifit_fdfsolver_type * type = gsl_multifit_fdfsolver_lmsder;

    gsl_multifit_fdfsolver * fit = gsl_multifit_fdfsolver_alloc(type, std::max(data.positions.size() + 1, 4 * data.peaks.size()), 4 * data.peaks.size());

    gsl_multifit_fdfsolver_set(fit, &fit_function, start_value);

    // initial norm
    // std::cout << "Before optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;

    // Iteration
    unsigned int iteration = 0;
    int status;

    do
    {
      iteration++;
      status = gsl_multifit_fdfsolver_iterate(fit);
#ifdef DEBUG_PEAK_PICKING
      std::cout << "Iteration " << iteration << "; Status " << gsl_strerror(status) << "; " << std::endl;
      std::cout << "||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
      std::cout << "Number of parms: " << data.peaks.size() * 4 << std::endl;
      std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
#endif
      if (boost::math::isnan(gsl_blas_dnrm2(fit->dx)))
        break;

      // We use the gsl function gsl_multifit_test_delta to decide if we can finish the iteration.
      // We only finish if all new parameters deviates only by a small amount from the parameters of the last iteration
      status = gsl_multifit_test_delta(fit->dx, fit->x, eps_abs_, eps_rel_);
      if (status != GSL_CONTINUE)
        break;

    }
    while (status == GSL_CONTINUE && iteration < max_iteration_);

#ifdef DEBUG_PEAK_PICKING
    std::cout << "Finished!" << std::endl;
    std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
    double chi = gsl_blas_dnrm2(fit->f);
    std::cout << "chisq/dof = " << pow(chi, 2.0) / (data.positions.size() - 4 * data.peaks.size());
#endif

    // OptimizationFunctions::printSignal(fit->x, 5.,param);

    // iterate over all peaks and store the optimized values in peaks
    for (size_t current_peak = 0; current_peak < data.peaks.size(); current_peak++)
    {
      // Store the current parameters for this peak
      peaks[global_peak_number + current_peak].height          = gsl_vector_get(fit->x, 4 * current_peak);
      peaks[global_peak_number + current_peak].mz_position = gsl_vector_get(fit->x, 4 * current_peak + 3);
      peaks[global_peak_number + current_peak].left_width  = gsl_vector_get(fit->x, 4 * current_peak + 1);
      peaks[global_peak_number + current_peak].right_width = gsl_vector_get(fit->x, 4 * current_peak + 2);

      // compute the area
      // is it a Lorentz or a Sech - Peak?
      if (peaks[global_peak_number + current_peak].type == PeakShape::LORENTZ_PEAK)
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * sqrt(p.height / 1 - 1);
        double x_rigth_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
        double area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
        double area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_rigth_endpoint));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Lorentz " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
#endif
      }
      else  //It's a Sech - Peak
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
        double x_rigth_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
        double area_left = p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint)) / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
        double area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_rigth_endpoint)) / cosh(p.right_width * (p.mz_position - x_rigth_endpoint)));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Sech " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
        std::cout << p.mz_position << " " << x_left_endpoint << " " << x_rigth_endpoint << std::endl;
#endif
      }
    }
    global_peak_number += data.peaks.size();

    gsl_multifit_fdfsolver_free(fit);
    gsl_vector_free(start_value);
  }

  // double OptimizePick::correlate_(const PeakShape& peak,
//                                  double left_endpoint,
//                                                                  double right_endpoint,Data& data)
//   {
//     double SSxx = 0., SSyy = 0., SSxy = 0.;

//     // compute the averages
//     double data_average=0., fit_average=0.;
//     double data_sqr=0., fit_sqr=0.;
//     double cross=0.;

//     int number_of_points = 0;

//     int first=0;
//     int last=data.positions.size()-1;

//     // search for the left endpoint position
//     while (data.positions[first] < left_endpoint)
//     {
//       ++first;
//     }

//     // search for the right endpoint position
//     while (data.positions[last] > right_endpoint)
//     {
//       --last;
//     }

//     // for separate overlapping peak correlate until the max position...
//     for (int i=first; i <= last; i++)
//     {
//       double data_val = data.signal[i];
//       double peak_val = peak(data.positions[i]);

//       data_average += data_val;
//       fit_average  += peak_val;

//       data_sqr += data_val * data_val;
//       fit_sqr  += peak_val * peak_val;

//       cross += data_val * peak_val;

//       number_of_points++;
//     }

//     if (number_of_points == 0)
//       return 0.;

//     data_average /= number_of_points;
//     fit_average  /= number_of_points;

//     SSxx = data_sqr - number_of_points * (data_average * data_average);
//     SSyy = fit_sqr - number_of_points * (fit_average * fit_average);
//     SSxy = cross - number_of_points * (data_average * fit_average);

//     return (SSxy * SSxy) / (SSxx * SSyy);
//   }

}