/////////////////////////////////////////////////

#include "gl_main.h"
#include "read_file.h"
#include "opencv_inc.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

#include <string>
#include <limits>
using namespace std;
#include <sys/time.h>
#include <unistd.h>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Float32MultiArray.h>

#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

#include <iostream>

ros::Time lrf_scantime;
ros::Time pantilt_time;

//Robot制御のパラメータ
extern struct ROBOT myrobot;
extern struct URG urg_data;
extern struct MOVE_OBSTACLE move_obst;


extern struct ROBOT myrobot;
extern struct URG urg_data;
extern struct URG urg_data2;
extern struct URG urg_data3;
extern struct URG urg_area;
extern struct pantilt pantable;
struct pantilt pantable_in;

//extern struct PointCloud pt_xyz;
extern pcl::PointCloud<pcl::PointXYZ> pt_xyz_out;

extern struct MOVE_OBSTACLE move_obst;
extern IplImage *camera_color;
extern IplImage *camera_depth;
extern IplImage *capture_video;

double top_urg_data_num = 1080;
double top_urg_deg = 270.0;
double top_urg_reso = 1440.0;
double top_urg_len_max = 30.0;
double top_urg_len_min = 0.2;

double classic_urg_data_num = 768;
double classic_urg_deg = 240.0;
double classic_urg_reso = 768.0;
double classic_urg_len_max = 4.0;
double classic_urg_len_min = 0.2;

double pantilt_pan_rot_speed = 1.0;
double pantilt_pan_init_deg = 1.0;
double pantilt_tilt_rot_sin_amp = 1.0;
double pantilt_tilt_rot_sin_speed = 1.0;
double pantilt_tilt_init_deg = 1.0;
extern double robot_z;

double move_obst_num = 0;
int odometory_tf_out_error = 0;

int load_config(string fname);

extern IplImage *video;
extern IplImage *depth;

double obstacle_z = 0;
void   cv_bridge_init();

int view_sim_cap_flg;
int view_camera_pcl_flg;
int view_camera_rgb_flg;
int view_camera_depth_flg;

double sim_fps=10.0;

void vel_Callback(const geometry_msgs::Twist &vel_msg)
{
    myrobot.v = vel_msg.linear.x;
    myrobot.w = -vel_msg.angular.z;
}
//時間を計測する
static double get_time2(void)
{
    double freq = 1000.0 / cv::getTickFrequency();
    static int64 old_time = cv::getTickCount();
    int64 time = cv::getTickCount();
    double dt_msec = (time - old_time) * freq;
    old_time = time;
    return dt_msec;
}

