#include "Agv_card.h"

Agv_card::Agv_card()
{
    //laser_base_link_.setOrigin(tf::Vector3(0.365,0.28,0.0));   //urg 20m
    bConnect_ = false;
    //雷达相对小车坐标系的 tf变换
    laser_base_link_.setOrigin(tf::Vector3(-1.65,0,0));     //R2000
    laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));


    tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1, boost::bind(&Agv_card::location_tf_callback, _1, this));

    // Error_manager ec = m_cifx.init();
    // printf(ec.to_string().append("\n").c_str());
    // printf("00000");
    Error_manager ec = Motor_manager::get_instance_references().motor_manager_init();
    if(ec != SUCCESS)
    {
        std::cout<<"motor init: "<<ec.to_string()<<std::endl;
    }else{
        ec = Motor_communication::get_instance_references().communication_init();
        if(ec != SUCCESS)
        {
            std::cout<<"can comm init: "<<ec.to_string()<<std::endl;
        }else{
            bConnect_=true;
        }
    }
    // printf("11111");
}

Agv_card::~Agv_card()
{
    // Motor_manager::get_instance_references().motor_driver_stop(1);
	// Motor_manager::get_instance_references().motor_driver_stop(2);
	Motor_communication::get_instance_references().communication_uninit();
	Motor_manager::get_instance_references().motor_manager_uninit();
}

void Agv_card::location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, Agv_card *par)
{
    //获取雷达定位tf
    
    for (size_t i = 0; i < msg->transforms.size(); i++)
    {
        geometry_msgs::TransformStamped temp = msg->transforms[i];
        if (temp.header.frame_id == "map" && temp.child_frame_id == "laser")
        {
            par->laser_pose_.setRotation(tf::Quaternion(temp.transform.rotation.x, temp.transform.rotation.y, temp.transform.rotation.z, temp.transform.rotation.w));
            par->laser_pose_.setOrigin(tf::Vector3(temp.transform.translation.x, temp.transform.translation.y, temp.transform.translation.z));
            
            break;
        }
    }
}

bool Agv_card::get_odom()
{
    if(bConnect_==false) return false;
    
    base_link_pose_=laser_pose_*laser_base_link_;
    if(!readSpeed())
        return false;
    if(!wheelsToPoint(current_linear_speed_, current_angular_speed_)) return false;
    std::cout<<"current v vth: \t\t\t\t "<<current_linear_speed_<<", "<<current_angular_speed_<<std::endl;
    return true;
}

bool Agv_card::post_tf()
{
    static tf::TransformBroadcaster br;
    tf::Transform tf;
    tf.setOrigin(tf::Vector3(0,0,0));
    tf.setRotation(tf::Quaternion(0,0,0,1));
    br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "map", "odom"));
    br.sendTransform(tf::StampedTransform(base_link_pose_, ros::Time::now(), "odom", "base_link"));

    // tf.setOrigin(tf::Vector3(8.65,1.54,0));
    // tf.setRotation(tf::Quaternion(0,0,0,1));
    // br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "map", "laser"));

    
}

bool Agv_card::down_speed()
{
    writeSpeed(cmd_vel_linear_, cmd_vel_angular_);
    return true;
}

bool Agv_card::readSpeed()
{
    if(bConnect_==false) return false;
    // if(!m_cifx.is_initialized()) return false;
    // /*
    //  * 读取两轮速度值
    //  */
    // ws_load.right_linear_speed = m_cifx.m_driver_data.front_motor_data[0];
    // ws_load.left_linear_speed = m_cifx.m_driver_data.front_motor_data[1];

    ws_load.right_linear_speed = Motor_manager::get_instance_references().get_actual_velocity(2);
    ws_load.left_linear_speed = Motor_manager::get_instance_references().get_actual_velocity(1);

    // printf("read data \t\t\t\t %.3f %.3f\n",ws_load.left_linear_speed, ws_load.right_linear_speed);
    return true;
}

bool Agv_card::writeSpeed(const double &linear_speed, const double &angular_speed)
{
    if(bConnect_==false) return false;
    if(!point2Wheels(linear_speed, angular_speed)) return false;
    /*
     * 下发两轮速度值
     */
    // if(m_cifx.is_initialized()){
    //     Driver_data t_data;
    //     t_data.front_motor_data[1] = ws_down.left_linear_speed;
    //     t_data.front_motor_data[0] = ws_down.right_linear_speed;
    //     Error_manager ec = m_cifx.write_driver_data(t_data);
    //     if(ec != SUCCESS)
    //         printf("write failed\n");
    //     else
    //     {
    //         printf("write data \t %.3f %.3f\n",linear_speed, angular_speed);
    //     }
    // }
    Motor_manager::get_instance_references().set_target_velocity(1, ws_down.left_linear_speed);
	Motor_manager::get_instance_references().set_target_velocity(2, ws_down.right_linear_speed);
    
    return true;

}

bool Agv_card::point2Wheels(const double linear_speed, const double angular_speed)
{
    //std::cout<<" recv v="<<linear_speed<<", a="<<angular_speed;
    double v=linear_speed;
    double w=angular_speed;

    //两轮间距
    double width_robot=0.275*2.0;
    if (fabs(v) < 1e-2)
    {
        ws_down.right_linear_speed = w * width_robot / 2.0;
        ws_down.left_linear_speed = (-1) * ws_down.right_linear_speed;
    }
    else if (fabs(w) < 1e-2)
    {
        ws_down.left_linear_speed = ws_down.right_linear_speed = v;
    }
    else
    {
        ws_down.left_linear_speed = v - w * width_robot / 2.0;
        ws_down.right_linear_speed = v + w * width_robot / 2.0;
    }
    
    return true;
}
bool Agv_card::wheelsToPoint(double &linear_speed, double &angular_speed)
{
    double v_l=double(ws_load.left_linear_speed);
    double v_r=double(ws_load.right_linear_speed);
    //两轮间距
    double L=0.275*2;
    double delta_v=v_r-v_l;
    if(fabs(delta_v)<1e-2)
    {
        linear_speed=(v_l+v_r)/2.0;
        angular_speed=0;
    }
    else
    {
        double w=delta_v/(L);
        linear_speed=(v_l+v_r)/2.0;;//(v_l-w*L1)/2.0;
        angular_speed=w;
        
    }

    /* std::cout<<"v_l:"<<v_l<<"\t v_r:"<<v_r<<"\tv:"<<linear_speed
                        <<"\t \t v_a:"<<angular_speed<<std::endl;*/
    return true;
}