
#include <iostream>
#include <thread>

#include "rokae/robot.h"

using namespace rokae;

int main() {
    std::cout << "Start running" << std::endl;
    try {
        std::string ip = "192.168.2.160";
        rokae::xMateErProRobot robot(ip);
        std::cout << "Establish a connection with Robot(192.168.2.160)" << std::endl;
        error_code ec;
        int timeout_ms = 50;
        std::vector<CANFrame> send_frames;

        while (true) {
            CANFrame can_data;
            can_data.frame_id = 0x27;
            can_data.frame_format = static_cast<int>(CANFormat::STANDARD);
            can_data.frame_type = static_cast<int>(CANType::CAN);

            //手指弯曲
            can_data.frame_valid_length = 7;
            can_data.data = { 0x01, 150, 150, 150, 150, 150, 150 };
            send_frames.push_back(can_data);

            can_data.data = { 0x01, 255, 255, 255, 255, 255, 255 };
            send_frames.push_back(can_data);

            /*can_data.frame_valid_length = 5;
            can_data.data = { 0x04, 0, 0, 0, 0 };
            send_frames.push_back(can_data);

            can_data.frame_valid_length = 5;
            can_data.data = { 0x04, 255, 255, 255, 255 };
            send_frames.push_back(can_data);*/

            auto start = std::chrono::steady_clock::now();
            robot.CANSendData(send_frames, ec);
            auto end = std::chrono::steady_clock::now();
            auto send_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
            std::cout << "CANSendData send time is: " << send_time_ms << "ms" << std::endl;
            std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;

            std::this_thread::sleep_for(std::chrono::seconds(2));


        }
        

        //while (true) {
        //    can_data.frame_valid_length = 6;
        //    can_data.data = { 0x05, 125, 125, 125, 125, 125 };
        //    robot.CANSendData(can_data, ec);
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));

        //    can_data.data = { 0x06, 125, 125, 125, 125, 125 };
        //    robot.CANSendData(can_data, ec);
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));

        //    //手指弯曲
        //    can_data.frame_valid_length = 7;
        //    can_data.data = { 0x01, 5, 6, 7, 8, 9, 10 };
        //    auto start = std::chrono::steady_clock::now();
        //    robot.CANSendData(can_data, ec);
        //    auto end = std::chrono::steady_clock::now();
        //    auto send_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        //    std::cout << "CANSendData 0x01 0 is: " << send_time_ms << "ms" << std::endl;
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));

        //    can_data.frame_valid_length = 7;
        //    can_data.data = { 0x01, 255, 255, 255, 255, 255, 255 };
        //    start = std::chrono::steady_clock::now();
        //    robot.CANSendData(can_data, ec);
        //    end = std::chrono::steady_clock::now();
        //    send_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        //    std::cout << "CANSendData 0x01 255 is: " << send_time_ms << "ms" << std::endl;
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));

        //    //手指并拢
        //    can_data.frame_valid_length = 5;
        //    can_data.data = { 0x04, 0, 0, 0, 0 };
        //    start = std::chrono::steady_clock::now();
        //    robot.CANSendData(can_data, ec);
        //    end = std::chrono::steady_clock::now();
        //    send_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        //    std::cout << "CANSendData 0x04 0 is: " << send_time_ms << "ms" << std::endl;
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));

        //    can_data.data = { 0x04, 255, 255, 255, 255 };
        //    start = std::chrono::steady_clock::now();
        //    robot.CANSendData(can_data, ec);
        //    end = std::chrono::steady_clock::now();
        //    send_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        //    std::cout << "CANSendData 0x04 255 is: " << send_time_ms << "ms" << std::endl;
        //    std::cout << "192.168.2.160 send can frame code: " << ec << std::endl;
        //    std::this_thread::sleep_for(std::chrono::seconds(1));
        //}
        
        /*CANFrame can_recv_1;
        robot.CANReceiveData(timeout_ms, can_recv_1, ec);
        std::cout << "192.168.2.160 recv can frame code: " << ec << std::endl;
        if (!ec) {
            std::cout << "192.168.2.160 receive can frame: " << "frame_id[" << can_recv_1.frame_id << "] frame_format[" << can_recv_1.frame_format << "] frame_type["
                << can_recv_1.frame_type << "] frame_valid_length[" << can_recv_1.frame_valid_length << "] data[";
            for (auto item : can_recv_1.data) {
                std::cout << item << " ";
            }
            std::cout << "]" << std::endl;
        }
        else {
            std::cout << "192.168.2.160 No CAN message received!" << std::endl;
        }*/
	}
	catch (const std::exception& e) {
		std::cout << e.what();
		system("pause");
	}
	std::cout << "End running" << std::endl;

	return 0;
}