#include <math.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include "autoware_msgs/DetectedObject.h"
#include <autoware_msgs/DetectedObjectArray.h>
#include "zr_msgs/object_info.h"
#include "autoware_msgs/PointsImage.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <string>
#include <cstdlib>
#include <vector>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include <deque>
float X;
float Y;                                               //X and Y are the coordinates of the cloest person to the Camera.

void follow_vel_control(float &dist_person_min)
{
    float follow_range = 1.5;
    float vel_diff_turn = 0.1; // 定义左右转速度的改变量的大小， 大小还需要调整
    //  float vel_diff_turn = 0.15;
    //     float theta_ = (minIndex/2 + 67.5) * 3.14159265 / 180;
    //fix the vel in follow mode
    float robot_radius = 0.3;
    float vel_L = 0.8;
    float vel_R = 0.8;
   // float y = Y;
    float x = X;
    //     ROS_INFO("Index: %f    ; Min: %f", minIndex, rangeMin);
    //     ROS_INFO("X: %f    ; Y: %f", x, y);

    //case go straight
    if (fabs(x) <= robot_radius + 0.1) //在竖直方向判断
    {
        ROS_INFO("now the robot and the leader are in the same line");
        if (dist_person_min > (follow_range + 0.2))
        {
            ROS_INFO("the leader is far with robot, the robot will speed up to make the distance smaller!!!");
            vel_L = vel_L * (1.0 - follow_range / dist_person_min); //这里加一个反馈控制，距离越远，车的速度越大
            vel_R = vel_R * (1.0 - follow_range / dist_person_min);
        }
        if (dist_person_min <= (follow_range + 0.2) && dist_person_min >= (follow_range - 0.2)) //定义跟随范围为1.3---1.7
        {
            ROS_INFO("the distance betwwen the robot and leader is in good value");
            vel_L = 0;
            vel_R = 0;
        }
        if (dist_person_min < (follow_range - 0.2))
        {
            ROS_INFO("the distance now is too small, please be careful!!!");
            vel_R = 0;
            vel_L = 0; //这里是否考虑刹车？如果小于跟随范围，小车还有速度的话，是否考虑刹车 brake_flag
        }
    }
    if (x < (-robot_radius - 0.1)) //判断人是否左偏， turn left
    {
        vel_L = 0.5;
        vel_R = 0.5;
        ROS_INFO("now the leader is on the robot's left");
        float diff_L = fabs(x) - fabs(-robot_radius - 0.1); //定义人的左偏程度
        vel_L -= vel_diff_turn * (diff_L / fabs(x));        //这里diff/fabs(x)为左偏程度， 人左偏的越厉害，轮子速度改变量越大，角速度也越大
        vel_R += vel_diff_turn * (diff_L / fabs(x));
        if (x > robot_radius + 0.1) //判断人是否偏右， 偏右则右转
        {
            vel_L = 0.5;
            vel_R = 0.5;
            ROS_INFO("now the leader is on the robot's right");
            float diff_R = x - (robot_radius + 0.1);
            vel_L -= vel_diff_turn * (diff_R / fabs(x)); //这里diff/fabs(x)为右偏程度， 人右偏的越厉害，轮子速度改变量越大，角速度也越大
            vel_R += vel_diff_turn * (diff_R / fabs(x));
        }

    }
    std::cout << "vel_L is " << vel_L << std::endl;
    std::cout << "vel_R is " << vel_R << std::endl;
}

