#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/tf.h>

int main( int argc, char* argv[] )
{
    ros::init(argc, argv, "test_tf");
    ros::NodeHandle nh;

    tf::TransformListener listener;
    geometry_msgs::PoseStamped in_pose;
    in_pose.header.frame_id = "parent";
    in_pose.header.stamp = ros::Time(0);
    in_pose.pose.position.x = 1;
    in_pose.pose.position.y = 2;
    in_pose.pose.position.z = 0;

    in_pose.pose.orientation.x = 0;
    in_pose.pose.orientation.y = 0;
    in_pose.pose.orientation.z = 0.344215153;
    in_pose.pose.orientation.w = 0.93889079678;
    ros::Rate r(10);
    while(nh.ok() )
    {
        out_pose.header.frame_id = "child";
        // listener->waitForTransform("child", "parent", ros::Time(0),  ros::Duration(1) );
        listener->transformPose("child", in_pose, out_pose);
        r.sleep();
    }
}

inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
{
   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
   {
     std::stringstream ss;
     ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
     throw tf::InvalidArgument(ss.str());
   }
};

Gnuplot g1;

void plot(const vector<double>& Xs, const vector<double>& Ys, const string& comment, const string& style)
{
    g1.set_grid();
    g1.set_pointsize(3);
    g1.set_style(style).plot_xy(Xs, Ys, comment);
}

void wait_for_key()
{
    cout << "Press Enter to continue ..." <<endl;
    cin.clear();
    cin.ignore(cin.rdbuf()->in_avail() );
    cin.get();
    return;
}

            /*以下为main函数部分*/
std::vector<double> X_t, Y_t;
/*对所有 vector 插入数据*/

g1.set_grid();
g1.set_xlabel("X");  //设置x轴说明
g1.set_ylabel("Y");  //设置y轴说明
// 图例文字
g1.set_style("lines").plot_xy(X_t, Y_t, "left");
plot(X_t, Y_t, "left", "points lc rgb 'red' pt 7");

wait_for_key();

std::string s("Hello");
std::string f;

// 前两个参数是变换的范围，第三个是变换的起始位置
// 第四个参数为lambda表达式，变换的规则
std::transform(s.begin(), s.end(), s.begin(),
  [](unsigned char c) { return std::toupper(c); });
std::cout <<"after transform, s: "<< s << std::endl;  // HELLO

std::transform( s.begin(), s.end(), std::back_inserter(f),
              [](unsigned char c) { return std::tolower(c); }  );
std::cout << "after transform, f: " << f << std::endl;  // hello

std::copy( s.begin(), s.end(), std::back_inserter(f) );
std::cout << "after copy, f: " << f << std::endl;  // helloHELLO

vector<int> v={1,2,3,4,5,6};
vector<int> s;
copy(v.begin(), v.end(), back_inserter(s) );  // s: 123456

tf2_ros::Buffer buffer_;
tf2_ros::TransformListener listener(buffer_);
buffer_.transform(in_pose, out_pose, frame, transform_duration_);

vector<std::string> params;
std::string file = "123.45.abc.0";
std::istringstream param_stream(file);
std::string param;
while(getline(param_stream, param, '.'))
{
    cout << param <<endl;
    params.push_back(param);
}

bool readCSVInput(int N, vector<double> &input_points_x, vector<double> &input_points_y, vector<double> &anchor_points_x, vector<double> &anchor_points_y)
{
    ifstream readCSV("/home/user/data.csv", ios::in);
    int lineNum = 0;
    string lineString;
    vector<vector<string>> stringArray;
    cout << endl;
    while (getline(readCSV, lineString) )
    {
        //cout << lineString << endl;
        if (lineNum > 0)
        {
            stringstream ss(lineString);
            string s;
            vector<string> oneLineStrings;
            while (getline(ss, s, ',') )
                oneLineStrings.push_back(s);
            if (oneLineStrings.size() != 5)
            {
                cout << "ERROR:oneLineStrings.size() != 5" << endl;
                return false;
            }
            else
            {
                input_points_x.push_back(std::stod(oneLineStrings[1]));
                input_points_y.push_back(std::stod(oneLineStrings[2]));
                anchor_points_x.push_back(std::stod(oneLineStrings[3]));
                anchor_points_y.push_back(std::stod(oneLineStrings[4]));
            }
        }
        lineNum++;
    }
    if (N == input_points_x.size())
    {
        return true;
    }
    else
    {
        input_points_x.clear();
        input_points_y.clear();
        anchor_points_x.clear();
        anchor_points_y.clear();
        cout << "ERROR:N == input_points_x.size()" << endl;
        return false;
    }
}