#include <ros/ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "geometry_msgs/Twist.h"

using namespace std;


class TCPPublisher
{
    public:
        TCPPublisher();
        ~TCPPublisher();
        void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);

    private:
        ros::NodeHandle n, nPrivate;
        ros::Publisher tcpPub;
        ros::Subscriber cmdVelSub;
        // 創(chuàng)建服務(wù)器套接字
        int serverSocket;
        // 設(shè)置服務(wù)器地址結(jié)構(gòu)
        sockaddr_in serverAddr;
        int clientSocket;
        std::string topicStatus;
};

#include "./tcp_pub/tcp_pub.h"


TCPPublisher::TCPPublisher():nPrivate("~")
{
    nPrivate.param("topicStatus", topicStatus, std::string("/cmd_vel"));
    /*訂閱話題*/
    cmdVelSub = n.subscribe(topicStatus.c_str(), 10, &TCPPublisher::cmdVelCallback, this);

    serverSocket = socket(AF_INET, SOCK_STREAM, 0);
    // 設(shè)置服務(wù)器地址結(jié)構(gòu)
    sockaddr_in serverAddr;
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = INADDR_ANY;
    serverAddr.sin_port = htons(8080); // 服務(wù)器監(jiān)聽的端口號

    // 綁定套接字
    if (bind(serverSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {
        std::cerr << "Bind failed." << std::endl;
        close(serverSocket);
        return;
    }

    // 監(jiān)聽連接
    if (listen(serverSocket, SOMAXCONN) == -1) {
        std::cerr << "Listen failed." << std::endl;
        close(serverSocket);
        return;
    }

    std::cout << "Server is listening for incoming connections..." << std::endl;

    ROS_INFO("TCPPublisher init successfully!!!");
}

TCPPublisher::~TCPPublisher()
{
    close(serverSocket);
}


void TCPPublisher::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    float velX = msg->linear.x;
    float angularZ = msg->angular.z;
    // ROS_INFO("velX : %f, angularZ : %f", velX, angularZ);

    // 定義字符數(shù)組，用于存儲轉(zhuǎn)換后的結(jié)果
    char buffer[50]; // 適當調(diào)整數(shù)組大小以適應(yīng)你的需求
    snprintf(buffer, sizeof(buffer), "%f,%f", velX, angularZ);
    // ROS_INFO("buffer %s", buffer);

    // 接受連接
    int clientSocket = accept(serverSocket, NULL, NULL);
    if (clientSocket == -1) {
        std::cerr << "Accept failed." << std::endl;
        close(serverSocket);
        return;
    }

    std::cout << "Connection established with a client." << std::endl;

    // 發(fā)送消息給客戶端
    const char* message = buffer;
    ROS_INFO("message %s", message);

    if (send(clientSocket, message, strlen(message), 0) == -1) {
        std::cerr << "Error sending message." << std::endl;
    }

    // 關(guān)閉客戶端套接字
    close(clientSocket);
}

int main(int argc, char **argv) {
  //創(chuàng)建節(jié)點
  ros::init(argc, argv, "pure_pursuit");
  TCPPublisher tp;  
  ros::spin();
  return 0;
}

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("tcp_client");
    /*define publisher*/
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
   // Advertise velocity commands
    auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
    cmd_pub_ = node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", default_qos);


    //連接到服務(wù)器
    // if (connect(clientSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {
    //     std::cerr << "Connection failed." << std::endl;
    //     close(clientSocket);
    //     return 1;
    // }

    // std::cout << "Connected to the server." << std::endl;

    while (true) {

        // 創(chuàng)建客戶端套接字
        int clientSocket = socket(AF_INET, SOCK_STREAM, 0);
        if (clientSocket == -1) {
            std::cerr << "Failed to create client socket." << std::endl;
            return 1;
        }

        // 設(shè)置服務(wù)器地址結(jié)構(gòu)
        sockaddr_in serverAddr;
        serverAddr.sin_family = AF_INET;
        serverAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); // 本地回環(huán)地址 // 服務(wù)器的 IP 地址
        serverAddr.sin_port = htons(8080); // 服務(wù)器監(jiān)聽的端口號

        // 連接到服務(wù)器
        if (connect(clientSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {
            std::cerr << "Connection failed." << std::endl;
            close(clientSocket);
            // return 1;
        }

        // std::cout << "Connected to the server." << std::endl;
        // 接收消息
        char buffer[50];
        memset(buffer, 0, sizeof(buffer));
        if (recv(clientSocket, buffer, sizeof(buffer), 0) == -1) {
            std::cerr << "Error receiving message." << std::endl;
        } else {
            std::cout << "Received message from server: " << buffer << std::endl;

            // 定義兩個變量來存儲解析后的浮點數(shù)
            float floatValue1, floatValue2;

            // 使用 sscanf 解析字符數(shù)組
            if (std::sscanf(buffer, "%f,%f", &floatValue1, &floatValue2) == 2) {
                // 打印解析結(jié)果
                std::cout << "解析后的浮點數(shù)1: " << floatValue1 << std::endl;
                std::cout << "解析后的浮點數(shù)2: " << floatValue2 << std::endl;
            } else {
                // 解析失敗
                std::cerr << "解析失敗" << std::endl;
            }
         
            geometry_msgs::msg::Twist cmd_msg;
            cmd_msg.linear.x = floatValue1;
            cmd_msg.angular.z = floatValue2;
            cmd_pub_->publish(cmd_msg);
            std::cout << "Publishing zero speed to /cmd_vel. " << std::endl;
        }

        // 關(guān)閉客戶端套接字
        close(clientSocket);
        // 在這里可以添加一些延時，以避免過于頻繁地連接服務(wù)器
        sleep(0.1);
    }



    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}