void Person_select_follow(std::vector<float> &distance_raw, std::vector<std::string> &label_raw, std::vector<float> &x_raw, std::vector<float> &y_raw)
{
    int n = distance_raw.size();
    std::vector<std::string> label; // label of person
    std::vector<float> x;
    std::vector<float> y;
    std::vector<float> distance;
//    std::cout << "n = :::"<< n <<std::endl;
//     std::cout << "444 = :::"<< distance_raw[n-1] <<std::endl;
        for (int i = 0; i < n; i++)
        {
            if (label_raw[i] == "person")
            {
                distance.push_back(distance_raw[i]);
                label.push_back(label_raw[i]);
                x.push_back(x_raw[i]);
                y.push_back(y_raw[i]);
            }
        }
        if (!distance.size())
        {
            ROS_INFO("there is no leader before the robot, please make sure there is one leader");
            return;
        }
        else
        {
            int num = distance.size();
            int num1 = label.size();
            int num2 = x.size();
            if (!(num = num1 = num2))
            {
                ROS_INFO("there is something wrong");
                return;
            }
            else
            {
                float dist_person_min = 999;
                float min_index = -1;
                for (int i = 0; i < num; i++)
                {
                    if (dist_person_min > distance[i])
                    {
                        dist_person_min = distance[i];
                        min_index = i;
                    }
                }
                std::cout << "distance = " << dist_person_min << std::endl;
                std::cout << "55555===" << distance[num-1]<<std::endl;
                X = x[min_index];
                Y = y[min_index];
                std::cout << "x in the space is  " << X << std::endl;
                std::cout << "y in the space is  " << Y << std::endl;
                follow_vel_control(dist_person_min);
            }
        }
}
//by jie get the disdtance of all labels
static void DistanceCallback(const zr_msgs::object_info &result)
{
    std::vector<float> distance_raw;    // all objects (people, car ...)
    std::vector<std::string> label_raw; //all labels
    std::vector<float> x_raw;
    std::vector<float> y_raw;
    if (result.distance.empty())
    {
        ROS_ERROR("this frame did not detect anything, it will enter into next frame!!!!!");
        return;
    }
    //ROS_INFO("size of distance", result.distance.size());
    // std::cout << "222222====" << result.distance.size() << std::endl;
    // std::cout << "333333====" << result.label.size() << std::endl;
    distance_raw = result.distance;
    //label.push_back(result->label);
    label_raw = result.label;
    x_raw = result.x;
    y_raw = result.y;
    //std::cout<<"size=   "<<distance_raw.size()<<std::endl;
    std::cout << label_raw[0] << std::endl;
    Person_select_follow(distance_raw, label_raw, x_raw, y_raw);
    //     bounding_box_raw.clear();
}

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

    // Write to console that we are starting trajectory generation
    //#ifdef Z_DEBUG
    //	ROS_INFO_STREAM("Trajectory Generation Begins: ");
    //#endif
    ros::init(argc, argv, "follower_node_1");

    // Create node handles
    ros::NodeHandle nh;
    //ros::NodeHandle private_nh("~");

    // Publish the following topics:
    //    g_vis_pub = nh.advertise<visualization_msgs::Marker>("next_waypoint_mark", 1);
    //    g_vis_clo_pub = nh.advertise<visualization_msgs::Marker>("closest_waypoint_mark", 1);
    // Publish the curvature information:
    // ros::Publisher vel_L_R_pub = nh.advertise<geometry_msgs::TwistStamped>("/twist_raw", 10);
    //    ros::Publisher state_parameters_pub = nh.advertise<std_msgs::Float64MultiArray>("lattice_state", 10);

    // Subscribe to the following topics:
    ros::Subscriber distance_subscriber = nh.subscribe("/obj_detect_distance", 1, DistanceCallback);
    //  ros::Subscriber  label_coord_subscriber = nh.subscribe("/detection/image_detector/objects",1,CoordCallback);
    //    ros::Subscriber  intrinsic = nh.subscribe("/camera_info", 1, intrinsic_callback);
     ros::Rate loop_rate(25);
    //Person_sort();
    //   pseudo_follow();

     while (ros::ok())
      {

     ros::spinOnce();
      loop_rate.sleep();

      }
    //ros::spin();
    return 0;
}


#include <functional>
#include <iostream>
#include <vector>
#include <catch2/catch.hpp>
#include <Eigen/Dense>
#include "function_dispatcher.h"

/**
 * Check the dispatcher class implementation
 */
TEST_CASE("Check the dispatcher class implementation", "[Dispatcher]") {

  SECTION("Create dispatcher at compile time") {
    std::function<double(int)> add_small_number = [](int input) -> double {
      return static_cast<double>(input) + 0.01;
    };

    std::function<std::vector<double>(const std::vector<double> &, double)>
        multiply_vector = [](const std::vector<double> input,
                             double multiplier) -> std::vector<double> {
      std::vector<double> output(input.size());

      for (unsigned int i = 0; i < input.size(); ++i) {
	output[i] = input[i] * multiplier;
      };

      return output;
    };

    DispatchRegister<double, int> add_small_number_func("AddSmallNumber", add_small_number);
    DispatchRegister<std::vector<double>, std::vector<double>, double>
        vector_mull("MultiplyVecs", multiply_vector);

    // Create dispatchers
    REQUIRE(Dispatcher<double, int>::instance()->check("AddSmallNumber") == true);
    REQUIRE(Dispatcher<std::vector<double>, std::vector<double>, double>::instance()->check("MultiplyVecs"));

    // Check correct functions are called
    auto answer_1 =
        Dispatcher<double, int>::instance()->dispatch("AddSmallNumber", 3);
    REQUIRE(answer_1 == Approx(3.0 + 0.01).epsilon(0.01));

    std::vector<double> input_vector{0.0, 1.0, 2.0, 3.0};
    double multiplier = 2.0;
    auto answer_2 = Dispatcher<std::vector<double>, std::vector<double>,
                               double>::instance()
                        ->dispatch("MultiplyVecs", input_vector, multiplier);

    for (unsigned int i = 0; i < input_vector.size(); ++i) {
      REQUIRE(input_vector[i] * multiplier ==
              Approx(answer_2[i]).epsilon(0.01));
    }

    // Create a non-existant element
    REQUIRE(
        Dispatcher<std::vector<double>, std::vector<double>, double>::instance()
            ->check("NoFunc") == false);
    try {
      auto no_func = Dispatcher<std::vector<double>, std::vector<double>,
                                double>::instance()
                         ->dispatch("NoFunc", std::vector<double>(), 1.0);
    } catch (std::exception &exception) {
      std::cout << "Dispatcher error: " << exception.what() << std::endl;
    }
  }
}


