
#include <iostream>

#include "rokae/robot.h"

using namespace rokae;

int main() {
	std::cout << "Begin Write SN..." << std::endl;
    try {
        std::string ip = "192.168.71.160";
		std::cout << "Connect robot? (default 192.168.71.160): ";
		std::string ip_input;
		std::getline(std::cin, ip_input);
		if (!ip_input.empty()) {
			ip = ip_input;
		}

		std::cout << "Establish a connection with Robot(" << ip << ")" << std::endl;

		xMateErProRobot robot(ip);
		BaseEthercat ecat = robot.ethercat();
        error_code ec;

		std::cout << "Please input SN: ";
		std::string SN_input;
		std::getline(std::cin, SN_input);
		if (!SN_input.empty()) {
			ip = SN_input;
		}
		size_t copyLength = (SN_input.length() < 50) ? SN_input.length() : 49;
		uint8_t SN_data[50];
		std::memcpy(SN_data, SN_input.c_str(), copyLength);
		SN_data[copyLength] = '\0';
		ecat.WriteSDO(10001, 0x3306, 0, 34, SN_data, 5000, ec);
		std::cout << "WriteSDO result: " << ec << std::endl;
	}
	catch (const std::exception& e) {
		std::cout << e.what();
		system("pause");
	}
	std::cout << "End Write SN..." << std::endl;
	return 0;
}