#include "Mesh.hpp"

#include "igl/barycentric_coordinates.h"
#include "Util.hpp"

#include <Eigen/QR>

#include <iostream>
#include <cstdlib>
#include <iostream>

template <typename T> using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
template <typename T> using VecC3 = Eigen::Matrix<T, 3, 1>;
template <typename T> using VecC2 = Eigen::Matrix<T, 2, 1>;
template <typename T> using VecR3 = Eigen::Matrix<T, 1, 3>;

template <typename T>
Mesh<T>::Mesh()
{
#ifdef ENABLE_CUDA
  std::cout << " ----**** Creating mesh with CUDA solver ****----" << std::endl;
#else
  std::cout << " ----**** Creating mesh with CPU solver ****----" << std::endl;
#endif

  color_counter =  Eigen::MatrixXi::Zero(TEXTURE_RESOLUTION, TEXTURE_RESOLUTION);

  texture.red   = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
  texture.green = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
  texture.blue  = Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>::Constant(TEXTURE_RESOLUTION,TEXTURE_RESOLUTION, static_cast<unsigned char>(115));
}

template <typename T>
Mesh<T>::~Mesh()
{
  std::ofstream residual_file;
  residual_file.open("error.csv");
  residual_file << "pc_idx,level,iteration,residuals\n";

  for (int pci = 0; pci < residuals.size(); ++pci)
  {
    for (int li = 0; li < residuals[pci].size(); ++li)
    {
      for (int it = 0; it < residuals[pci][li].size(); ++it)
        residual_file << pci << "," << li << "," << it << "," << residuals[pci][li][it] << std::endl;
    }
  }

  residual_file.close();
}

template <typename T>
void Mesh<T>::cleanup()
{
#ifdef ENABLE_CUDA
    cudaDeviceSynchronize();
#endif

    for (auto& m : JtJ)
        m.clear();

    for (auto& v : Jtz)
        v.clear();
}

inline int subdivided_side_length(const int level, const int base_resolution)
{
  return (base_resolution-1)*static_cast<int>(std::pow(2,level))+1;
}

template <typename T>
void upsample(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V, const Eigen::MatrixXi& F, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out)
{
    const int old_resolution = static_cast<int>(std::sqrt(static_cast<double>(V.rows())));
    const int resolution = subdivided_side_length(1, old_resolution);
    const int faces = (resolution - 1) * (resolution - 1) * 2;
    const int vertices = resolution * resolution;

    F_out = Eigen::MatrixXi::Zero(faces, 3);
    V_out = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(vertices, 3);

#pragma omp parallel for
    for (int y_step = 0; y_step < old_resolution-1; ++y_step)
    {
      for (int x_step = 0; x_step < old_resolution-1; ++x_step)
      {

        Eigen::Vector3i f_u = F.row(x_step*2 + y_step*(old_resolution-1)*2);
        Eigen::Vector3i f_l = F.row(x_step*2 + y_step*(old_resolution-1)*2 + 1);

        const VecC3<T> old_v0 = V.row(f_u(0));
        const VecC3<T> old_v1 = V.row(f_u(1));
        const VecC3<T> old_v2 = V.row(f_u(2));

        const VecC3<T> old_v3 = V.row(f_l(0));
        const VecC3<T> old_v4 = V.row(f_l(1));
        //const TvecC3 old_v5 = V.row(f_l(2));

        const VecC3<T> v0 = old_v0;
        const VecC3<T> v1 = (old_v0 + old_v1) * T(0.5);
        const VecC3<T> v2 = old_v1;
        const VecC3<T> v3 = (old_v0 + old_v2) * T(0.5);
        const VecC3<T> v4 = (old_v2 + old_v1) * T(0.5);
        const VecC3<T> v5 = (old_v3 + old_v1) * T(0.5);
        const VecC3<T> v6 = old_v2;
        const VecC3<T> v7 = (old_v3 + old_v4) * T(0.5);
        const VecC3<T> v8 = old_v3;

        const int new_v_xi = x_step * 2;
        const int new_v_yi = y_step * 2;

        int
          v0i = new_v_xi       + new_v_yi * resolution,
          v1i = new_v_xi + 1 + new_v_yi * resolution,
          v2i = new_v_xi + 2+ new_v_yi * resolution,
          v3i = new_v_xi + (new_v_yi+1) * resolution,
          v4i = new_v_xi + 1 + (new_v_yi+1) * resolution,
          v5i = new_v_xi + 2 + (new_v_yi+1) * resolution,
          v6i = new_v_xi       + (new_v_yi+2) * resolution,
          v7i = new_v_xi + 1 + (new_v_yi+2) * resolution,
          v8i = new_v_xi + 2 + (new_v_yi+2) * resolution;

        V_out.row(v0i) = v0;
        V_out.row(v1i) = v1;
        V_out.row(v3i) = v3;
        V_out.row(v4i) = v4;

        if (x_step == old_resolution-2)
        {
          V_out.row(v2i) = v2;
          V_out.row(v5i) = v5;
        }

        if (y_step == old_resolution-2)
        {
          V_out.row(v6i) = v6;
          V_out.row(v7i) = v7;
        }

        if (x_step == old_resolution - 2 && y_step == old_resolution-2)
          V_out.row(v8i) = v8;

        const int new_f_xi = x_step * 2;
        const int new_f_yi = y_step * 2;

        F_out.row(2*new_f_xi     + new_f_yi * (resolution-1) * 2) << v0i,v1i,v3i;
        F_out.row(2*new_f_xi + 1 + new_f_yi * (resolution-1) * 2) << v4i,v3i,v1i;
        F_out.row(2*new_f_xi + 2 + new_f_yi * (resolution-1) * 2) << v1i,v2i,v4i;
        F_out.row(2*new_f_xi + 3 + new_f_yi * (resolution-1) * 2) << v5i,v4i,v2i;
        F_out.row(2*new_f_xi +     (new_f_yi+1) * (resolution-1)*2) << v3i,v4i,v6i;
        F_out.row(2*new_f_xi + 1 + (new_f_yi+1) * (resolution-1)*2) << v7i,v6i,v4i;
        F_out.row(2*new_f_xi + 2 + (new_f_yi+1) * (resolution-1)*2) << v4i,v5i,v7i;
        F_out.row(2*new_f_xi + 3 + (new_f_yi+1) * (resolution-1)*2) << v8i,v7i,v5i;
      }
    }
}

template <typename T>
void upsample(const unsigned int levels, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V, const Eigen::MatrixXi& F, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out)
{
    if (levels < 1)
    {
      V_out = V;
      F_out = F;
      return;
    }

    Eigen::MatrixXi F_in = F;
    Mat<T> V_in = V;

    for (unsigned int i = 0; i < levels; ++i)
    {
      upsample(V_in, F_in, V_out, F_out);
      F_in = F_out;
      V_in = V_out;
    }
}

// Compute barycentric coordinates based on the corner points of a triangle
template <typename T>
void barycentric(const VecC2<T>& p, const Eigen::Matrix<T, 2, 1>& a, const Eigen::Matrix<T, 2, 1>& b, const Eigen::Matrix<T, 2, 1>& c, T &u, T &v, T &w)
{
    Eigen::Matrix<T, 2, 1> v0 = b - a,
                                           v1 = c - a,
                                           v2 = p - a;
    T a00 = v0.dot(v0);
    T a01 = v0.dot(v1);
    T a11 = v1.dot(v1);
    T a20 = v2.dot(v0);
    T a21 = v2.dot(v1);
    T den = 1 / (a00 * a11 - a01 * a01);
    v =  (a11 * a20 - a01 * a21) * den;
    w = (a00 * a21 - a01 * a20) * den;
    u = T(1.0) - v - w;
}