/*********************************************************************************
*  This file is part of reference implementation of SIGGRAPH Asia 2021 Paper     *
*  `Efficient and Robust Discrete Conformal Equivalence with Boundary`           *
*  v1.0                                                                          *
*                                                                                *
*  The MIT License                                                               *
*                                                                                *
*  Permission is hereby granted, free of charge, to any person obtaining a       *
*  copy of this software and associated documentation files (the "Software"),    *
*  to deal in the Software without restriction, including without limitation     *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,      *
*  and/or sell copies of the Software, and to permit persons to whom the         *
*  Software is furnished to do so, subject to the following conditions:          *
*                                                                                *
*  The above copyright notice and this permission notice shall be included in    *
*  all copies or substantial portions of the Software.                           *
*                                                                                *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR    *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,      *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE  *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER        *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING       *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS  *
*  IN THE SOFTWARE.                                                              *
*                                                                                *
*  Author(s):                                                                    *
*  Marcel Campen, Institute for Computer Science, Osnabrück University, Germany. *
*  Ryan Capouellez, Hanxiao Shen, Leyi Zhu, Daniele Panozzo, Denis Zorin,        *
*  Courant Institute of Mathematical Sciences, New York University, USA          *
*                                          *                                     *
*********************************************************************************/

#ifndef HALFEDGE_HH
#define HALFEDGE_HH
#include <vector>
#include <Eigen/Sparse>

struct Connectivity
{
    std::vector<int> n;    // #h, next halfedge id
    std::vector<int> prev; // #h, previous halfedge id
    std::vector<int> to;   // #h, vertex id the current halfedge is pointing to
    std::vector<int> f;    // #h, face id on the left of the halfedge
    std::vector<int> h;    // #f, one halfedge that's inside the current face
    std::vector<int> out;  // #v, one halfedge that's pointing from the current vertex
    std::vector<int> opp;  // #h, opposite halfedge id
};

/**
* Convert from matrix (F) mesh representation to NOB data structure
*
* @param F, #f*n, each row represents the vertex id (ccw) of current face
* @param (N)next_he, size #h vector, next halfedge id
* @param (O)opp, size #h vector, opposite halfedge id
* @param (B)bnd_loops, collection of boundary face ids.
* @param vtx_reindex, map from new vertices to old (vertex id in NOB is different from F)
* @return void
*/
void FV_to_NOB(const std::vector<std::vector<int>> &F,
               std::vector<int> &next_he,
               std::vector<int> &opp,
               std::vector<int> &bnd_loops,
               std::vector<int> &vtx_reindex);

void FV_to_NOB(const Eigen::MatrixXi &F,
               std::vector<int> &next_he,
               std::vector<int> &opp,
               std::vector<int> &bnd_loops,
               std::vector<int> &vtx_reindex);

/**
* Extend next_he and opp to add extra halfedges along the boundaries.
*
* @param next_he, next-halfedge map same length as opp
* @param opp, halfedge map, for boundary halfedges -1
* @param next_he_ext, next_halfedge map, same length as opp_ext; newly added halfedges are linked into boundary loops
* @param opp_ext, opp with an extra coupled halfedge added at the end for each boundary halfedge
*                 all halfedges have a pair
* @return void
*/
void build_boundary_loops(const std::vector<int> &next_he,
                          const std::vector<int> &opp,
                          std::vector<int> &next_he_ext,
                          std::vector<int> &opp_ext);

/**
* Build orbits following next id recorded in perm.
*
* @param perm, a permutation  given by a list of non-repeating integers in the range 0..len(perm)
* @param cycles, a list of lists, each list represents a cycle of perm
* @return void
*/
void build_orbits(const std::vector<int> &perm,
                  std::vector<std::vector<int>> &cycles);

