/*
 * @Description: 里程计节点，发布odom数据，接收cmd_vel话题下发plc
 * @Author: yct
 * @Date: 2020-06-11 17:27:41
 * @LastEditTime: 2020-06-11 21:42:54
 * @LastEditors: yct
 */
#include <iostream>
#include <chrono>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>

#include "odom/Odom.h"
using namespace agv;

ros::Publisher g_odom_pub;
// 两主动轮距离
double g_wheelbase = 0.055;

double deg2rad(double deg)
{
  return deg / 180.0 * M_PI;
}

double rad2deg(double rad)
{
  return rad / M_PI * 180.0;
}

class Odom_test
{

public:
  Odom_test()
  {
	if(mp_odom == nullptr)
    		mp_odom = new Odom();
  }
  ~Odom_test()
  {
    if (mp_odom != nullptr)
    {
      delete mp_odom;
      mp_odom = nullptr;
    }
  }
  bool init()
  {
    if (mp_odom != nullptr)
    {
      mp_odom->set_driver_data_callback(get_data_callback, this);
      Error_manager ec = mp_odom->init();
      if (ec == SUCCESS)
        return true;
      std::cout << "odom init: " << ec.to_string() << std::endl;
    }
    else
    {
      std::cout << "NULL POINTER" << std::endl;
    }
    return false;
  }

  // 待下发cmd_vel消息回调，将其转换为两轮速度下发plc
  static void CmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg)
  {
    if (mp_odom != nullptr)
    {
      Eigen::Vector2d speed;
      Eigen::Vector2d wheel;
      speed << msg->linear.x, msg->angular.z;
      speed2wheel(speed, wheel);
      Driver_data data;
      data.front_motor_data[0] = wheel[0];
      data.front_motor_data[1] = wheel[1];
      Error_manager ec = mp_odom->write_driver_data(data);
      if (ec != SUCCESS)
        std::cout << "write error: " << ec.to_string() << std::endl;
      else{
		char buf[255];
	    memset(buf, 0, 255);
	    sprintf(buf + strlen(buf), "write   \tfront: %2.3f, %2.3f ", data.front_motor_data[0], data.front_motor_data[1]);
	    // sprintf(buf+strlen(buf), "rear: %2.3f, %2.3f", data.rear_motor_data[0], data.rear_motor_data[1]);
	    std::cout << buf << std::endl;
      }
    }
    else
    {
      std::cout << "NULL POINTER" << std::endl;
    }
  }

  // 线角速度转两轮速度
  static void speed2wheel(Eigen::Vector2d vvth, Eigen::Vector2d &v1v2)
  {
    v1v2[0] = vvth[0] - vvth[1] * g_wheelbase / 2.0;
    v1v2[1] = vvth[0] + vvth[1] * g_wheelbase / 2.0;
  }

  // 两轮速度转线角速度
  static void wheel2speed(Eigen::Vector2d v1v2, Eigen::Vector2d &vvth)
  {
    vvth[0] = v1v2.sum() / 2.0;
    vvth[1] = (v1v2[1] - v1v2[0]) / g_wheelbase;
  }

  static void get_data_callback(Driver_data &data, void *pOwner)
  {
    static int read_callback_count = 0;
    read_callback_count++;
    char buf[255];
    memset(buf, 0, 255);
    sprintf(buf + strlen(buf), "index: %d \t", read_callback_count);
    sprintf(buf + strlen(buf), "front: %2.3f, %2.3f \t", data.front_motor_data[0], data.front_motor_data[1]);
    // sprintf(buf+strlen(buf), "rear: %2.3f, %2.3f", data.rear_motor_data[0], data.rear_motor_data[1]);
    std::cout << buf << std::endl;

    Eigen::Vector2d wheel, speed;
    wheel << data.front_motor_data[0], data.front_motor_data[1];
    speed2wheel(wheel, speed);

    nav_msgs::Odometry t_odom;
    t_odom.child_frame_id = "base_link";
    t_odom.header.frame_id = "map";
    t_odom.header.stamp = ros::Time::now();
    t_odom.header.seq = read_callback_count;
    t_odom.twist.twist.linear.x = speed[0];
    t_odom.twist.twist.angular.z = speed[1];
    g_odom_pub.publish(t_odom);
  }

  static Odom *mp_odom;
};
Odom *Odom_test::mp_odom = nullptr;
//主函数
int main(int argc, char **argv)
{
  ros::init(argc, argv, "odom_node");

  ros::NodeHandle gn;

  Odom_test ot;
  ot.init();

  ros::Subscriber vel_sub_ = gn.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Odom_test::CmdVelCallback); //move_base_simple

  g_odom_pub = gn.advertise<nav_msgs::Odometry>("odom", 1, true);

  ros::spin();

  return 0;
}