template <typename T>
template <typename Derived>
void Mesh<T>::align_to_point_cloud(const Eigen::MatrixBase<Derived>& P)
{
  const VecR3<T> bb_min = P.colwise().minCoeff();
  const VecR3<T> bb_max = P.colwise().maxCoeff();
  const VecR3<T> bb_d = (bb_max - bb_min).cwiseAbs();

  JtJ.resize(MESH_LEVELS);
  Jtz.resize(MESH_LEVELS);
  V.resize(MESH_LEVELS);
  F.resize(MESH_LEVELS);

  const int max_resolution = subdivided_side_length(MESH_LEVELS - 1, MESH_RESOLUTION);
  const Eigen::Matrix<T, 2, 1> a(0.f, 0.f), b(1.f, 0.f), c(0.f, 1.f), d(1.f, 1.f);

  for (int li = 0; li < MESH_LEVELS; ++li)
  {
    const T scaling_factor(MESH_SCALING_FACTOR);
    const int resolution = subdivided_side_length(li, MESH_RESOLUTION);

    // Scaling matrix
    const Eigen::Transform<T, 3, Eigen::Affine> scaling(Eigen::Scaling(VecC3<T>(scaling_factor*bb_d(0)/T(resolution), 0.f,scaling_factor*bb_d(2)/T(resolution))));

    //const TvecR3 pc_mean = P.colwise().mean();
    // P_centr: mean of the point cloud
    VecR3<T> P_centr = bb_min + 0.5f*(bb_max - bb_min);
    P_centr(1) = 0.0;

    const Eigen::Transform<T, 3, Eigen::Affine> t(Eigen::Translation<T, 3>(P_centr.transpose()));

    transform = t.matrix();

    V[li].resize(resolution*resolution, 3);
    F[li].resize((resolution-1)*(resolution-1)*2, 3);
    JtJ[li].resize(resolution);
    Jtz[li].resize(resolution);

  #pragma omp parallel for
    for (int z_step = 0; z_step < resolution; ++z_step)
    {
      for (int x_step = 0; x_step < resolution; ++x_step)
      {
        Eigen::Matrix<T, 1, 4> v; v << VecR3<T>(T(x_step)-T(resolution-1)/2.f,T(0.0), T(z_step)-T(resolution-1)/2.f),T(1.0);
        VecR3<T> pos;

        if (li == 0) // Only transform the first layer. Subsequent layers only denot a difference between the first
          pos << (v * scaling.matrix().transpose() * transform.transpose()).template head<3>();
        else
          pos << (v * scaling.matrix().transpose()).template head<3>();

        V[li].row(x_step + z_step*resolution) << pos;
      }
    }

  #pragma omp parallel for
    for (int y_step = 0; y_step < resolution-1; ++y_step)
    {
      for (int x_step = 0; x_step < resolution-1; ++x_step)
      {
        F[li].row(x_step*2 + y_step*(resolution-1)*2)     << x_step+   y_step   *resolution,x_step+1+y_step*   resolution,x_step+(y_step+1)*resolution;
        F[li].row(x_step*2 + y_step*(resolution-1)*2 + 1) << x_step+1+(y_step+1)*resolution,x_step+ (y_step+1)*resolution,x_step+1+y_step*resolution;
      }
    }

    const int factor = static_cast<int>(std::pow(2, MESH_LEVELS - li - 1));

    // Place constraints for every vertex on the highest resolution grid for all levels
    for (int i = 0; i < (max_resolution-1) * (max_resolution-1); ++i)
    {

      const int x = i % (max_resolution-1);
      const int y = i / (max_resolution-1);

      const int gx = x / factor;
      const int gy = y / factor;

      const int t = (gx + (resolution-1) * gy) * 2;

      // inside current resolution rect
      const T tx(static_cast<T>(x % factor) / static_cast<T>(factor));
      const T ty(static_cast<T>(y % factor) / static_cast<T>(factor));

      T u,v,w;
      const VecC2<T> p(tx, ty);

      if (tx + ty > T(1.))
      {
        // lower right triangle
        barycentric(p, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      else
      {
        // upper left triangle
        barycentric(p, a, b, c, u, v, w);
        JtJ[li].update_triangle(t, u, v);
        Jtz[li].update_triangle(t, u, v, 0.f);
      }

      // Special cases > last cell, last column, last row
      if (x + 1 >= max_resolution - 1 && y + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(T(1.), T(1.));
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      if (x + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(T(1.), ty);
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
      if (y + 1 >= max_resolution - 1)
      {
        const VecC2<T> p2(tx, T(1.));
        barycentric(p2, d, c, b, u, v, w);
        JtJ[li].update_triangle(t+1, u, v);
        Jtz[li].update_triangle(t+1, u, v, 0.f);
      }
    }
  }
}

template <typename T>
void Mesh<T>::get_mesh(const unsigned int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out, ColorData& colordata) const
{
    const int mesh_resolution = JtJ[level].get_mesh_width();

    colordata.UV.resize(mesh_resolution*mesh_resolution, 2);
    const T tdx = T(1.0) / (mesh_resolution-1);
    for(int i = 0; i < mesh_resolution; i++)
    {
      for(int j = 0; j < mesh_resolution; j++)
      {
        const int index = j * mesh_resolution + i;
        colordata.UV(index, 0) = i*tdx;
        colordata.UV(index, 1) = j*tdx;
      }
    }

    colordata.texture.red = texture.red;
    colordata.texture.green = texture.green;
    colordata.texture.blue = texture.blue;

    get_mesh(level, V_out, F_out);
}

template <typename T>
void Mesh<T>::get_mesh(const unsigned int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V_out, Eigen::MatrixXi& F_out) const
{
  if (level < 1 || level > MESH_LEVELS - 1)
  {
    V_out = V[0];
    F_out = F[0];
  }
  else
  {
    upsample(level, V[0], F[0], V_out, F_out);

    for (unsigned int li = 1; li <= level; ++li)
    {
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> V_upsampled;
      Eigen::MatrixXi F_upsampled;
      upsample(level-li, V[li], F[li], V_upsampled, F_upsampled);

       V_out.col(1) += V_upsampled.col(1);
    }
  }
}

// Project all points onto the 2D plane, we only need to project onto XZ plane as the algorithm only runs on height fields
// Also why we only need to project only when the input point cloud changes
template <typename T>
void Mesh<T>::project_points(const int level, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc) const
{
  bc = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(current_target_point_cloud.rows(), 3);

  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Vl;
  Eigen::MatrixXi Fl;
  get_mesh(level, Vl, Fl);

  const int resolution = subdivided_side_length(level, MESH_RESOLUTION);

  // Upper left triangle. Used to compute the index of the triangle that is hit
  const VecC2<T> V_ul(Vl.row(0)(0), Vl.row(0)(2));
  const VecC2<T> V_00(Vl.row(Fl.row(0)(0))(0), Vl.row(Fl.row(0)(0))(2));
  const VecC2<T> V_01(Vl.row(Fl.row(0)(1))(0), Vl.row(Fl.row(0)(1))(2));
  const VecC2<T> V_10(Vl.row(Fl.row(0)(2))(0), Vl.row(Fl.row(0)(2))(2));

  const double dx = (V_01- V_00).norm();
  const double dy = (V_10 - V_00).norm();

#pragma omp parallel for
  for (int pi = 0; pi < current_target_point_cloud.rows(); ++pi)
  {
    const VecC2<T> current_point(current_target_point_cloud.row(pi)(0), current_target_point_cloud.row(pi)(2));
    const VecC2<T> offset(current_point - V_ul);

    const int c = static_cast<int>(offset(0) / dx);
    const int r = static_cast<int>(offset(1) / dy);

    const int inner_size = resolution - 1;

    // Indices are outside of mesh borders => a triangle cannot be hit
    if (c >= inner_size || r >= inner_size || c < 0 || r < 0)
    {
      bc.row(pi) << -1, T(0.0), T(0.0);
      continue;
    }
    const  VecC3<T> ul_3d = Vl.row(c + r*resolution);
    const  VecC3<T> br_3d = Vl.row(c + 1 + (r+1)*resolution);

    const VecC2<T> ul_reference(ul_3d(0), ul_3d(2));
    const VecC2<T> br_reference(br_3d(0), br_3d(2));

    const double ul_squared_dist = (ul_reference - current_point).squaredNorm();
    const double br_squared_dist = (br_reference - current_point).squaredNorm();

    VecC2<T> v_a;
    VecC2<T> v_b;
    VecC2<T> v_c;

    int f_idx = -1;

    // Find corner vertices of hit triangle
    if (ul_squared_dist <= br_squared_dist)
    {
      f_idx = 2 * c + r * 2 * inner_size;

      v_a << Vl.row(c + r*resolution)(0), Vl.row(c + r*resolution)(2);
      v_b << Vl.row(c+1 + r*resolution)(0),Vl.row(c+1 + r*resolution)(2);
      v_c << Vl.row(c + (r+1)*resolution)(0),Vl.row(c + (r+1)*resolution)(2);
    }else
    {
      f_idx = 2 * c + r * 2 * inner_size + 1;

      v_a << Vl.row(c+1 + (r+1)*resolution)(0),Vl.row(c+1 + (r+1)*resolution)(2);
      v_b << Vl.row(c + (r+1)*resolution)(0),Vl.row(c + (r+1)*resolution)(2);
      v_c << Vl.row(c+1 + r*resolution)(0),Vl.row(c+1 + r*resolution)(2);
    }

    T u,v,w;
    barycentric(current_point, v_a, v_b, v_c, u, v, w);

    bc.row(pi) << T(f_idx), u, v;
  }
}

template <typename T>
void Mesh<T>::set_target_point_cloud(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P)
{
  current_target_point_cloud = P;

  residuals.push_back(std::vector<std::vector<T>>(MESH_LEVELS-1));

  // First fuse to base level
  Mat<T> bc;
  project_points(0, bc);
  update_JtJ(0, bc); // Update lh
  update_Jtz(0, bc, current_target_point_cloud.col(1));

  for (int li = 1; li < MESH_LEVELS; ++li)
  {
    Mat<T> V_upsampled;
    Eigen::MatrixXi F_upsampled;

    get_mesh(li, V_upsampled, F_upsampled);
    project_points(li, bc);

    update_JtJ(li, bc); // Update lh
  }
}

template <typename T>
void Mesh<T>::set_target_point_cloud(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& P, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& C)
{
  set_target_point_cloud(P);

  if (C.rows() != P.rows())
    return;

  const VecC3<T> bb_min = V[0].row(0);
  const VecC3<T> bb_max = V[0].row(V[0].rows()-1);

  const T cdx = (bb_max(0)-bb_min(0)) / static_cast<T>(TEXTURE_RESOLUTION-1);
  const T cdz = (bb_max(2)-bb_min(2)) / static_cast<T>(TEXTURE_RESOLUTION-1);

  for(int i=0; i<current_target_point_cloud.rows(); i++)
  {
    T x = current_target_point_cloud(i, 0) - bb_min(0);
    T z = current_target_point_cloud(i, 2) - bb_min(2);
    x = std::floor(x / cdx);
    z = std::floor(z / cdz);

    const int xi = static_cast<int>(x);
    const int zi = static_cast<int>(z);

    if (xi >= color_counter.rows() || zi >= color_counter.cols() || xi < 0 || zi < 0)
      continue;

    const int count = color_counter(xi, zi);

    texture.red(xi, zi)   = static_cast<unsigned char>((texture.red(xi, zi)  * count + (C(i,0)*255)) / (count + 1));
    texture.green(xi, zi) = static_cast<unsigned char>((texture.green(xi, zi) * count + (C(i,1)*255)) / (count + 1));
    texture.blue(xi, zi)  = static_cast<unsigned char>((texture.blue(xi, zi)  * count + (C(i,2)*255)) / (count + 1));
    ++color_counter(xi, zi);
  }
}

template <typename T>
void Mesh<T>::update_JtJ(const int level, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc)
{
  for (int i = 0; i < bc.rows(); ++i)
  {
    const VecR3<T>& row = bc.row(i);

    if (static_cast<int>(row(0)) == -1)
      continue;

    JtJ[level].update_triangle(static_cast<int>(row(0)), row(1), row(2));
  }
}

template <typename T>
void Mesh<T>::update_Jtz(const int level, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& bc, const Eigen::Matrix<T, Eigen::Dynamic, 1>& z)
{
  for (int i = 0; i < bc.rows(); ++i)
  {
    const VecR3<T>& row = bc.row(i);

    if (static_cast<int>(row(0)) == -1)
      continue;

    Jtz[level].update_triangle(static_cast<int>(row(0)), row(1), row(2), z(i));
  }
}

template <typename T>
void Mesh<T>::solve(const int iterations)
{

  // First fuse to ba se level
  Mat<T> bc;

// lh and rh for first layer already updated in set_target_point_cloud
#ifdef ENABLE_CUDA
  sor_gpu(iterations, 0, V[0].col(1));
#else
  sor_parallel(iterations, 0, V[0].col(1));
#endif

  for (int li = 1; li < MESH_LEVELS; ++li)
  {
    Mat<T> residual_height = Mat<T>::Zero(current_target_point_cloud.rows(), current_target_point_cloud.cols());

    Mat<T> V_upsampled;
    Eigen::MatrixXi F_upsampled;
    get_mesh(li, V_upsampled, F_upsampled);
    project_points(li, bc);

    // Compute residual
#pragma omp parallel for
    for (int pi = 0; pi < bc.rows(); ++pi)
    {
      if (static_cast<int>(bc.row(pi)(0)) == -1) // No hit
        continue;

      const VecC3<T> v0 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(0));
      const VecC3<T> v1 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(1));
      const VecC3<T> v2 = V_upsampled.row(F_upsampled.row(static_cast<int>(bc.row(pi)(0)))(2));

      // Ideally these should be equal, then there is no error
      const VecC3<T> solved_point = bc.row(pi)(1) * v0 + bc.row(pi)(2) * v1 + (1.f - bc.row(pi)(1) - bc.row(pi)(2)) * v2;
      const VecC3<T> measured_point = current_target_point_cloud.row(pi);

      residual_height.row(pi) = measured_point - solved_point;
    }

    residuals.back()[li-1].push_back(residual_height.array().sum());

    // Update equation rh with residual
    update_Jtz(li, bc, residual_height.col(1));

#ifdef ENABLE_CUDA
      sor_gpu(iterations, li, V[li].col(1));
#else
      sor_parallel(iterations, li, V[li].col(1));
#endif
  }
}

// Basic solve without any parallelism
template <typename T>
void Mesh<T>::sor(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>> h) const
{
  const auto& Jtz_vec = Jtz[level];
  const auto& Jtj_mat = JtJ[level];

  // For each iteration, solve system for each vertex
  for(int it = 0; it < iterations; it++)
  {
    for (int vi = 0; vi < h.rows(); vi++)
      sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
  }
}

/*
 Solve system for a single vertex vi in the mesh
 vi is the vertex index,
 JtJ is the MatrixGrid, matrix on the lhs
 Jtz is the vector on the rhs
 h is an array of the vertex heights
*/
template <typename T>
CUDA_HOST_DEVICE inline void sor_inner(const int vi, const JtJMatrixGrid<T>& JtJ, const JtzVector<T>& Jtz_vec, T* h)
{
  T xn = Jtz_vec.get(vi);
  T acc = 0;

  T vals[6];
  int ids[6];

  T a;
  JtJ.get_matrix_values_for_vertex(vi, vals, ids, a);

  for (int j = 0; j < 6; ++j)
    acc += vals[j] * h[ids[j]];

  xn -= acc;

  // Weighting of previous height vs newly solved height
  // only use new height if w = 1.
  const T w = 1.0;
  h[vi] = (1.f-w) * h[vi] + w*xn/a;
}

// Run solve in parallel by using four-color reordering
template <typename T>
void Mesh<T>::sor_parallel(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>> h) const
{
  const auto& Jtz_vec = Jtz[level];
  const auto& Jtj_mat = JtJ[level];
  const int resolution = JtJ[level].get_mesh_width();

  for(int it = 0; it < iterations; it++)
  {
#pragma omp parallel for collapse(2)
    for (int x = 0; x < resolution; x+=2)
      for (int y = 0; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 1; x < resolution; x+=2)
      for (int y = 0; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 0; x < resolution; x+=2)
      for (int y = 1; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }

#pragma omp parallel for collapse(2)
    for (int x = 1; x < resolution; x+=2)
      for (int y = 1; y < resolution; y+=2)
      {
        const int vi = x + y * resolution;
        sor_inner(vi, Jtj_mat, Jtz_vec, h.data());
      }
  }
}

// Define CUDA specific functions here
#ifdef ENABLE_CUDA

// This function solves a single iteration for a point based on the index
// xoffset and yoffset define the offset of the four-color reordering
__global__ void solve_kernel(const int xoffset, const int yoffset, const int mesh_width, const JtzVector<float> Jtz_vec, const JtJMatrixGrid<float> JtJ_mat, float* h) {
	const int idx = blockIdx.x * blockDim.x + threadIdx.x;

    if (idx >= mesh_width*mesh_width)
    {
	    return;
    }

	const int x = ((idx) % mesh_width);
	const int y = ((idx) / mesh_width);
	const int vi = x + y * mesh_width;

  // Make sure that indices is okay for our offsets
	if ((x % 2 == xoffset) && (y % 2 == yoffset))
		sor_inner(vi, JtJ_mat, Jtz_vec, h);
}

// GPU solve for non-float meshes, does not work
template <typename T>
void Mesh<T>::sor_gpu(const int, const int, Eigen::Ref<Eigen::Matrix<T, Eigen::Dynamic, 1>>)
{
    assert(false && "GPU solver works only with float meshes");
}

/*
 Function for solving the system on the GPU using CUDA
 Similar to the parallel solve this method uses four-color reordering
 and therefore there are four solve_kernel calls,
 GPU solver also only works with float meshes
*/
template <>
void Mesh<float>::sor_gpu(const int iterations, const int level, Eigen::Ref<Eigen::Matrix<float, Eigen::Dynamic, 1>> h)
{
    const JtJMatrixGrid<float> JtJ_mat = JtJ[level];
    const JtzVector<float> Jtz_vec = Jtz[level];
    const int mesh_width = JtJ[level].get_mesh_width();
    const int mesh_width_squared = mesh_width*mesh_width;
    const int mat_width = JtJ[level].get_matrix_width();
    //const int mat_width_squared = mat_width*mat_width;

    float *devH;
    CUDA_CHECK(cudaMalloc(&devH, h.rows() * sizeof(float)));
    CUDA_CHECK(cudaMemcpy(devH, h.data(), h.rows() * sizeof(float), cudaMemcpyHostToDevice));

    dim3 block(64);
    dim3 grid((mesh_width_squared + block.x - 1) / block.x);

    for (int it = 0; it < iterations; it++) {
	    solve_kernel<<<grid, block>>>(0, 0, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(1, 0, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(0, 1, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
	    solve_kernel<<<grid, block>>>(1, 1, mesh_width, Jtz_vec, JtJ_mat, devH);
	    cudaDeviceSynchronize();
    }

    CUDA_CHECK(cudaMemcpy(h.data(), devH, h.rows() * sizeof(float), cudaMemcpyDeviceToHost));
    CUDA_CHECK(cudaFree(devH));
}
#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"MinimumJerkInterpolation.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"
#include "left_hand.h"
#include <sensor_msgs/JointState.h>
#include <cstdlib>
#include<termios.h>
#include <iostream>
#include <QTime>
#include"trajectory_generation/handmove.h"
#include "trajectory_generation/handmoveRequest.h"
#include "trajectory_generation/handmoveResponse.h"

using namespace  std;
using namespace  Eigen;

bool simulation=true;

bool move_activate=false;

int qc_offset[12];
int qra_offset[4];
int qla_offset[4];
bool qc_initial_bool;

VectorXd absolute_q0_r(7);VectorXd absolute_q0_l(7);
bool abs_initial_bool;

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 ;
right_hand hand_r;
left_hand hand_l;
// waist
double Waist2ArmZ=0.2694;
double Waist2RArmY=-0.239;
double Waist2LArmY=0.239;
//double WaistYaw=0; double WaistPitch=0;
//MatrixXd R_shoulder(3,3);
//VectorXd r_left_shoulder(3);
//VectorXd r_right_shoulder(3);
//    R_shoulder<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
//            cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
//            -sin(WaistPitch),              0,               cos(WaistPitch);
//    r_left_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
//                      Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
//                                                               Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
//    r_right_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
//                       Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
//                                                                Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
 std::string scenario_r="n";
 std::string scenario_l="n";
 bool initializing=true;
bool handMove(trajectory_generation::handmoveRequest &req, trajectory_generation::handmoveResponse &res){

    qDebug()<<"req";
    if(!move_activate){
        scenario_l=req.scenario_l;
        scenario_r=req.scenario_r;

    //qDebug()<<"scenario_l="<<scenario_l.c_str()<<"scenario_r="<<scenario_r.c_str();
    initializing=true;

    move_activate=true;
    res.result=1;
    return true;
    }
        else{
        return false;
        }
}

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()<<"";
}

double move2pose(double max,double t_local,double T_start ,double T_end){
    double T_move=T_end-T_start;
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_end){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else{theta=max;}
    return theta;
}

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

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]);

        }

        for (int i = 12; i < 16; ++i){
            qra_offset[i-12]=int(msg.position[i+1]);
        }

        for (int i = 20; i < 24; ++i){
            qla_offset[i-20]=int(msg.position[i+1]);
        }

        qc_initial_bool=false;

        ROS_INFO("Offset_feet=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n",
                 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]);
        ROS_INFO("Offset_righthand=%d\t%d\t%d\t%d",qra_offset[0],qra_offset[1], qra_offset[2],qra_offset[3]);
        ROS_INFO("Offset_lefthand=%d\t%d\t%d\t%d",qla_offset[0],qla_offset[1], qla_offset[2],qla_offset[3]);
        ROS_INFO("Initialized!");
    }
}

