#include "PlcAgvRobot.h"

PlcAgvRobot::PlcAgvRobot()
{
    laser_base_link_.setOrigin(tf::Vector3(-0.38,-0.095,0.0));
    laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));

    ws_down.left_linear_speed=0;
    ws_down.right_linear_speed=0;
    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);
    if(!result)
    {
        std::cout<<" plc connect failed..."<<std::endl;
        exit(0);
    }
    exit_ = !result;
    bConnect_=true;
}

// PlcAgvRobot::PlcAgvRobot()
// {
// }

PlcAgvRobot::~PlcAgvRobot()
{
    plc_.disconnect();
}

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 == "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 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;
    return true;
}

bool PlcAgvRobot::post_tf()
{
    static tf::TransformBroadcaster br;
    br.sendTransform(tf::StampedTransform(laser_base_link_, ros::Time::now(), "laser", "base_link"));
    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(), "base_link", "odom"));
}

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

bool PlcAgvRobot::readSpeed()
{
    bool result;
    if (exit_)
    {
        return false;
    }
    else
    {
        result = true;
    }
    uint16_t arr[2]={0};
    result &= plc_.read_registers(ADDRESS_LEFT_LINEAR_CURRENT,2, arr);
    //result &= plc_.read_register(ADDRESS_RIGHT_LINEAR_CURRENT, arr+1);
    

    ws_load.right_linear_speed = arr[0];
    if(arr[0]>9000)
         ws_load.right_linear_speed=-double(arr[0]-10000);
    ws_load.left_linear_speed = arr[1];
    if(arr[1]>9000)
         ws_load.left_linear_speed=-double(arr[1]-10000);
    if(fabs(ws_load.left_linear_speed)>1000 || fabs(ws_load.right_linear_speed)>1000)
    {
       // std::cout<<" speed > 1000"<<std::endl;
        return false;
    }
    
    return result;
}
bool PlcAgvRobot::writeSpeed(const double &linear_speed, const double &angular_speed)
{
    if(!point2Wheels(linear_speed, angular_speed)) return false;
    bool result;
    if(exit_){
        return false;
    }else{
        result = true;
    }
   
    uint16_t arr[2]={0};
    if(ws_down.left_linear_speed>0)
    {
        arr[1]=uint16_t(ws_down.left_linear_speed*1000.0);

    }
    else
    {
        arr[1]=uint16_t(-ws_down.left_linear_speed*1000.0+10000);
    }
    

    if(ws_down.right_linear_speed>0)
    {
        arr[0]=uint16_t(ws_down.right_linear_speed*1000.0);
    }
    else
    {
        arr[0]=uint16_t(-ws_down.right_linear_speed*1000.0+10000);
    }
    
    
    result &= plc_.write_register(ADDRESS_LEFT_LINEAR_TARGET, arr);
    result &= plc_.write_register(ADDRESS_RIGHT_LINEAR_TARGET, arr+1);
    std::cout<<" down v1="<<arr[0]<<",["<<ws_down.left_linear_speed<<"], v2="<<arr[1]
        <<",["<<ws_down.right_linear_speed<<"]"<<std::endl;
    return result;
}

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)<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 = fabs(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 L1=0.475;
    double L2=0.475;
    double delta_v=v_l-v_r;
    if(fabs(delta_v)<1e-3)
    {
        linear_speed=(v_l+v_r)/2.0;
        angular_speed=0;
    }
    else
    {
        double w=delta_v/(L1+L2);
        double linear_speed=(v_l-w*L1)/2.0;
        angular_speed=w;
        
    }
    if(fabs(angular_speed)>0.1)
    {
        angular_speed=angular_speed>0?0.1:-0.1;
    }
    /*std::cout<<"v_l:"<<v_l<<"\t v_r:"<<v_r<<"\tv:"<<linear_speed
                        <<"\t \t v_a:"<<angular_speed<<std::endl;*/
    return true;
}