#include <sys/types.h>
#include <winsock.h>
#include <windows.h>
#include "xarm/wrapper/xarm_api.h"


int connect_remote_server(const char* server_ip, int port_num)

{
    struct sockaddr_in server_addr;
    int client_sockfd = socket(AF_INET, SOCK_STREAM, 0);
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = inet_addr(server_ip);
    server_addr.sin_port = htons(port_num);
    if (connect(client_sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr)))
    {
        fprintf(stderr, "Error connecting remote server!\n");
        //close(client_sockfd);
        return -1;
    }
    return client_sockfd;
}


// Run Example: $ ./xArm_report_30003 192.168.1.227 1
// Run Example: $ ./exe 192.168.1.227 0

// Use "Ctrl-C" to terminate

int main(int argc, char** argv)

{
    // PAY ATTENTION to this notice
    fprintf(stderr, "\nMake sure robot is already ENABLED! Firmware version >= 1.8.0!\n\n");
    if (argc != 3)
    {
        fprintf(stderr, "[Usage: ] argument 1: robot ip address (e.g. 192.168.1.227), argument 2: 0 for joint angle report, 1 for actual motor current report\n");
        return -1;

    }

    // connect to xArm control port
    XArmAPI* arm = new XArmAPI(argv[1]);
    sleep_milliseconds(500);
    int ret = 0;
    int ret2 = 0;
    std::string report_str;
    if (arm->is_connected()) {
        switch (atoi(argv[2]))
        {
        case 0:
            ret2 = 0;
            report_str = "feedback joint angle:\n";
            break;

        case 1:
            ret = arm->set_report_tau_or_i(1);
            ret2 = 1;
            report_str = "feedback motor currents:\n";
            break;

        default:
            fprintf(stderr, "[Error: ] Invalid argument 2, 0 for joint angle, 1 for actual motor current report\n");
            arm->disconnect();
            return -1;
        }
        arm->disconnect();
        unsigned char recv_chars[128] = { 0 };
        char* p = (char*)recv_chars;
        float data[7] = { 0 };
        int client_sock = connect_remote_server(argv[1], 30003);
        if (client_sock != -1)
        {
            while (true)
            {
                if (ret2 == 0) {
                    ret = recv(client_sock, p, 35, 0);
                    if (ret < 0)
                        break;
                    hex_to_nfp32(&recv_chars[7], data, 7);
                    fprintf(stderr, "%s", report_str.c_str());
                    for (int i = 0; i < 7; i++)
                        fprintf(stderr, "%f\t", data[i]);
                    fprintf(stderr, "\n");
                    sleep_milliseconds(2);
                }
                if (ret2 == 1) {
                    ret = recv(client_sock, p, 87, 0);
                    if (ret < 0)
                        break;
                    hex_to_nfp32(&recv_chars[59], data, 7);
                    fprintf(stderr, "%s", report_str.c_str());
                    for (int i = 0; i < 7; i++)
                        fprintf(stderr, "%f\t", data[i]);
                    fprintf(stderr, "\n");
                    sleep_milliseconds(2);
                }
            }
        }

        else
        {
            fprintf(stderr, "Please check the robot connection!\n");
            return -1;
        }
    }

    return 0;

}