void abs_read(const sensor_msgs::JointState & msg){
    if (abs_initial_bool){

        VectorXd absolute_sensor_r(7);VectorXd absolute_sensor_l(7);
        for (int i = 0; i < 7; ++i) {
            absolute_sensor_r(i)=msg.position[i+13];            absolute_q0_r(i)=0;
            absolute_sensor_l(i)=msg.position[i+13+8];            absolute_q0_l(i)=0;

        }
        //right hand
        absolute_q0_r(0)=double(absolute_sensor_r(0)+863)/8192*2*M_PI;
        absolute_q0_r(1)=double(absolute_sensor_r(1)-(-889+2275))/8192*2*M_PI;
        absolute_q0_r(2)=-double(absolute_sensor_r(2)-2881)/8192*2*M_PI;
        absolute_q0_r(3)=double(absolute_sensor_r(3)-2058)/8192*2*M_PI;
        //    //left hand
        absolute_q0_l(0)=double(absolute_sensor_l(0)-705)/8192*2*M_PI;
        absolute_q0_l(1)=double(absolute_sensor_l(1)-(3642-2275))/8192*2*M_PI;
        absolute_q0_l(2)=-double(absolute_sensor_l(2)+344)/8192*2*M_PI;
        absolute_q0_l(3)=-double(absolute_sensor_l(3)-47)/8192*2*M_PI;
        qDebug()<<"absolutes:";
        matrix_view(absolute_sensor_r);matrix_view(absolute_sensor_l);
        qDebug()<<"positions:";
        VectorXd absolute_q0_r_deg(7);
        absolute_q0_r_deg=180/M_PI*absolute_q0_r;
        matrix_view(absolute_q0_r_deg);
        VectorXd absolute_q0_l_deg(7);
        absolute_q0_l_deg=180/M_PI*absolute_q0_l;
        matrix_view(absolute_q0_l_deg);

        abs_initial_bool=false;
    }

}

void  SendGazebo(vector<double> q){

    std_msgs::Float64 data;
    data.data=q[1];
    pub1.publish(data);
    data.data=q[2];
    pub2.publish(data);
    data.data=q[3];
    pub3.publish(data);
    data.data=q[4];
    pub4.publish(data);
    data.data=q[5];
    pub5.publish(data);
    data.data=q[6];
    pub6.publish(data);
    data.data=q[7];
    pub7.publish(data);
    data.data=q[8];
    pub8.publish(data);
    data.data=q[9];
    pub9.publish(data);
    data.data=q[10];
    pub10.publish(data);
    data.data=q[11];
    pub11.publish(data);
    data.data=q[12];
    pub12.publish(data);
    data.data=q[13];
    pub13.publish(data);
    data.data=q[14];
    pub14.publish(data);
    data.data=q[15];
    pub15.publish(data);
    data.data=q[16];
    pub16.publish(data);
    data.data=q[17];
    pub17.publish(data);
    data.data=q[18];
    pub18.publish(data);
    data.data=q[19];
    pub19.publish(data);
    data.data=q[20];
    pub20.publish(data);
    data.data=q[21];
    pub21.publish(data);
    data.data=q[22];
    pub22.publish(data);
    data.data=q[23];
    pub23.publish(data);
    data.data=q[24];
    pub24.publish(data);
    data.data=q[25];
    pub25.publish(data);
    data.data=q[26];
    pub26.publish(data);
    data.data=q[27];
    pub27.publish(data);
    data.data=q[28];
    pub28.publish(data);
    data.data=q[29];
    pub29.publish(data);
    data.data=q[30];
    pub30.publish(data);
    data.data=q[31];
    pub31.publish(data);

}

