#include "PlcAgvRobot.h"

PlcAgvRobot::PlcAgvRobot()
{

    //laser_base_link_.setOrigin(tf::Vector3(0.365,0.28,0.0));   //urg 20m 
    laser_base_link_.setOrigin(tf::Vector3(0.29,-0.415,0.0));     //R2000
    laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));


    tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1, boost::bind(&PlcAgvRobot::location_tf_callback, _1, this));
    //bool result = plc_.connect("192.168.0.30", 502);

    bConnect_=true;

}

PlcAgvRobot::~PlcAgvRobot()
{

}

void PlcAgvRobot::location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, PlcAgvRobot *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 == "base_link")
        {
            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 PlcAgvRobot::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<<" read   v= "<<current_linear_speed_<<"  , a= "<<current_angular_speed_<<std::endl;
    return true;
}

bool PlcAgvRobot::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(laser_base_link_, ros::Time::now(), "odom", "base_link"));

    tf.setOrigin(tf::Vector3(-0.62,0.4975,0));
    tf.setRotation(tf::Quaternion(0,0,0,1));
    br.sendTransform(tf::StampedTransform(tf, ros::Time::now(), "base_link", "laser"));

    
}

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

bool PlcAgvRobot::readSpeed()
{
    if(bConnect_==false) return false;

    ws_load.right_linear_speed=m_card.ReadData(1000);
    ws_load.left_linear_speed = m_card.ReadData(1001);
    
    return true;
}
bool PlcAgvRobot::writeSpeed(const double &linear_speed, const double &angular_speed)
{
    if(!point2Wheels(linear_speed, angular_speed)) return false;
    m_card.WriteData(1002,ws_down.left_linear_speed);
    m_card.WriteData(1003,ws_down.right_linear_speed);
    return true;

}

bool PlcAgvRobot::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;

    /*if(fabs(w)>0.02&& fabs(v)<1e-4)
    {
        w=w>0?0.02:-0.02;
    }*/
    double width_robot=0.475*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;
    }

    /*if(fabs(w)<1e-2)
    {
        ws_down.left_linear_speed = v;
        ws_down.right_linear_speed = v;
        return true;
    }
    double L1=0.475;
    double L2=0.475;
    double R = v/w;

    ws_down.left_linear_speed = w*(R+L1);
    ws_down.right_linear_speed =w*(R-L2);*/
    //std::cout<<"v:"<<v<<",w:"<<w<<", R:"<<R<<" down v1="<<ws_down.left_linear_speed<<", v2="<<ws_down.right_linear_speed<<std::endl;
    
    return true;
}
bool PlcAgvRobot::wheelsToPoint(double &linear_speed, double &angular_speed)
{
    double v_l=double(ws_load.left_linear_speed)/1000.0;
    double v_r=double(ws_load.right_linear_speed)/1000.0;
    double L=0.475*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;
}