void move_obst_Callback(const geometry_msgs::PoseArray &msg)
{
    double dt = get_time2() / 1000.0;

    for (int i = 0; i < 5; i++)
    {
        move_obst.x[i] += msg.poses[i].position.x * dt;
        move_obst.y[i] += msg.poses[i].position.y * dt;
    }
}



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

    string fname = "setting.ini";
    string exe_name = argv[0];
    load_config(exe_name + "_" + fname);

    cout << "start" << endl;
    Graphics GL;
    //sleep(1);

    urg_data.data_num = top_urg_data_num;
    urg_data.start_rad = -top_urg_deg / 2.0 / 360.0 * 2 * M_PI;
    urg_data.reso = top_urg_deg / top_urg_data_num / 360.0 * 2 * M_PI;
    urg_data.leng_max = top_urg_len_max;
    urg_data.leng_min = top_urg_len_min;

    urg_data2.data_num = classic_urg_data_num;
    urg_data2.start_rad = -classic_urg_deg / 2.0 / 360.0 * 2 * M_PI;
    urg_data2.reso = classic_urg_deg / classic_urg_data_num / 360.0 * 2 * M_PI;
    urg_data2.leng_max = classic_urg_len_max;
    urg_data2.leng_min = classic_urg_len_min;

    urg_data3.data_num = classic_urg_data_num;
    urg_data3.start_rad = -classic_urg_deg / 2.0 / 360.0 * 2 * M_PI;
    urg_data3.reso = classic_urg_deg / classic_urg_data_num / 360.0 * 2 * M_PI;
    urg_data3.leng_max = classic_urg_len_max;
    urg_data3.leng_min = classic_urg_len_min;


    urg_area.data_num = classic_urg_data_num;
    urg_area.start_rad = urg_data2.start_rad;
    urg_area.reso = urg_data2.reso;
    urg_area.leng_max = classic_urg_len_max;
    urg_area.leng_min = classic_urg_len_min;

    move_obst.n = move_obst_num;
    //出力のフレームレート

    //    double urg1_rate = 0.005;
    //    double urg2_rate = 0.1;
    //    double odom_rate = 0.05;
    //    double camera_rate = 0.025;
    //    double pantilt_rate = 0.10;

    TWIST twist_vel;

    ros::init(argc, argv, "docu_lrf_simulator");
    ros::NodeHandle n;

    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom_real", 10);
    ros::Publisher odom_pub2 = n.advertise<nav_msgs::Odometry>("odom", 10);
    ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 10);
    ros::Publisher scan_pub2 = n.advertise<sensor_msgs::LaserScan>("scan2", 10);
    ros::Publisher scan_pub3 = n.advertise<sensor_msgs::LaserScan>("scan3", 10);
    ros::Publisher panunit_pub = n.advertise<geometry_msgs::Pose>("/pantilt", 10);

    tf::TransformBroadcaster odom_broadcaster;
    tf::TransformBroadcaster broadcaster;
    cv_bridge_init();
    usleep(1000*1000);

    //画像をpublishする。
    ros::Publisher rgb_image_pub = n.advertise<sensor_msgs::Image>("docu_camera/rgb_image", 5);
    ros::Publisher depth_image_pub = n.advertise<sensor_msgs::Image>("docu_camera/depth_image", 5);
    ros::Publisher capture_image_pub = n.advertise<sensor_msgs::Image>("docu_camera/sim_capture_image", 5);
    ros::Publisher pcl_pub = n.advertise<sensor_msgs::PointCloud2> ("docu_camera/pcl", 1);




    const std::string vel_topic_ = "cmd_vel";
    ros::Subscriber cmd_vel = n.subscribe(vel_topic_, 10, vel_Callback);
    ros::Subscriber move_obst_cb = n.subscribe("move_obst", 10, move_obst_Callback);

    //scan
    unsigned int num_readings = urg_data.data_num;
    double laser_frequency = 50;
    double ranges[num_readings];
    double intensities[num_readings];

    unsigned int num_readings2 = urg_data2.data_num;
    double laser_frequency2 = 40;
    double ranges2[num_readings2];
    double intensities2[num_readings2];

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();
    ros::Time mid_time;

    lrf_scantime = ros::Time::now();
    pantilt_time = ros::Time::now();

    ros::Rate r(sim_fps);

    cout << "ROS start" << endl;

    while (n.ok())
    {
        ros::spinOnce();
        r.sleep();

        current_time = ros::Time::now();
        //compute odometry in a typical way given the velocities of the robot
        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        geometry_msgs::Quaternion odom_quat;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        if (odometory_tf_out_error == 0)
        {
            odom_trans.transform.translation.x = myrobot.y;
            odom_trans.transform.translation.y = -myrobot.x;
            odom_trans.transform.translation.z = 0.0;
            odom_quat = tf::createQuaternionMsgFromYaw(myrobot.theta);
            odom_trans.transform.rotation = odom_quat;
        }
        else
        {
            odom_trans.transform.translation.x = myrobot.y_dummy;
            odom_trans.transform.translation.y = -myrobot.x_dummy;
            odom_trans.transform.translation.z = 0.0;
            odom_quat = tf::createQuaternionMsgFromYaw(myrobot.theta_dummy);
            odom_trans.transform.rotation = odom_quat;
        }

        odom_broadcaster.sendTransform(odom_trans);

        nav_msgs::Odometry odom;

        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";
        odom.pose.pose.position.x = myrobot.y;
        odom.pose.pose.position.y = -myrobot.x;
        odom.pose.pose.position.z = 0.0;
        odom_quat = tf::createQuaternionMsgFromYaw(myrobot.theta);
        odom.pose.pose.orientation = odom_quat;

        odom.twist.twist.linear.x = myrobot.v;
        odom.twist.twist.linear.y = 0.0;
        odom.twist.twist.linear.z = 0.0;
        odom.twist.twist.angular.z = myrobot.w;
        odom_pub.publish(odom); //publish the message

        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";
        odom.pose.pose.position.x = myrobot.y_dummy;
        odom.pose.pose.position.y = -myrobot.x_dummy;
        odom.pose.pose.position.z = 0.0;
        odom_quat = tf::createQuaternionMsgFromYaw(myrobot.theta_dummy);
        odom.pose.pose.orientation = odom_quat;

        odom.twist.twist.linear.x = myrobot.v;
        odom.twist.twist.linear.y = 0.0;
        odom.twist.twist.linear.z = 0.0;
        odom.twist.twist.angular.z = myrobot.w;
        odom_pub2.publish(odom); //publish the message

        double roll_ = 0.0;
        double pitch_ = -pantable.tilt;
        double yaw_ = pantable.pan;
        //	tf::Quaternion quaternion = tf::createQuaternionFromRPY(yaw_, pitch_, roll_);
        tf::Quaternion quaternion;
        quaternion.setRPY(roll_, pitch_, yaw_);

        geometry_msgs::Quaternion quat_Msg;
        tf::quaternionTFToMsg(quaternion, quat_Msg); //この関数はROSのライブラリ
        quat_Msg = tf::createQuaternionMsgFromYaw(yaw_);

        geometry_msgs::Pose pose_panunit;
        pose_panunit.position.x = 0;
        pose_panunit.position.y = 0;
        pose_panunit.position.z = 0;
        pose_panunit.orientation = quat_Msg;
        panunit_pub.publish(pose_panunit);


        ////////////////////////////////////////////////////
        //populate the LaserScan message
        sensor_msgs::LaserScan scan3;
        scan3.header.stamp = current_time;
        scan3.header.frame_id = "base_scan3";
        scan3.angle_min = urg_data3.start_rad;
        scan3.angle_max = urg_data3.start_rad + urg_data3.reso * urg_data3.data_num;
        scan3.angle_increment = urg_data3.reso;
        scan3.time_increment = (1 / laser_frequency2) / (urg_data3.data_num);
        scan3.range_min = urg_data3.leng_min;
        scan3.range_max = urg_data3.leng_max;

        scan3.ranges.resize(num_readings2);
        scan3.intensities.resize(num_readings2);
        for (unsigned int i = 0; i < num_readings2; ++i)
        {

            if (urg_data2.leng[i] < 0.01)
            {
                scan3.ranges[i] = std::numeric_limits<float>::quiet_NaN();
            }
            else
            {
                scan3.ranges[i] = urg_data3.leng[i];
            }
            scan3.intensities[i] = 0.0;
        }

        scan_pub3.publish(scan3);

        ////////////////////////////////////////////////////

        ////////////////////////////////////////////////////
        //populate the LaserScan message
        sensor_msgs::LaserScan scan2;
        scan2.header.stamp = current_time;
        scan2.header.frame_id = "base_scan2";
        scan2.angle_min = urg_data2.start_rad;
        scan2.angle_max = urg_data2.start_rad + urg_data2.reso * urg_data2.data_num;
        scan2.angle_increment = urg_data2.reso;
        scan2.time_increment = (1 / laser_frequency2) / (urg_data2.data_num);
        scan2.range_min = urg_data2.leng_min;
        scan2.range_max = urg_data2.leng_max;

        scan2.ranges.resize(num_readings2);
        scan2.intensities.resize(num_readings2);
        for (unsigned int i = 0; i < num_readings2; ++i)
        {

            if (urg_data2.leng[i] < 0.01)
            {
                scan2.ranges[i] = std::numeric_limits<float>::quiet_NaN();
            }
            else
            {
                scan2.ranges[i] = urg_data2.leng[i];
            }
            scan2.intensities[i] = 0.0;
        }

        scan_pub2.publish(scan2);

        ////////////////////////////////////////////////////
        //populate the LaserScan message
        sensor_msgs::LaserScan scan;
        scan.header.stamp = lrf_scantime;
        scan.header.frame_id = "base_scan";
        scan.angle_min = urg_data.start_rad;
        scan.angle_max = urg_data.start_rad + urg_data.reso * urg_data.data_num;
        scan.angle_increment = urg_data.reso;
        //scan.time_increment = (1 / laser_frequency) / (urg_data.data_num);
        scan.time_increment = 0.00;

        scan.range_min = urg_data.leng_min;
        scan.range_max = urg_data.leng_max;

        scan.ranges.resize(num_readings);
        scan.intensities.resize(num_readings);
        for (unsigned int i = 0; i < num_readings; ++i)
        {

            if (urg_data.leng[i] < 0.01)
            {
                scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
            }
            else
            {
                scan.ranges[i] = urg_data.leng[i];
            }
            scan.intensities[i] = 0.0;
        }

        scan_pub.publish(scan);

        broadcaster.sendTransform(
            tf::StampedTransform(
                tf::Transform(
                    tf::Quaternion(0, 0, 0, 1),
                    tf::Vector3(0.0, 0.0, 0.01)),
                current_time, "base_footprint", "base_link"));

        broadcaster.sendTransform(
            tf::StampedTransform(
                tf::Transform(
                    quaternion,
                    tf::Vector3(0.0, 0.0, robot_z)),
                current_time, "base_link", "base_scan"));
        //tf

        broadcaster.sendTransform(
            tf::StampedTransform(
                tf::Transform(
                    tf::Quaternion(0, 0, 0, 1),
                    tf::Vector3(0.0, 0.0, 0.1)),
                current_time, "base_link", "base_scan2"));

        tf::Quaternion urg_back_qt;
        urg_back_qt.setRPY(0, 0, M_PI);

        broadcaster.sendTransform(
            tf::StampedTransform(
                tf::Transform(
                    urg_back_qt,
                    tf::Vector3(0.0, 0.0, 0.1)),
                current_time, "base_link", "base_scan3"));


        broadcaster.sendTransform(
            tf::StampedTransform(
                tf::Transform(
                     tf::Quaternion(0, 0, 0, 1),
                    tf::Vector3(0.0, 0.0, robot_z)),
                pantilt_time, "base_link", "camera_link"));

        
            if(view_camera_rgb_flg!=0){
            cv::Mat cvMat_rgb=video;
            sensor_msgs::ImagePtr rgb_image_msg;
            rgb_image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cvMat_rgb).toImageMsg();
            rgb_image_pub.publish(rgb_image_msg);
            }

            if(view_camera_depth_flg!=0){

            cv::Mat cvMat_depth=depth;
            sensor_msgs::ImagePtr depth_image_msg;
            depth_image_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", cvMat_depth).toImageMsg();

            depth_image_pub.publish(depth_image_msg);
            }

            if(view_camera_pcl_flg!=0){
            sensor_msgs::PointCloud2 pcl_output;
            pcl::toROSMsg(pt_xyz_out, pcl_output);
            pcl_output.header.frame_id = "camera_link";
            pcl_pub.publish(pcl_output);
            }


            if(view_sim_cap_flg!=0){
            cv::Mat Mat_capture=capture_video;
            sensor_msgs::ImagePtr capture_image_msg=cv_bridge::CvImage(std_msgs::Header(), "bgr8", capture_video).toImageMsg();
            capture_image_pub.publish(capture_image_msg);
            }



        last_time = current_time;
    }
    return 0;
}

