#include "../include/Air_Ground_CEC/qnode.hpp"

#include "sensor_msgs/image_encodings.h"
#include <QtConcurrent/QtConcurrent>

QNode::QNode(int argc, char **argv) : init_argc(argc), init_argv(argv){


}


QNode::~QNode() {
  if (ros::isStarted()) {
    ros::shutdown();  // explicitly needed since we use ros::start();
    ros::waitForShutdown();
  }
  wait();
}

bool QNode::init() {
    QSettings topic_setting("ros_controller", "settings");
    odom_topic = topic_setting.value("topic/topic_odom", "odom").toString();
    batteryState_topic =
          topic_setting.value("topic/topic_power", "battery_state").toString();
    qRegisterMetaType<sensor_msgs::BatteryState>("sensor_msgs::BatteryState");
    ros::init(init_argc,init_argv,"Air_Ground_CEC",
            ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
    QtConcurrent::run([=](){
        SubAndPubTopic();
    });


    return true;
}

void QNode::SubAndPubTopic(){
    ros::NodeHandle n;

    //创建速度话题的订阅者
    cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 10,
                                                 &QNode::speedCallback, this);

    //power_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
      //                          &QNode::powerCallback, this);
    cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    battery_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
                              &QNode::batteryCallback, this);

    image_transport::ImageTransport it(n);
    image_sub = it.subscribe("camera/rgb/image_raw", 100, &QNode::imageCallback, this);

    //image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this)

    //chatter_publisher = n.advertise<std_msgs::String>("chatter1", 1000);
    //chatter_subscriber = n.subscribe("chatter",1000,&QNode::myCallback,this);
    ros::spin();
}

void QNode::batteryCallback(const sensor_msgs::BatteryState& message) {
  emit batteryState(message);
}

void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
    std::map<char, std::vector<float>> moveBindings {
        // {'key', {x, y, z, th}}
        {'7', {1, 1, 0, 1}},  {'8', {1, 0, 0, 0}},  {'9', {1, -1, 0, -1}},
        {'4', {0, 0, 0, 1}}, {'5', {0, 0, 0, 0}},   {'6', {0, 0, 0, -1}},
        {'1', {-1, 1, 0, -1}}, {'2', {-1, 0, 0, 0}}, {'3', {-1, -1, 0, 1}}
    };

    float x = moveBindings[key][0];
    float y = moveBindings[key][1];
    float z = moveBindings[key][2];
    float th = moveBindings[key][3];

    //计算线速度和角速度
    float speed = speed_linear;
    float turn = speed_trun;



    geometry_msgs::Twist twist;
    twist.linear.x = x * speed;
    twist.linear.y = y * speed;
    twist.linear.z = z * speed;

    twist.angular.x = 0;
    twist.angular.y = 0;
    twist.angular.z = th * turn;

    cmd_pub.publish(twist);

    //ros::spinOnce();
}

void QNode::AutoMove(float speed_linear, float speed_turn,
              float target_linear, float target_turn){

    // wrong command
    if (speed_linear==0 && speed_turn ==0){
        std::cout<<"wrong command"<<endl;
        return;
    }


    //How fast will we update the robot's movement?
    double rate = 1;
    //Set the equivalent ROS rate variable
    ros::Rate loopRate(rate);

    std::map<char, std::vector<float>> Directions {
        {'w',{1, 0, 0, 0}}, {'x',{-1, 0, 0, 0}}, {'s',{0, 0, 0, 0}},
        {'a',{0, 0, 0, 1}}, {'d',{0, 0, 0, -1}}
    };
    char key = 's';
    if(target_linear < 0){
        key = 'x';
    }
    else if (target_linear > 0){
        key = 'w';
    }
    else if (target_turn > 0){
        key = 'a';
    }
    else if (target_turn < 0){
        key = 'd';
    }

    float x = Directions[key][0];
    float y = Directions[key][1];
    float z = Directions[key][2];
    float th = Directions[key][3];



    float total_times = 0;
    if (target_linear == 0){
        total_times = fabs(target_turn)/speed_turn;
    }
    else{
        total_times = fabs(target_linear)/speed_linear;
    }

    std::cout<< total_times<<std::endl;

    int ticks;
    geometry_msgs::Twist twist;


    twist.linear.x = speed_linear * x;
    twist.linear.y = speed_linear * y;
    twist.linear.z = speed_linear * z;

    twist.angular.x = 0;
    twist.angular.y = 0;
    twist.angular.z = speed_turn * th;

    ticks = int(total_times);

    for(int i = 0; i < ticks; i++)
    {
      cmd_pub.publish(twist); // 将刚才设置的指令发送给机器人
      loopRate.sleep();
    }

    ros::spinOnce();
}


//速度回调函数
void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    emit speed_x(msg->twist.twist.linear.x);
    emit speed_th(msg->twist.twist.angular.z);

}


void QNode::imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    try
      {
        cv_bridge::CvImageConstPtr cv_ptr =
            cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
        cv::Mat img = cv_ptr->image;
        QImage image = QImage(img.data,img.cols,img.rows,img.step[0],QImage::Format_RGB888);//change  to QImage format
        //ROS_INFO("I'm setting picture in mul_t callback function!");
        //image.save("/home/jackyma/test.jpg");
        emit show_image(image);
      }
    catch (cv_bridge::Exception& e)
      {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
      }

}


QImage QNode::Mat2QImage(cv::Mat const& src) {
  QImage dest(src.cols, src.rows, QImage::Format_ARGB32);

  const float scale = 255.0;

  if (src.depth() == CV_8U) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = src.at<quint8>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  } else if (src.depth() == CV_32F) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = scale * src.at<float>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  }

  return dest;
}