MatrixXd rightshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
        R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
         cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
                      -sin(WaistPitch),              0,               cos(WaistPitch);

               P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
                Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
                Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;

    T<<R.transpose(),-R.transpose()*P,0,0,0,1;

    return T;
}

MatrixXd leftshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
    R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
     cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
                  -sin(WaistPitch),              0,               cos(WaistPitch);

           P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
            Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
            Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;                                                                1;
T<<R.transpose(),-R.transpose()*P,0,0,0,1;
    return T;
}

int main(int argc, char **argv)
{
//    scenario_l="a";
//    scenario_r="b";
//    move_activate=true;
QByteArray joint_data;

    abs_initial_bool=!simulation;
    qc_initial_bool=!simulation;

ros::init(argc, argv, "handnode");
    ros::NodeHandle nh;

    //nh.getParam("scenario_r", scenario_r);nh.getParam("scenario_l", scenario_l);

   // ROS_INFO("scenario_r: %s",scenario_r.c_str());ROS_INFO("scenario_l: %s",scenario_l.c_str());

    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
    ros::Subscriber abs_sensor = nh.subscribe("/surena/abs_joint_state", 1000, abs_read);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
    ros::ServiceServer HandMoveService = nh.advertiseService("HandMove", handMove);
    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);
    double t_r_offset=0;double t_l_offset=0;
    if(simulation){
        pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",100);
        pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",100);
        pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",100);
        pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",100);
        pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",100);
        pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",100);
        pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",100);
        pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",100);
        pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",100);
        pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",100);
        pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",100);
        pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",100);
        pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",100);
        pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",100);
        pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",100);
        pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",100);
        pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",100);
        pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",100);
        pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",100);
        pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",100);
        pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",100);
        pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",100);
        pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",100);
        pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",100);
        pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",100);
        pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",100);
        pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",100);
        pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",100);
        pub29 = nh.advertise<std_msgs::Float64>("rrbot/joint29_position_controller/command",100);
        pub30 = nh.advertise<std_msgs::Float64>("rrbot/joint30_position_controller/command",100);
        pub31 = nh.advertise<std_msgs::Float64>("rrbot/joint31_position_controller/command",100);
    }

    right_hand hand_funcs;

    VectorXd r_target_r(3);   VectorXd r_target_l(3);
    VectorXd r_middle_r(3);   VectorXd r_middle_l(3);
    MatrixXd R_target_r(3,3); MatrixXd R_target_l(3,3);
    VectorXd r_target_rt[10];   VectorXd r_target_lt[10];
    MatrixXd R_target_rt[10]; MatrixXd R_target_lt[10];
    for (int i = 0; i < 10; ++i) {
        r_target_rt[i].resize(3,1);
        r_target_lt[i].resize(3,1);
        R_target_rt[i].resize(3,3);
        R_target_lt[i].resize(3,3);
    }
    VectorXd r_right_palm(3);VectorXd r_left_palm(3);
    int fingers_mode_r;       int fingers_mode_l;
    VectorXd q0_r(7);         VectorXd q0_l(7);

    double d0_r;              double d0_l;
    double d_r ;              double d_l ;
    double d_des_r;           double d_des_l;
    double theta_r;           double theta_l;
    double theta_target_r;    double theta_target_l;
    double sai_r;             double sai_l;
    double sai_target_r;      double sai_target_l;
    double phi_r;             double phi_l;
    double phi_target_r;      double phi_target_l;

    QVector<double> qr1;      QVector<double> ql1;
    QVector<double> qr2;      QVector<double> ql2;
    QVector<double> qr3;      QVector<double> ql3;
    QVector<double> qr4;      QVector<double> ql4;
    QVector<double> qr5;      QVector<double> ql5;
    QVector<double> qr6;      QVector<double> ql6;
    QVector<double> qr7;      QVector<double> ql7;

    MinimumJerkInterpolation coef_generator;
    MatrixXd X_coef_r;        MatrixXd X_coef_l;
    MatrixXd Y_coef_r;        MatrixXd Y_coef_l;
    MatrixXd Z_coef_r;        MatrixXd Z_coef_l;

    MatrixXd t_r(1,3);        MatrixXd t_l(1,3);
    MatrixXd P_x_r(1,3);      MatrixXd P_x_l(1,3);
    MatrixXd V_x_r(1,3);      MatrixXd V_x_l(1,3);
    MatrixXd A_x_r(1,3);      MatrixXd A_x_l(1,3);

    MatrixXd P_y_r(1,3);      MatrixXd P_y_l(1,3);
    MatrixXd V_y_r(1,3);      MatrixXd V_y_l(1,3);
    MatrixXd A_y_r(1,3);      MatrixXd A_y_l(1,3);

    MatrixXd P_z_r(1,3);      MatrixXd P_z_l(1,3);
    MatrixXd V_z_r(1,3);      MatrixXd V_z_l(1,3);
    MatrixXd A_z_r(1,3);      MatrixXd A_z_l(1,3);

    int fingers_r=9;  int fingers_l=9;
    VectorXd q_ra;VectorXd q_la;

    int q_motor_r[8];int q_motor_l[8];
    for (int var = 0; var < 8; ++var) {
        q_motor_r[var]=0;q_motor_l[var]=0;
    }

    vector<double> q(31);

    ros::Rate loop_rate(200);
    int count = 0;
    double time=0.0;
    double time_r,time_l;

    int gest_r=0;int gest_l=0;
    int gest_count;
    VectorXd qr_end(7);VectorXd ql_end(7);
    double WaistYaw=0;
    double WaistPitch=0;
    QTime chronometer;