void bnd_face_list(const std::vector<int> &next_he,
                   const int n_he,
                   std::vector<int> &bnd_loops);

template <class T>
void flatten(const std::vector<std::vector<T>> &arr,
             std::vector<T> &arr_flat)
{
    for (int i = 0; i < arr.size(); ++i)
    {
        for (int j = 0; j < arr[i].size(); ++j)
        {
            arr_flat.push_back(arr[i][j]);
        }
    }
}

/**
 * Given next_he, opp, bnd_loops as defined in Connectivity, infer the rest fields.
 * @param next_he(n), opp, bnd_loops (NOB)
 * @param Connectivity structure C, with fields above plus inferred from these:
 *    f:    index of face for each halfedge (faces ordered as next_he orbits, i.e., by minimal halfedge index
 *    prev: inverse of next_he
 *    to:   index of the vertex halfedge is pointing to (vertices ordered as circulator orbits, i.e., by minimal index
 *          of the halfedge pointing from v
 *    h:    an index of a halfedge of f (initially set to the min index halfedge in the face)
 *    out:  an index of a halfedge pointing from v (initially set to min index)
*/
void NOB_to_connectivity(const std::vector<int> &next_he,
                         const std::vector<int> &opp,
                         const std::vector<int> &bnd_loops,
                         Connectivity &C,
                         const std::vector<int> &diag_he = std::vector<int>());
/**
 * Creates a doubled mesh with reflectional symmetry map and corresponding labels froma   mesh connectivity with (possibly empty) boundary, specified by next_he, opp, bnd_loops
 * ASSUMES that the boundary loop halfedges are at the end of indices so boundary loops are at the end
 * construct a mesh with double number of faces and glued to the original mesh along the boundary
 * @params: next_he, opp, bnd_loops
 * @params: Connectivity and Reflection structures for the double mesh
 * @return: void
 */
void NOB_to_double(const std::vector<int> &next_he_in,
                   const std::vector<int> &opp_in,
                   const std::vector<int> &bnd_loops_in,
                   Connectivity &C,
                   std::vector<char> &type,
                   std::vector<int> &R);

/**
 * Partition vertices in a doubled mesh with reflection map into independent and dependent
   vertices for symmetric functions on the vertices of the mesh, and create a map from the
   vertices to their corresponding independent vertices.
 * @params: out, to, and Reflection structures for the double mesh
 * @params: indep_vtx, dep_vtx partitioned sets of vertices
 * @params: v_rep map of vertices to independent vertex
 * @return: void
 */
void find_indep_vertices(const std::vector<int> &out,
                         const std::vector<int> &to,
                         const std::vector<char> &type,
                         const std::vector<int> &R,
                         std::vector<int> &indep_vtx,
                         std::vector<int> &dep_vtx,
                         std::vector<int> &v_rep);

/**
 * Dependent vertices of a doubled mesh are identified with their corresponding independent vertex
 * @params: to, out, and Reflection structures for the double mesh
 * @return: void
 */
void create_tufted_cover(const std::vector<char> &type,
                         const std::vector<int> &R,
                         const std::vector<int> &indep_vtx,
                         const std::vector<int> &dep_vtx,
                         const std::vector<int> &v_rep,
                         std::vector<int> &out,
                         std::vector<int> &to);

template <typename T>
void permute(std::vector<T> &arr,
             const std::vector<int> &perm)
{
    std::vector<T> arr_copy = arr;
    for (int i = 0; i < arr.size(); ++i)
    {
        arr[perm[i]] = arr_copy[i];
    }
}

/**
 * Infer the intial edge lengths from 3d vertices positions defined in matrix V.
 * @params: Connectivity defined on the top
 * @params: V, #v*3 matrix, represents 3d-coordinates for each vertex
 * @params: vtx_reindex, map between new mesh indices (in Connectivity) to old (in V)
 * @return: void
 */
template <typename Scalar>
void compute_l_from_vertices(const Connectivity &C,
                             const Eigen::MatrixXd &V,
                             const std::vector<int> &vtx_reindex,
                             std::vector<Scalar> &l)
{
    int n_he = C.to.size();
    l.resize(n_he);

    for (int he = 0; he < n_he; ++he)
    {
        // Get the verticess at the tip and tail of the halfedge
        Eigen::Vector3d v_to = V.row(vtx_reindex[C.to[he]]);
        Eigen::Vector3d v_fr = V.row(vtx_reindex[C.to[C.opp[he]]]);

        // Compute the length of the halfedge from the displacement vector
        Eigen::Vector3d vec_disp = v_to - v_fr;
        l[he] = sqrt(vec_disp.dot(vec_disp));
    }
}

std::vector<int> range(int start, int end);

#endif