int load_config(string fname)
{
    //extern unsigned int urg_data_num;
    //extern double urg_range_deg;
    //extern double urg_leng_max;
    //extern double urg_leng_min;
    //extern double noise_odometory_v_gain;
    //extern double noise_odometory_w_gain;
    //extern double noise_urg;
    cout << "fname:" << fname << endl;
    //string fname="setting.ini";
    std::ifstream ifs(fname.c_str());

    vector<string> lines;
    string line;

    while (getline(ifs, line))
        lines.push_back(line);
    ifs.close();

    string para;
    string findword;

    findword = "noise_odometory_v_gain=";
    string_Matching(lines, findword, para);
    noise_odometory_v_gain = StringToDouble(para);
    cout << findword << para << endl;

    findword = "noise_odometory_w_gain=";
    string_Matching(lines, findword, para);
    noise_odometory_w_gain = StringToDouble(para);
    cout << findword << para << endl;

    findword = "noise_urg=";
    string_Matching(lines, findword, para);
    noise_urg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "robot_z=";
    string_Matching(lines, findword, para);
    robot_z = StringToDouble(para);
    cout << findword << para << endl;

    findword = "top_urg_data_num=";
    string_Matching(lines, findword, para);
    top_urg_data_num = StringToDouble(para);
    cout << findword << para << endl;

    findword = "top_urg_deg=";
    string_Matching(lines, findword, para);
    top_urg_deg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "top_urg_reso=";
    string_Matching(lines, findword, para);
    top_urg_reso = StringToDouble(para);
    cout << findword << para << endl;

    findword = "top_urg_len_max=";
    string_Matching(lines, findword, para);
    top_urg_len_max = StringToDouble(para);
    cout << findword << para << endl;

    findword = "top_urg_len_min=";
    string_Matching(lines, findword, para);
    classic_urg_len_min = StringToDouble(para);
    cout << findword << para << endl;

    findword = "classic_urg_data_num=";
    string_Matching(lines, findword, para);
    classic_urg_data_num = StringToDouble(para);
    cout << findword << para << endl;

    findword = "classic_urg_deg=";
    string_Matching(lines, findword, para);
    classic_urg_deg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "classic_urg_reso=";
    string_Matching(lines, findword, para);
    classic_urg_reso = StringToDouble(para);
    cout << findword << para << endl;

    findword = "classic_urg_len_max=";
    string_Matching(lines, findword, para);
    classic_urg_len_max = StringToDouble(para);
    cout << findword << para << endl;

    findword = "classic_urg_len_min=";
    string_Matching(lines, findword, para);
    classic_urg_len_min = StringToDouble(para);
    cout << findword << para << endl;

    findword = "pantilt_pan_rot_speed=";
    string_Matching(lines, findword, para);
    pantilt_pan_rot_speed = StringToDouble(para);
    cout << findword << para << endl;

    findword = "pantilt_pan_init_deg=";
    string_Matching(lines, findword, para);
    pantilt_pan_init_deg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "pantilt_tilt_rot_sin_amp=";
    string_Matching(lines, findword, para);
    pantilt_tilt_rot_sin_amp = StringToDouble(para);
    cout << findword << para << endl;

    findword = "pantilt_tilt_rot_sin_speed=";
    string_Matching(lines, findword, para);
    pantilt_tilt_rot_sin_speed = StringToDouble(para);
    cout << findword << para << endl;

    findword = "pantilt_tilt_init_deg=";
    string_Matching(lines, findword, para);
    pantilt_tilt_init_deg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "odometory_tf_out_error=";
    string_Matching(lines, findword, para);
    odometory_tf_out_error = StringToDouble(para);
    cout << findword << para << endl;

    findword = "move_obst_num=";
    string_Matching(lines, findword, para);
    move_obst_num = StringToDouble(para);
    cout << findword << para << endl;

    findword = "obstacle_z=";
    string_Matching(lines, findword, para);
    obstacle_z = StringToDouble(para);
    cout << findword << para << endl;


    findword = "view_sim_cap_flg=";
    string_Matching(lines, findword, para);
    view_sim_cap_flg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "view_camera_pcl_flg=";
    string_Matching(lines, findword, para);
    view_camera_pcl_flg = StringToDouble(para);
    cout << findword << para << endl;
    
    findword = "view_camera_rgb_flg=";
    string_Matching(lines, findword, para);
    view_camera_rgb_flg = StringToDouble(para);
    cout << findword << para << endl;

    findword = "view_camera_depth_flg=";
    string_Matching(lines, findword, para);
    view_camera_depth_flg = StringToDouble(para);
    cout << findword << para << endl;
    
    
    findword = "sim_fps=";
    string_Matching(lines, findword, para);
    sim_fps = StringToDouble(para);
    cout << findword << para << endl;


    return 0;
}