#include <iostream>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <ros_message.pb.h>
#include "message_communicator.h"
#include "pathcreator.h"
#include <math.h>

using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size);
void init_glog();

ros::NodeHandle* gp_nh= nullptr;
ros::Subscriber subGoal;
std::map<std::string,ros::Publisher*>       g_topic_map;

sensor_msgs::PointCloud create_cloudmsg(pcl::PointCloud<pcl::PointXYZ> cloud_pcl,std::string frame_id)
{
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = frame_id;
    cloud.points.resize(cloud_pcl.size());
    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "xyz";
    cloud.channels[0].values.resize(cloud_pcl.size());

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < cloud_pcl.size(); ++i){
        cloud.points[i].x = cloud_pcl[i].x;
        cloud.points[i].y = cloud_pcl[i].y;
        cloud.points[i].z = cloud_pcl[i].z;
        cloud.channels[0].values[i] = 255;
    }
    return cloud;
}


Error_manager Ros_data_callback(Communication_message* p_msg)
{
    message::Base_msg msg;
    msg.ParseFromString(p_msg->get_message_buf());

    switch (msg.base_info().msg_type())
    {
        case message::ePointCloud_msg:
        {
            message::PointCloud_msg cloud_msg;
            if(cloud_msg.ParseFromString(p_msg->get_message_buf()))
            {
                sensor_msgs::PointCloud cloud;
                cloud.header.stamp = ros::Time::now();
                cloud.header.frame_id = cloud_msg.frame_id();
                cloud.points.resize(cloud_msg.points().size());
                //we'll also add an intensity channel to the cloud
                cloud.channels.resize(1);
                cloud.channels[0].name = "xyz";
                cloud.channels[0].values.resize(cloud_msg.points().size());

                //generate some fake data for our point cloud
                for (unsigned int i = 0; i < cloud_msg.points().size(); ++i)
                {
                    cloud.points[i].x = cloud_msg.points(i).x();
                    cloud.points[i].y = cloud_msg.points(i).y();
                    cloud.points[i].z = cloud_msg.points(i).z();
                    cloud.channels[0].values[i] = cloud_msg.points(i).intensity();
                }
                std::string topic=cloud_msg.topic();
                if(g_topic_map.find(topic)==g_topic_map.end())
                {
                    ros::Publisher* publisher=new ros::Publisher();
                    *publisher = gp_nh->advertise<sensor_msgs::PointCloud> (topic, 1);
                    g_topic_map[topic]=publisher;
                }
                g_topic_map[topic]->publish(cloud);
            }
            break;
        }
        case message::ePose2d_msg:
        {
            message::Pose2d_msg pose_msg;
            if(pose_msg.ParseFromString(p_msg->get_message_buf()))
            {
                geometry_msgs::PoseStamped pose_stamped;
                pose_stamped.header.stamp = ros::Time::now();
                pose_stamped.header.frame_id = pose_msg.frame_id();
                pose_stamped.pose.position.x = pose_msg.x();
                pose_stamped.pose.position.y = pose_msg.y();
                pose_stamped.pose.position.z = 0;
                pose_stamped.pose.orientation.x = 0;
                pose_stamped.pose.orientation.y = 0;
                pose_stamped.pose.orientation.z = sin(pose_msg.theta() / 2.0);
                pose_stamped.pose.orientation.w = cos(pose_msg.theta() / 2.0);

                std::string topic=pose_msg.topic();
                if(g_topic_map.find(topic)==g_topic_map.end())
                {
                    ros::Publisher* publisher=new ros::Publisher();
                    *publisher = gp_nh->advertise<geometry_msgs::PoseStamped> (topic, 1);
                    g_topic_map[topic]=publisher;
                }
                g_topic_map[topic]->publish(pose_stamped);

            }
            break;
        }

    }
    //ros::spinOnce();
    return SUCCESS;

}

void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

    double x=msg->pose.position.x;
    double y=msg->pose.position.y;
    double z=msg->pose.position.z;
    double qx=msg->pose.orientation.x;
    double qy=msg->pose.orientation.y;
    double qz=msg->pose.orientation.z;
    double qw=msg->pose.orientation.w;

    std::cout<<"  post goal "<<qx<<","<<qy<<","<<qz<<","<<qw<<std::endl;
    double theta=acos(qw)*2.0;
    theta=(qz>0)?theta:-theta;

    message::Goal_msg goal;
    message::Base_info base_info;
    base_info.set_sender(message::eEmpty);
    base_info.set_receiver(message::eAGV);
    base_info.set_msg_type(message::eGoal_msg);
    goal.mutable_base_info()->CopyFrom(base_info);

    goal.set_x(x);
    goal.set_y(y);
    goal.set_theta(theta);


    Communication_message message;
    message.reset(goal.base_info(),goal.SerializeAsString());
    Message_communicator::get_instance_pointer()->send_msg(&message);
    //发送目标点
}

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

	ros::init(argc, argv, "agv_rviz");
    ros::NodeHandle nh;
    gp_nh=&nh;
    subGoal = nh.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple

    Message_communicator::get_instance_pointer()->communication_connect("tcp://127.0.0.1:30001");
    Message_communicator::get_instance_pointer()->set_ros_data_callback(Ros_data_callback);
    Message_communicator::get_instance_pointer()->communication_run();


    pcl::PointCloud<pcl::PointXYZ>    cloud;
    for(int i=0;i<100;++i)
    {
        pcl::PointXYZ point(0.02*i,0,0);
        cloud.push_back(point);
    }

    ros::spin();


return 0;
}



GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
    time_t tt;
    time( &tt );
    tt = tt + 8*3600;  // transform the time zone
    tm* t= gmtime( &tt );
    char buf[255]={0};
    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
            t->tm_year + 1900,
            t->tm_mon + 1,
            t->tm_mday,
            t->tm_hour,
            t->tm_min,
            t->tm_sec);

    FILE* tp_file=fopen(buf,"w");
    fprintf(tp_file,data,strlen(data));
    fclose(tp_file);

}
void init_glog()
{
    time_t tt = time(0);//时间cuo
    struct tm* t = localtime(&tt);

    char strYear[255]={0};
    char strMonth[255]={0};
    char strDay[255]={0};

    sprintf(strYear,"%04d", t->tm_year+1900);
    sprintf(strMonth,"%02d", t->tm_mon+1);
    sprintf(strDay,"%02d", t->tm_mday);

    char buf[255]={0};
    getcwd(buf,255);
    char strdir[255]={0};
    sprintf(strdir,"%s/log/%s/%s/%s", buf,strYear,strMonth,strDay);
    PathCreator creator;
    creator.Mkdir(strdir);

    char logPath[255] = { 0 };
    sprintf(logPath, "%s/", strdir);
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("node");
    google::SetStderrLogging(google::INFO);
    google::SetLogDestination(0, logPath);
    google::SetLogFilenameExtension("zxlog");
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&shut_down_logging);
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}