qDebug()<<"start!";
    while (ros::ok())
    {
if(move_activate){
    //    chronometer.start();
        if (abs_initial_bool) {
            ROS_INFO_ONCE("abs is initializing!");
            ros::spinOnce();
            continue;
        }

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

        if(initializing){
            qDebug()<<"scenario_l="<<scenario_l.c_str()<<"scenario_r="<<scenario_r.c_str();

            if(simulation){
                q0_r<<10*M_PI/180,
                        -10*M_PI/180,
                        0,
                        -20*M_PI/180,
                        0,
                        0,
                        0;

                q0_l<<10*M_PI/180,
                        10*M_PI/180,
                        0,
                        -20*M_PI/180,
                        0,
                        0,
                        0;
            }

            else{
                if (scenario_r=="h"){
                    q0_r=absolute_q0_r;
                }
                else{
                    q0_r<<10*M_PI/180,
                            -10*M_PI/180,
                            0,
                            -20*M_PI/180,
                            0,
                            0,
                            0;
                }

                if (scenario_l=="h"){
                    q0_l=absolute_q0_l;
                }
                else{
                    q0_l<<10*M_PI/180,
                            10*M_PI/180,
                            0,
                            -20*M_PI/180,
                            0,
                            0,
                            0;
                }

                qDebug("q0 init ok");

            }
            ROS_INFO("q0_r= %f, %f, %f, %f, %f, %f, %f",q0_r(0),q0_r(1),q0_r(2),q0_r(3),q0_r(4),q0_r(5),q0_r(6));
            ROS_INFO("q0_l= %f, %f, %f, %f, %f, %f, %f",q0_l(0),q0_l(1),q0_l(2),q0_l(3),q0_l(4),q0_l(5),q0_l(6));

            //self recognition
            if(scenario_r=="a"){

                r_target_r<<.4,
                        0.07,
                        -0.15;
                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(2,-40*M_PI/180,3);
                r_middle_r<<.3,
                        -.1,
                        -.3;
                fingers_mode_r=10;
            }
            if(scenario_l=="a"){

                r_target_l<<.4,
                        -0.07,
                        -0.15;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3)*hand_funcs.rot(2,-40*M_PI/180,3);
                r_middle_l<<.3,
                        .1,
                        -.3;
                fingers_mode_l=10;
            }
            //pointing
            if(scenario_r=="b"){

                r_target_r<<.05,
                        -0.56,
                        -0;
                R_target_r=hand_funcs.rot(1,-89*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);
                r_middle_r<<.1,
                        -.35,
                        -.3;
                fingers_mode_r=11;
            }
            if(scenario_l=="b"){

                r_target_l<<.05,
                        0.56,
                        -0;
                R_target_l=hand_funcs.rot(1,89*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);
                r_middle_l<<.1,
                        .35,
                        -.3;
                fingers_mode_l=11;
            }
            // mini touch
            if(scenario_r=="c"){

                r_target_r<<.4,
                        -0.3,
                        -0.2;
                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(2,20*M_PI/180,3);
                r_middle_r<<.2,
                        -.25,
                        -.35;
                fingers_mode_r=10;
            }
            if(scenario_l=="c"){

                r_target_l<<.4,
                        0.3,
                        -0.2;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3)*hand_funcs.rot(2,20*M_PI/180,3);
                r_middle_l<<.2,
                        .25,
                        -.35;
                fingers_mode_l=10;
            }
            // looking at horizon
            if(scenario_r=="d"){

                r_target_r<<.3,
                        0.09,
                        0.24;
                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,90*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3)*hand_funcs.rot(1,-28*M_PI/180,3);
                r_middle_r<<.4,
                        -.2,
                       - 0.05;
                fingers_mode_r=10;

            }
            if(scenario_l=="d"){

                r_target_l<<.3,
                        -0.09,
                        0.24;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,-90*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3)*hand_funcs.rot(1,28*M_PI/180,3);
                r_middle_l<<.4,
                        .2,
                        -0.05;
                fingers_mode_l=10;

            }
            // respct
            if(scenario_r=="e"){

                r_target_r<<.28,
                        0.1,
                        -0.35;
                R_target_r=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,50*M_PI/180,3);
                r_middle_r<<.35,
                        -0.1,
                        -0.3;
                fingers_mode_r=10;

            }
            if(scenario_l=="e"){

                r_target_l<<.28,
                        -0.1,
                        -0.35;
                R_target_l=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-50*M_PI/180,3);
                r_middle_l<<.35,
                        0.1,
                        -0.3;
                fingers_mode_l=10;

            }

            // hand-shake
            if(scenario_r=="f"){

                r_target_r<<.4,
                        -0.05,
                        -0.4;
                R_target_r=hand_funcs.rot(2,-65*M_PI/180,3);
                r_middle_r<<.3,
                        -0.1,
                        -0.4;
                fingers_mode_r=18;

            }
            if(scenario_l=="f"){

                r_target_l<<.4,
                        0.05,
                        -0.4;
                R_target_l=hand_funcs.rot(2,-65*M_PI/180,3);
                r_middle_l<<.3,
                        0.1,
                        -0.4;
                fingers_mode_l=18;

            }
            // waving
            if(scenario_r=="g"){

                r_target_r<<.35,
                        -0.1,
                        0.3;
                R_target_r=hand_funcs.rot(2,-180*M_PI/180,3)*hand_funcs.rot(3,90*M_PI/180,3);
                r_middle_r<<.4,
                        -0.2,
                        -0.2;
                fingers_mode_r=10;

            }
            if(scenario_l=="g"){

                r_target_l<<.35,
                        0.1,
                        0.3;
                R_target_l=hand_funcs.rot(2,-180*M_PI/180,3)*hand_funcs.rot(3,-90*M_PI/180,3);
                r_middle_l<<.4,
                        0.2,
                        -0.2;
                fingers_mode_l=10;

            }
            // gripping
            if(scenario_r=="j"){
                r_target_r<<.45,
                        0.,
                        -0.25;
                r_middle_r<<.3,
                        -.0,
                        -.3;

                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,-10*M_PI/180+atan2(r_target_r(1),r_target_r(0)),3);
                fingers_mode_r=4;
            }
            if(scenario_l=="j"){
                r_target_l<<.45,
                        -0.,
                        -0.25;
                r_middle_l<<.3,
                        .0,
                        -.3;

                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3)*hand_funcs.rot(1,-(-10*M_PI/180+atan2(r_target_r(1),r_target_r(0))),3);
                fingers_mode_l=4;
            }

            if(scenario_r=="t"||scenario_l=="t"){

                r_target_rt[0]<<.2,
                        -0.1,
                        -0.4;
                R_target_rt[0]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-25*M_PI/180,3)*hand_funcs.rot(3,-25*M_PI/180,3);

                r_target_rt[1]<<.35,
                        -0.1,
                        -0.35;
                R_target_rt[1]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(3,-70*M_PI/180,3)*hand_funcs.rot(2,-20*M_PI/180,3);

                r_target_rt[2]<<.4,
                        -0.1,
                        -0.15;
                R_target_rt[2]=hand_funcs.rot(2,-100*M_PI/180,3)*hand_funcs.rot(3,-40*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);

                r_target_rt[3]<<.2,
                        -0.1,
                        -0.4;
                R_target_rt[3]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-25*M_PI/180,3)*hand_funcs.rot(3,-25*M_PI/180,3);

                r_target_rt[4]<<.1,
                        -0.25,
                        -0.45;
                R_target_rt[4]=hand_funcs.rot(3,-70*M_PI/180,3)*hand_funcs.rot(2,-50*M_PI/180,3);

                r_target_rt[5]<<.25,
                        -0.25,
                        -0.35;
                R_target_rt[5]=hand_funcs.rot(2,-80*M_PI/180,3)*hand_funcs.rot(1,-50*M_PI/180,3);
                r_target_rt[6]<<.25,
                        -0.1,
                        -0.45;
                R_target_rt[6]=hand_funcs.rot(2,-45*M_PI/180,3)*hand_funcs.rot(3,-30*M_PI/180,3);
                r_target_rt[7]<<.3,
                          0.1,
                        -0.35;
                R_target_rt[7]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,20*M_PI/180,3);
                r_target_rt[8]<<.25,
                        -0.2,
                        -0.45;
                R_target_rt[8]=hand_funcs.rot(2,-30*M_PI/180,3)*hand_funcs.rot(3,-15*M_PI/180,3);

                r_target_lt[0]<<.2,
                        0.1,
                        -0.4;
                R_target_lt[0]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,25*M_PI/180,3)*hand_funcs.rot(3,25*M_PI/180,3);

                r_target_lt[1]<<.35,
                        0.1,
                        -0.35;
                R_target_lt[1]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(3,70*M_PI/180,3)*hand_funcs.rot(2,-20*M_PI/180,3);

                r_target_lt[2]<<.4,
                        0.1,
                        -0.15;
                R_target_lt[2]=hand_funcs.rot(2,-100*M_PI/180,3)*hand_funcs.rot(3,40*M_PI/180,3)*hand_funcs.rot(2,-10*M_PI/180,3);

                r_target_lt[3]<<.2,
                        0.1,
                        -0.4;
                R_target_lt[3]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,25*M_PI/180,3)*hand_funcs.rot(3,25*M_PI/180,3);

                r_target_lt[4]<<.1,
                        0.25,
                        -0.45;
                R_target_lt[4]=hand_funcs.rot(3,70*M_PI/180,3)*hand_funcs.rot(2,-50*M_PI/180,3);

                r_target_lt[5]<<.25,
                        0.25,
                        -0.35;
                R_target_lt[5]=hand_funcs.rot(2,-80*M_PI/180,3)*hand_funcs.rot(1,50*M_PI/180,3);

                r_target_lt[6]<<.25,
                        0.1,
                        -0.45;
                R_target_lt[6]=hand_funcs.rot(2,-45*M_PI/180,3)*hand_funcs.rot(3,30*M_PI/180,3);
                r_target_lt[7]<<.3,
                         -0.1,
                        -0.35;
                R_target_lt[7]=hand_funcs.rot(2,-70*M_PI/180,3)*hand_funcs.rot(1,-20*M_PI/180,3);
                r_target_lt[8]<<.25,
                        0.2,
                        -0.45;
                R_target_lt[8]=hand_funcs.rot(2,-30*M_PI/180,3)*hand_funcs.rot(3,15*M_PI/180,3);

            }
            //synchronized movement
            if(scenario_r=="s"){
                r_target_r<<.35,
                       - 0.05,
                        -0.3;

                r_middle_r<<.2,
                        -.10,
                        -.45;
                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_r=11;}
            if(scenario_r=="o"){
                r_target_r<<.4,
                       - 0.1,
                        -0.25;

                r_middle_r<<.2,
                        -.10,
                        -.45;
                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_r=11;}

                if(scenario_r=="ws"){
                    r_target_r<<.4,
                           - 0.1,
                            -0.15;

                    r_middle_r<<.2,
                            -.10,
                            -.45;
                    R_target_r=hand_funcs.rot(2,-90*M_PI/180,3);

                fingers_mode_r=11;}
                if(scenario_r=="wu"){
                    r_target_r<<.4,
                           - 0.1,
                            -0.15;

                    r_middle_r<<.2,
                            -.10,
                            -.45;
                    R_target_r=hand_funcs.rot(2,-90*M_PI/180,3);

                fingers_mode_r=11;}
               if(scenario_l=="s"){
                r_target_l<<.35,
                        0.05,
                        -0.3;
                r_middle_l<<.2,
                        .1,
                        -.45;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_l=11;
            }
               if(scenario_l=="o"){
                r_target_l<<.4,
                        0.1,
                        -0.25;
                r_middle_l<<.2,
                        .1,
                        -.45;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_l=11;
            }

               if(scenario_l=="ws"){
                r_target_l<<.4,
                        0.1,
                        -0.15;
                r_middle_l<<.2,
                        .1,
                        -.45;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_l=11;
            }

               if(scenario_l=="wu"){
                r_target_l<<.4,
                        0.1,
                        -0.15;
                r_middle_l<<.2,
                        .1,
                        -.45;
                R_target_l=hand_funcs.rot(2,-90*M_PI/180,3);
                fingers_mode_l=11;
            }

            // temp
            if(scenario_r=="z"){

                r_target_r<<.25,
                        -0.25,
                        -0.35;
                R_target_r=hand_funcs.rot(2,-80*M_PI/180,3)*hand_funcs.rot(1,-50*M_PI/180,3);
                r_middle_r<<.25,
                        -0.25,
                        -0.35;
            }

            double v0_r=0;
            double v_target_r =.4;
            right_hand hand0_r(q0_r,r_target_r,R_target_r,0,0);
            left_hand hand0_l(q0_l,r_target_l,R_target_l,0,0);
            //  ROS_INFO("r0=%f,%f,%f",hand0.r_right_palm(0),hand0.r_right_palm(1),hand0.r_right_palm(2));
            r_right_palm=hand0_r.r_right_palm;
            r_left_palm=hand0_l.r_left_palm;
            d0_r=hand0_r.dist;                        d0_l=hand0_l.dist;
            d_r=d0_r;                                 d_l=d0_l;
            d_des_r=hand0_r.d_des;                    d_des_l=hand0_l.d_des;
            theta_r=hand0_r.theta;                    theta_l=hand0_l.theta;
            theta_target_r=hand0_r.theta_target;      theta_target_l=hand0_l.theta_target;
            sai_r=hand0_r.sai;                        sai_l=hand0_l.sai;
            sai_target_r=hand0_r.sai_target;          sai_target_l=hand0_l.sai_target;
            phi_r=hand0_r.phi;                        phi_l=hand0_l.phi;
            phi_target_r=hand0_r.phi_target;          phi_target_l=hand0_l.phi_target;
            hand0_r.HO_FK_right_palm(q0_r);           hand0_l.HO_FK_left_palm(q0_l);

            q_ra=q0_r;q_la=q0_l;

            qr1.clear(); ql1.clear();
            qr2.clear(); ql2.clear();
            qr3.clear(); ql3.clear();
            qr4.clear(); ql4.clear();
            qr5.clear(); ql5.clear();
            qr6.clear(); ql6.clear();
            qr7.clear(); ql7.clear();

            qr1.append(q_ra(0));     ql1.append(q_la(0));
            qr2.append(q_ra(1));     ql2.append(q_la(1));
            qr3.append(q_ra(2));     ql3.append(q_la(2));
            qr4.append(q_ra(3));     ql4.append(q_la(3));
            qr5.append(q_ra(4));     ql5.append(q_la(4));
            qr6.append(q_ra(5));     ql6.append(q_la(5));
            qr7.append(q_ra(6));     ql7.append(q_la(6));

            if (simulation)
            {
                vector<double> q_init(31);
                for (int i = 0; i < 31; ++i) {
                    q_init[i]=0;
                }
                q_init[15]=qr1[0];   q_init[15+7]=ql1[0];
                q_init[16]=qr2[0];   q_init[16+7]=ql2[0];
                q_init[17]=qr3[0];   q_init[17+7]=ql3[0];
                q_init[18]=qr4[0];   q_init[18+7]=ql4[0];
                q_init[19]=qr5[0];   q_init[19+7]=ql5[0];
                q_init[20]=qr6[0];   q_init[20+7]=ql6[0];
                q_init[21]=qr7[0];   q_init[21+7]=ql7[0];

                SendGazebo(q_init);
            }

            //****path generation

            if(scenario_r=="t"||scenario_l=="t"){
                t_r<<0,0.5,1.5;t_l<<0,0.5,1.5;
                r_middle_r=hand0_r.r_right_palm;
                r_middle_l=hand0_l.r_left_palm;
                gest_r=0;gest_l=gest_r;//=rand()%8;

                gest_count=1;
                r_target_r=r_target_rt[gest_r];
                R_target_r=R_target_rt[gest_r];

                r_target_l=r_target_lt[gest_l];
                R_target_l=R_target_lt[gest_l];
            }
            else{
                t_r<<0,2,4;t_l<<0,2,4;
            }

            MatrixXd T_right_shoulder_transpose=rightshoulder2waist(WaistYaw,WaistPitch);
            MatrixXd T_left_shoulder_transpose=leftshoulder2waist(WaistYaw,WaistPitch);

            VectorXd temp(4);
            temp<<r_target_r,1;
            r_target_r=(T_right_shoulder_transpose*temp).block(0,0,3,1);

            temp<<r_target_l,1;
            r_target_l=(T_left_shoulder_transpose*temp).block(0,0,3,1);

            P_x_r<< hand0_r.r_right_palm(0),r_middle_r(0),r_target_r(0);
            P_y_r<< hand0_r.r_right_palm(1),r_middle_r(1),r_target_r(1);
            P_z_r<< hand0_r.r_right_palm(2),r_middle_r(2),r_target_r(2);
            P_x_l<< hand0_l.r_left_palm(0),r_middle_l(0),r_target_l(0);
            P_y_l<< hand0_l.r_left_palm(1),r_middle_l(1),r_target_l(1);
            P_z_l<< hand0_l.r_left_palm(2),r_middle_l(2),r_target_l(2);

            R_target_r=T_right_shoulder_transpose.block(0,0,3,3)*R_target_r;
            R_target_l=T_left_shoulder_transpose.block(0,0,3,3)*R_target_l;

            V_x_r<<0,INFINITY,0;     V_x_l<<0,INFINITY,0;
            V_y_r<<0,INFINITY,0;     V_y_l<<0,INFINITY,0;
            V_z_r<<0,INFINITY,0;     V_z_l<<0,INFINITY,0;
            A_x_r<<0,INFINITY,0;     A_x_l<<0,INFINITY,0;
            A_y_r<<0,INFINITY,0;     A_y_l<<0,INFINITY,0;
            A_z_r<<0,INFINITY,0;     A_z_l<<0,INFINITY,0;

            X_coef_r=coef_generator.Coefficient(t_r,P_x_r,V_x_r,A_x_r);
            Y_coef_r=coef_generator.Coefficient(t_r,P_y_r,V_y_r,A_y_r);
            Z_coef_r=coef_generator.Coefficient(t_r,P_z_r,V_z_r,A_z_r);

            X_coef_l=coef_generator.Coefficient(t_l,P_x_l,V_x_l,A_x_l);
            Y_coef_l=coef_generator.Coefficient(t_l,P_y_l,V_y_l,A_y_l);
            Z_coef_l=coef_generator.Coefficient(t_l,P_z_l,V_z_l,A_z_l);
            //   ROS_INFO("%d,%d",X_coef.rows(),X_coef.cols());
            //ROS_INFO("\ncoeff_x=%f\t%f\t%f\t%f\t%f\t%f",X_coef(0)
            //ROS_INFO("\n
            //ROS_INFO("\n

            //home

            if(scenario_r=="h"){
                qr_end=q0_r;
                fingers_mode_r=9;
            }
            if(scenario_l=="h"){
                ql_end=q0_l;
                fingers_mode_l=9;
            }
//            ROS_INFO("press any key to start!");
//            getch();

            count=0;

            initializing=false;
        }

        else {

            time=double(count)*.005;
            time_r=time; time_l=time;

            if((scenario_r=="g"||scenario_r=="f")&&time_r>=t_r(2)*2-.005){scenario_r="h";;t_r_offset=2*t_r(2);fingers_mode_r=9;}
            if((scenario_l=="g"||scenario_l=="f")&&time_l>=t_l(2)*2-.005){scenario_l="h";;t_l_offset=2*t_l(2);fingers_mode_l=9;}

            if((scenario_r=="t"||scenario_l=="t")){time_r=fmod(time,t_r(2)); time_l= time_r;

                if( time_r==0){
                    r_middle_r=r_right_palm;
                    r_middle_l=r_left_palm;

//                    int temp1=rand()%8;
//                    int temp2=rand()%8;
                   ++gest_count;

//                    while(temp1==gest_r){temp1=rand()%8;}
//                    gest_r=temp1;
//                    while(temp2==gest_l){temp2=rand()%8;}
//                    gest_l=temp2;
                    gest_r++;gest_l=gest_r;
                    if (gest_r>=8){
                        gest_r=0;
                        gest_l=gest_r;
                    }

                    qDebug()<<"gest_count:"<<gest_count;
                    if (gest_count==10){scenario_r="h"; scenario_l="h";
                        t_r_offset=gest_count*t_r(2);
                        t_l_offset=gest_count*t_l(2);
                    }
                    gest_r=gest_l;
                    r_target_r=r_target_rt[gest_r];
                    R_target_r=R_target_rt[gest_r];

                    r_target_l=r_target_lt[gest_l];
                    R_target_l=R_target_lt[gest_l];

                    P_x_r<< r_middle_r(0),r_middle_r(0),r_target_r(0);
                    P_y_r<< r_middle_r(1),r_middle_r(1),r_target_r(1);
                    P_z_r<< r_middle_r(2),r_middle_r(2),r_target_r(2);
                    P_x_l<< r_middle_l(0),r_middle_l(0),r_target_l(0);
                    P_y_l<< r_middle_l(1),r_middle_l(1),r_target_l(1);
                    P_z_l<< r_middle_l(2),r_middle_l(2),r_target_l(2);

                    V_x_r<<0,INFINITY,0;     V_x_l<<0,INFINITY,0;
                    V_y_r<<0,INFINITY,0;     V_y_l<<0,INFINITY,0;
                    V_z_r<<0,INFINITY,0;     V_z_l<<0,INFINITY,0;
                    A_x_r<<0,INFINITY,0;     A_x_l<<0,INFINITY,0;
                    A_y_r<<0,INFINITY,0;     A_y_l<<0,INFINITY,0;
                    A_z_r<<0,INFINITY,0;     A_z_l<<0,INFINITY,0;

                    X_coef_r=coef_generator.Coefficient(t_r,P_x_r,V_x_r,A_x_r);
                    Y_coef_r=coef_generator.Coefficient(t_r,P_y_r,V_y_r,A_y_r);
                    Z_coef_r=coef_generator.Coefficient(t_r,P_z_r,V_z_r,A_z_r);

                    X_coef_l=coef_generator.Coefficient(t_l,P_x_l,V_x_l,A_x_l);
                    Y_coef_l=coef_generator.Coefficient(t_l,P_y_l,V_y_l,A_y_l);
                    Z_coef_l=coef_generator.Coefficient(t_l,P_z_l,V_z_l,A_z_l);
                }
            }

            if(scenario_r=="h"){time_r=time+(t_r(2)-t_r_offset);}
            if(scenario_l=="h"){time_l=time+(t_l(2)-t_l_offset);}

            if(scenario_r=="n"){time_r=t_r(2)*3;}
            if(scenario_l=="n"){time_l=t_l(2)*3;}

            VectorXd P_r(3); VectorXd V_r(3);
            VectorXd P_l(3); VectorXd V_l(3);
            if(time_r<t_r(1)){
                P_r<<   coef_generator.GetAccVelPos(X_coef_r.row(0),time_r,0,5)(0,0),
                        coef_generator.GetAccVelPos(Y_coef_r.row(0),time_r,0,5)(0,0),
                        coef_generator.GetAccVelPos(Z_coef_r.row(0),time_r,0,5)(0,0);
                V_r<<   coef_generator.GetAccVelPos(X_coef_r.row(0),time_r,0,5)(0,1),
                        coef_generator.GetAccVelPos(Y_coef_r.row(0),time_r,0,5)(0,1),
                        coef_generator.GetAccVelPos(Z_coef_r.row(0),time_r,0,5)(0,1);

                hand_r.update_right_hand(q_ra,V_r,r_target_r,R_target_r);
                r_right_palm=hand_r.r_right_palm;
                hand_r.doQP(q_ra);
                q_ra=hand_r.q_next;
                d_r=hand_r.dist;
                theta_r=hand_r.theta;
                sai_r=hand_r.sai;
                phi_r=hand_r.phi;
            }

            else if (time_r<t_r(2)){
                P_r<<   coef_generator.GetAccVelPos(X_coef_r.row(1),time_r,t_r(1),5)(0,0),
                        coef_generator.GetAccVelPos(Y_coef_r.row(1),time_r,t_r(1),5)(0,0),
                        coef_generator.GetAccVelPos(Z_coef_r.row(1),time_r,t_r(1),5)(0,0);
                V_r<<   coef_generator.GetAccVelPos(X_coef_r.row(1),time_r,t_r(1),5)(0,1),
                        coef_generator.GetAccVelPos(Y_coef_r.row(1),time_r,t_r(1),5)(0,1),
                        coef_generator.GetAccVelPos(Z_coef_r.row(1),time_r,t_r(1),5)(0,1);

                hand_r.update_right_hand(q_ra,V_r,r_target_r,R_target_r);
                r_right_palm=hand_r.r_right_palm;

                hand_r.doQP(q_ra);
                q_ra=hand_r.q_next;
                d_r=hand_r.dist;
                theta_r=hand_r.theta;
                sai_r=hand_r.sai;
                phi_r=hand_r.phi;
                fingers_r=fingers_mode_r;
                qr_end=q_ra;
                // ROS_INFO("right: %f\t%f\t%f\t%f\t%f\t%f\t%f\t",q_ra(0),q_ra(1),q_ra(2),q_ra(3),q_ra(4),q_ra(5),q_ra(6));
                //ROS_INFO("left:  %f\t%f\t%f\t%f\t%f\t%f\t%f\t",q_la(0),q_la(1),q_la(2),q_la(3),q_la(4),q_la(5),q_la(6));

            }
            else{
                if(scenario_r=="s"){
                    double t=time_r-t_r(2);
                    double L=.1;
                    double T=4;
                    if (t<=T) {
                        double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
//double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
//cout<<P<<endl;
                                                V_r<< 0,-V,0;
                        if (t<=T/4) {
                            T=T/4;
                            V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                            V_r<< 0,0,V;
                           }
                        else if (t<=T/2) {
                            t-=T/4;
                            T=T/4;

                            V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                            V_r<< 0,-V,0;
                           }
                        else if (t<=3*T/4) {
                            t-=T/2;
                            T=T/4;
                            V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                            V_r<< 0,0,-V;
                           }
                        else if (t<=T) {
                            t-=3*T/4;
                            T=T/4;
                            V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                            V_r<< 0,V,0;
                           }

                        hand_r.update_right_hand(q_ra,V_r,r_right_palm,R_target_r);

                        hand_r.doQP(q_ra);
                        q_ra=hand_r.q_next;
                        r_right_palm=hand_r.r_right_palm;
                        d_r=hand_r.dist;
                        theta_r=hand_r.theta;
                        sai_r=hand_r.sai;
                        phi_r=hand_r.phi;
                        fingers_r=fingers_mode_r;
                        qr_end=q_ra;}
                    else{t_r_offset=T+t_r(2);scenario_r="h";
                    }
                }

                else if(scenario_r=="o"){
                    double t=time_r-t_r(2);

                    double L=.07*2*M_PI;
                    double T=4;
                   if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
      V_r<<0,-V*sin(P/L*2*M_PI),V*cos(P/L*2*M_PI);

 hand_r.update_right_hand(q_ra,V_r,r_right_palm,R_target_r);
                        hand_r.doQP(q_ra);
                        q_ra=hand_r.q_next;
                        r_right_palm=hand_r.r_right_palm;
                        d_r=hand_r.dist;
                        theta_r=hand_r.theta;
                        sai_r=hand_r.sai;
                        phi_r=hand_r.phi;
                        fingers_r=fingers_mode_r;
                        qr_end=q_ra;}
                    else{t_r_offset=T+t_r(2);scenario_r="h";
                    }

}

                else if(scenario_r=="ws"){
                    double t=time_r-t_r(2);

                    double L=.1*3/2*M_PI;
                    double T=4;

                    if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
                    if(t<T/2){V_r<<0,-V*sin(3*P/L*M_PI),V*cos(3*P/L*M_PI);}
                    else{V_r<<0,V*cos(-3*(P-L/2)/L*M_PI),V*sin(-3*(P-L/2)/L*M_PI);}

                        hand_r.update_right_hand(q_ra,V_r,r_right_palm,R_target_r);
                        r_right_palm=hand_r.r_right_palm;
                        hand_r.doQP(q_ra);
                        q_ra=hand_r.q_next;
                        d_r=hand_r.dist;
                        theta_r=hand_r.theta;
                        sai_r=hand_r.sai;
                        phi_r=hand_r.phi;
                        fingers_r=fingers_mode_r;
                        qr_end=q_ra;

}

                    else{t_r_offset=T+t_r(2);scenario_r="h";
                    }
                }

                else if(scenario_r=="wu"){
                    double t=time_r-t_r(2);

                    double L=.1*M_PI/2+.3;
                    double T=4;

                    if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);

                    if(P<L/3){V_r<<0,0,-V;}
                    else if(P<2*L/3){V_r<<0,V*sin(-3*M_PI/L*(P-L/3)-M_PI),V*cos(-3*M_PI/L*(P-L/3)-M_PI);}
                    else{V_r<<0,0,V;}

                        hand_r.update_right_hand(q_ra,V_r,r_right_palm,R_target_r);
                        r_right_palm=hand_r.r_right_palm;
                        hand_r.doQP(q_ra);
                        q_ra=hand_r.q_next;
                        d_r=hand_r.dist;
                        theta_r=hand_r.theta;
                        sai_r=hand_r.sai;
                        phi_r=hand_r.phi;
                        fingers_r=fingers_mode_r;
                        qr_end=q_ra;

}

                    else{t_r_offset=T+t_r(2);scenario_r="h";
                    }
                }

                else{

                    if (time_r<t_r(2)*2){

                        if(scenario_r=="g"){q_ra(2)=qr_end(2)+15*M_PI/180*sin((time_r-t_r(2))/2*(2*M_PI));}
                        else if(scenario_r=="f"){
                            if(time_r>t_r(2)+1){
                                q_ra(3)=qr_end(3)+5*M_PI/180*sin((time_r-(t_r(2)+1))/4*(2*M_PI));}
                        }
                        else{
                            ROS_INFO_ONCE("right reached!");
                            //if(scenario_r=="h")

                            MatrixXd t_h(1,2);
                            MatrixXd p_r(7,2); MatrixXd p_l(7,2);
                            MatrixXd zero(1,2); zero<<0,0;
                            MatrixXd r_coeff;  MatrixXd l_coeff;
                            t_h<<t_r(2),t_r(2)*2;

                            p_r<<   qr_end(0),10*M_PI/180,
                                    qr_end(1),-10*M_PI/180,
                                    qr_end(2),0,
                                    qr_end(3),-20*M_PI/180,
                                    qr_end(4),0,
                                    qr_end(5),0,
                                    qr_end(6),0;

                            for (int i = 0; i < 7; ++i) {
                                r_coeff=coef_generator.Coefficient(t_h,p_r.row(i),zero,zero);
                                q_ra(i)=coef_generator.GetAccVelPos(r_coeff.row(0),time_r,t_h(0) ,5)(0,0);

                            }

                            fingers_r=9;

                        }

                        if(scenario_r=="j"&&time_r==t_r(2)){ fingers_r=3;}
                        if(scenario_r=="j"&&time_r==(t_r(2)+.005)){
                            fingers_r=10;
                            for (int var = 0; var < 10*200; ++var) {
                                ros::spinOnce();
                                loop_rate.sleep();
                                //  qDebug()<<"right gripping/finger r:"<<q_motor_r[7]<<"\tl:"<<q_motor_l[7]<<"\tt_l:"<<time_l<<"\tt_r:"<<time_r;
                            }
                        }

                        if(scenario_r=="j"&&time_r==(t_r(2)+.01)){
                            fingers_r=9;
                            for (int var = 0; var < 2*200; ++var) {
                                ros::spinOnce();
                                loop_rate.sleep();
                                //  qDebug()<<"right releasing/finger r:"<<q_motor_r[7]<<"\tl:"<<q_motor_l[7]<<"\tt_l:"<<time_l<<"\tt_r:"<<time_r;
                            }
                        }

                    }
                }
            }//

            if(time_l<t_l(1)){

                P_l<<   coef_generator.GetAccVelPos(X_coef_l.row(0),time_l,0,5)(0,0),
                        coef_generator.GetAccVelPos(Y_coef_l.row(0),time_l,0,5)(0,0),
                        coef_generator.GetAccVelPos(Z_coef_l.row(0),time_l,0,5)(0,0);
                V_l<<   coef_generator.GetAccVelPos(X_coef_l.row(0),time_l,0,5)(0,1),
                        coef_generator.GetAccVelPos(Y_coef_l.row(0),time_l,0,5)(0,1),
                        coef_generator.GetAccVelPos(Z_coef_l.row(0),time_l,0,5)(0,1);

                hand_l.update_left_hand(q_la,V_l,r_target_l,R_target_l);

                r_left_palm=hand_l.r_left_palm;
                //chronometer.restart();
                hand_l.doQP(q_la);

                q_la=hand_l.q_next;
                d_l=hand_l.dist;
                theta_l=hand_l.theta;
                sai_l=hand_l.sai;
                phi_l=hand_l.phi;

            }

            else if (time_l<t_l(2)){

                P_l<<   coef_generator.GetAccVelPos(X_coef_l.row(1),time_l,t_l(1),5)(0,0),
                        coef_generator.GetAccVelPos(Y_coef_l.row(1),time_l,t_l(1),5)(0,0),
                        coef_generator.GetAccVelPos(Z_coef_l.row(1),time_l,t_l(1),5)(0,0);
                V_l<<   coef_generator.GetAccVelPos(X_coef_l.row(1),time_l,t_l(1) ,5)(0,1),
                        coef_generator.GetAccVelPos(Y_coef_l.row(1),time_l,t_l(1),5)(0,1),
                        coef_generator.GetAccVelPos(Z_coef_l.row(1),time_l,t_l(1),5)(0,1);
                hand_l.update_left_hand(q_la,V_l,r_target_l,R_target_l);
                r_left_palm=hand_l.r_left_palm;
                hand_l.doQP(q_la);
                q_la=hand_l.q_next;
                d_l=hand_l.dist;
                theta_l=hand_l.theta;
                sai_l=hand_l.sai;
                phi_l=hand_l.phi;
                fingers_l=fingers_mode_l;
                ql_end=q_la;

            }

            else{
                if(scenario_l=="s"){

                    double t=time_l-t_l(2);
                    double L=.1;
                    double T=4;
                   if(t<T){
                       double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
//double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
//cout<<P<<endl;
                       V_l<< 0,V,0;
                       if (t<=T/4) {
                           T=T/4;
                           V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                           V_l<< 0,0,V;
                          }
                       else if (t<=T/2) {
                           t-=T/4;
                           T=T/4;

                           V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                           V_l<< 0,V,0;
                          }
                       else if (t<=3*T/4) {
                           t-=T/2;
                           T=T/4;
                           V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                           V_l<< 0,0,-V;
                          }
                       else if (t<=T) {
                           t-=3*T/4;
                           T=T/4;
                           V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                           V_l<< 0,-V,0;
                          }

                        hand_l.update_left_hand(q_la,V_l,r_left_palm,R_target_l);
                        r_left_palm=hand_l.r_left_palm;
                        hand_l.doQP(q_la);
                        q_la=hand_l.q_next;
                        d_l=hand_l.dist;
                        theta_l=hand_l.theta;
                        sai_l=hand_l.sai;
                        phi_l=hand_l.phi;
                        fingers_l=fingers_mode_l;
                        ql_end=q_la;

}

                    else{t_l_offset=T+t_l(2);scenario_l="h";
                    }
                }
                else if(scenario_l=="o"){
                    double t=time_l-t_l(2);

                    double L=.07*2*M_PI;
                    double T=4;
                   if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
      V_l<<0,V*sin(P/L*2*M_PI),V*cos(P/L*2*M_PI);

                        hand_l.update_left_hand(q_la,V_l,r_left_palm,R_target_l);
                        r_left_palm=hand_l.r_left_palm;
                        hand_l.doQP(q_la);
                        q_la=hand_l.q_next;
                        d_l=hand_l.dist;
                        theta_l=hand_l.theta;
                        sai_l=hand_l.sai;
                        phi_l=hand_l.phi;
                        fingers_l=fingers_mode_l;
                        ql_end=q_la;

}

                    else{t_l_offset=T+t_l(2);scenario_l="h";
                    }
                }

                else if(scenario_l=="ws"){
                    double t=time_l-t_l(2);

                    double L=.1*3/2*M_PI;
                    double T=4;

                    if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
                    if(t<T/2){V_l<<0,-V*sin(3*P/L*M_PI),V*cos(3*P/L*M_PI);}
                    else{V_l<<0,V*cos(-3*(P-L/2)/L*M_PI),V*sin(-3*(P-L/2)/L*M_PI);}

                        hand_l.update_left_hand(q_la,V_l,r_left_palm,R_target_l);
                        r_left_palm=hand_l.r_left_palm;
                        hand_l.doQP(q_la);
                        q_la=hand_l.q_next;
                        d_l=hand_l.dist;
                        theta_l=hand_l.theta;
                        sai_l=hand_l.sai;
                        phi_l=hand_l.phi;
                        fingers_l=fingers_mode_l;
                        ql_end=q_la;

}

                    else{t_l_offset=T+t_l(2);scenario_l="h";
                    }
                }

                else if(scenario_l=="wu"){
                    double t=time_l-t_l(2);

                    double L=.1/2*M_PI+.3;
                    double T=4;

                    if(t<T){
                    double V=30*L/pow(T,3)*pow(t,2)-60*L/pow(T,4)*pow(t,3)+30*L/pow(T,5)*pow(t,4);
                    double P=10*L/pow(T,3)*pow(t,3)-15*L/pow(T,4)*pow(t,4)+6*L/pow(T,5)*pow(t,5);
                    if(P<L/3){V_l<<0,0,-V;}
                    else if(P<2*L/3){V_l<<0,V*sin(-3*M_PI/L*(P-L/3)-M_PI),V*cos(-3*M_PI/L*(P-L/3)-M_PI);}
                    else{V_l<<0,0,V;}

                        hand_l.update_left_hand(q_la,V_l,r_left_palm,R_target_l);
                        r_left_palm=hand_l.r_left_palm;
                        hand_l.doQP(q_la);
                        q_la=hand_l.q_next;
                        d_l=hand_l.dist;
                        theta_l=hand_l.theta;
                        sai_l=hand_l.sai;
                        phi_l=hand_l.phi;
                        fingers_l=fingers_mode_l;
                        ql_end=q_la;

}

                    else{t_l_offset=T+t_l(2);scenario_l="h";
                    }
                }

                else{
                    if (time_l<t_l(2)*2){

                        if(scenario_l=="g"){q_la(2)=ql_end(2)+15*M_PI/180*sin((time_l-t_l(2))/2*(2*M_PI));}
                        else if(scenario_l=="f"){
                            if (time_l>t_l(2)+1)
                            {q_la(3)=ql_end(3)+5*M_PI/180*sin((time_l-(t_l(2)+1))/2*(2*M_PI));}
                        }
                        else{
                            ROS_INFO_ONCE("left reached!");
                            //  if(scenario_l=="h")

                            MatrixXd t_h(1,2);
                            MatrixXd p_l(7,2);
                            MatrixXd zero(1,2); zero<<0,0;
                            MatrixXd l_coeff;
                            t_h<<t_l(2),t_l(2)*2;

                            p_l<<   ql_end(0),10*M_PI/180,
                                    ql_end(1),10*M_PI/180,
                                    ql_end(2),0,
                                    ql_end(3),-20*M_PI/180,
                                    ql_end(4),0,
                                    ql_end(5),0,
                                    ql_end(6),0;

                            for (int i = 0; i < 7; ++i) {
                                l_coeff=coef_generator.Coefficient(t_h,p_l.row(i),zero,zero);
                                q_la(i)=coef_generator.GetAccVelPos(l_coeff.row(0),time_l,t_h(0) ,5)(0,0);
                            }
                            fingers_l=9;

                        }

                        if(scenario_l=="j"&&time_l==t_l(2)){ fingers_l=3;}

                        if(scenario_l=="j"&&time_l==(t_l(2)+.005)){
                            if(scenario_l=="j"){ fingers_l=10;}
                            for (int var = 0; var < 10*200; ++var) {
                                ros::spinOnce();
                                loop_rate.sleep();
                                // qDebug()<<"left gripping/finger r:"<<q_motor_r[7]<<"\tl:"<<q_motor_l[7]<<"\tt_l:"<<time_l<<"\tt_r:"<<time_r;
                            }

                        }

                        if((scenario_l=="j"&&time_l==(t_l(2)+.01))){
                            if(scenario_l=="j"){ fingers_l=9;}

                            for (int var = 0; var < 2*200; ++var) {
                                ros::spinOnce();
                                loop_rate.sleep();
                                // qDebug()<<"left releasing/finger r:"<<q_motor_r[7]<<"\tl:"<<q_motor_l[7]<<"\tt_l:"<<time_l<<"\tt_r:"<<time_r;
                            }

                        }
                    }
                }

            }

        if(scenario_l!="g"&&scenario_l!="f"&&scenario_r!="g"&&scenario_r!="f"){
            if(time_r==(t_r(2)-.005)||time_l==(t_l(2)-.005)){
                for (int var = 0; var < 2*200; ++var) {
                    ros::spinOnce();
                    loop_rate.sleep();
                }
            }
        }
          if(time_r>2*t_r(2)&&time_l>2*t_l(2)) {
              move_activate=false;
              qDebug()<<"done!";
          }

        //    matrix_view(P_r); matrix_view(P_l);
    }
    qr1.append(q_ra(0));    ql1.append(q_la(0));
    qr2.append(q_ra(1));    ql2.append(q_la(1));
    qr3.append(q_ra(2));    ql3.append(q_la(2));
    qr4.append(q_ra(3));    ql4.append(q_la(3));
    qr5.append(q_ra(4));    ql5.append(q_la(4));
    qr6.append(q_ra(5));    ql6.append(q_la(5));
    qr7.append(q_ra(6));    ql7.append(q_la(6));

    //   ROS_INFO("q1=%f\tq2=%f\tq3=%f\tq4=%f\tq5=%f\tq6=%f\tq7=%f\t",180/M_PI*q[15],180/M_PI*q[16],180/M_PI*q[17],180/M_PI*q[18],180/M_PI*q[19],180/M_PI*q[20],180/M_PI*q[21]);
    //            ROS_INFO("t=%f\nx=%f\ty=%f\tz=%f\t\nX_des=%f\tY_des=%f\tZ_des=%f",time,hand.r_right_palm(0),hand.r_right_palm(1),hand.r_right_palm(2),
    //                  P(0),P(1),P(2));
    //        ROS_INFO("psi=%f(%f)\ttheta=%f(%f)\tphi=%f(%f)\t",hand.sai,hand.sai_target,hand.theta,hand.theta_target,hand.phi,hand.phi_target);

    for (int i = 0; i < 31; ++i) {
        q[i]=0;
    }

    //         q[15]=-M_PI/2;
    //         q[16]=-10*M_PI/180;
    //         q[17]=-10*M_PI/180;

    q[15]=qr1[count];   q[15+7]=ql1[count];
    q[16]=qr2[count];   q[16+7]=ql2[count];
    q[17]=qr3[count];   q[17+7]=ql3[count];
    q[18]=qr4[count];   q[18+7]=ql4[count];
    q[19]=qr5[count];   q[19+7]=ql5[count];
    q[20]=qr6[count];   q[20+7]=ql6[count];
    q[21]=qr7[count];   q[21+7]=ql7[count];
q[14]=move2pose(WaistPitch,time,0,t_r(2))-move2pose(WaistPitch,time,t_r(2),2*t_r(2));
q[13]=move2pose(WaistYaw,time,0,t_r(2))-move2pose(WaistYaw,time,t_r(2),2*t_r(2));

    //     ROS_INFO("%f,%f,%f,%f,%f,%f,%f",q[15],q[16],q[17],q[18],q[19],q[20],q[21]);
    //   ROS_INFO("T=%f upper:%f %f lower:%f %f qdot=%f q=%f %f",hand.T,(hand.toRad(10)+q_ra(1))/hand.T,hand.qdot_max,(hand.toRad(-90)+q_ra(1))/hand.T,-hand.qdot_max,hand.qdot(2),180/M_PI*q[16],180/M_PI*q_ra(1));
    if(simulation){SendGazebo(q);}

    q_motor_r[0]=-int(10*(qr1[count]-q0_r(0))*180/M_PI*120/60)+qra_offset[0];
    q_motor_r[1]=int(10*(qr2[count]-q0_r(1))*180/M_PI*120/60)+qra_offset[1];
    q_motor_r[2]=-int(7*(qr3[count]-q0_r(2))*180/M_PI*100/60)+qra_offset[2];
    q_motor_r[3]=int(7*(qr4[count]-q0_r(3))*180/M_PI*100/60)+qra_offset[3];

    q_motor_r[4]=int((qr5[count]-q0_r(4))*(2048)/M_PI);
    q_motor_r[5]=int((qr6[count]-q0_r(5))*(4000-2050)/(23*M_PI/180));
    q_motor_r[6]=int((qr7[count]-q0_r(6))*(4000-2050)/(23*M_PI/180));

    if(q_motor_r[7]!=fingers_r&&q_motor_r[7]!=2){
        q_motor_r[7]=2;
    }
    else{
        q_motor_r[7]=fingers_r;
    }

    q_motor_l[0]=int(10*(ql1[count]-q0_l(0))*180/M_PI*120/60)+qla_offset[0];
    q_motor_l[1]=int(10*(ql2[count]-q0_l(1))*180/M_PI*120/60)+qla_offset[1];
    q_motor_l[2]=-int(7*(ql3[count]-q0_l(2))*180/M_PI*100/60)+qla_offset[2];
    q_motor_l[3]=-int(7*(ql4[count]-q0_l(3))*180/M_PI*100/60)+qla_offset[3];

    q_motor_l[4]=int((ql5[count]-q0_l(4))*(2048)/M_PI);
    q_motor_l[5]=-int((ql6[count]-q0_l(5))*(4000-2050)/(23*M_PI/180));
    q_motor_l[6]=int((ql7[count]-q0_l(6))*(4000-2050)/(23*M_PI/180));

    if(q_motor_l[7]!=fingers_l&&q_motor_l[7]!=2){
        q_motor_l[7]=2;
    }
    else{
        q_motor_l[7]=fingers_l;
    }

    msg.data.clear();
    for(int  i = 0;i < 12;i++)
    {
        msg.data.push_back(qc_offset[i]);
    }
    //right hand epose
    msg.data.push_back(q_motor_r[0]);//12 -y  a,z
    msg.data.push_back(q_motor_r[1]);//13 +x
    msg.data.push_back(q_motor_r[2]);//14 -z
    msg.data.push_back(q_motor_r[3]);//15 +y
    //right hand dynamixel + fingers
    msg.data.push_back(q_motor_r[4]);//16
    msg.data.push_back(q_motor_r[5]);//17
    msg.data.push_back(q_motor_r[6]);//18
    msg.data.push_back(q_motor_r[7]);//19
    //left hand epose
    msg.data.push_back(q_motor_l[0]);//20 +y
    msg.data.push_back(q_motor_l[1]);//21 +x
    msg.data.push_back(q_motor_l[2]);//22 -z
    msg.data.push_back(q_motor_l[3]);//23 -y
    //left hand dynamixel + fingers
    msg.data.push_back(q_motor_l[4]);//24
    msg.data.push_back(q_motor_l[5]);//25
    msg.data.push_back(q_motor_l[6]);//26
    msg.data.push_back(q_motor_l[7]);//27
    ++count;
    if(!simulation){chatter_pub.publish(msg);}

    joint_data.append(":["+QString::number(q_motor_r[0])+","
            +QString::number(q_motor_r[1])+","
            +QString::number(q_motor_r[2])+","
            +QString::number(q_motor_r[3])+","
            +QString::number(q_motor_r[4])+","
            +QString::number(q_motor_r[5])+","
            +QString::number(q_motor_r[6])+","
            +QString::number(q_motor_r[7])+","
            +QString::number(q_motor_l[0])+","
            +QString::number(q_motor_l[1])+","
            +QString::number(q_motor_l[2])+","
            +QString::number(q_motor_l[3])+","
            +QString::number(q_motor_l[4])+","
            +QString::number(q_motor_l[5])+","
            +QString::number(q_motor_l[6])+","
            +QString::number(q_motor_l[7])+"]\n");
//qDebug()<<chronometer.elapsed();
//    qDebug()<<"right end effector position:";
//    matrix_view(r_right_palm);

//    VectorXd tempo=q_ra*180/M_PI;
//     matrix_view(tempo);

//    qDebug()<<"left end effector position:";
//    matrix_view(r_left_palm);

    // qDebug()<<"finger r:"<<q_motor_r[7]<<"\tl:"<<q_motor_l[7];
}
ros::spinOnce();
loop_rate.sleep();
}

    QFile myfile("Desktop/hands_joint_data.txt");
    myfile.remove();
    myfile.open(QFile::ReadWrite);
    myfile.write(joint_data);
    myfile.close();

return 0;
}