#include "staubli.h"

namespace ri{

Staubli::Staubli(){}

Staubli::Staubli(tcpc::sRobot_interface interface)
            :parameters(interface){
                establish_connection();
            }

Staubli::~Staubli(){}

bool Staubli::isAccesible(){
    return true;
}

void Staubli::establish_connection(){
    if(!isAccesible()) throw std::runtime_error("Not possible to ping");
    int sockfd;
    struct sockaddr_in servaddr;

    // socket create and varification
    sockfd = ::socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd == -1) {
        throw std::runtime_error("Socket creation for Staubli robot TCP communcation has failed");
    }else std::cout << "Socket for Staubli robot TCP communication created" << std::endl;

    bzero(&servaddr, sizeof(servaddr));

    // assign IP, PORT
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = inet_addr(this->parameters.ipv4_Address.c_str());
    servaddr.sin_port = htons(this->parameters.port);

    // connect the client socket to server socket
    if (::connect(sockfd, (SA*)&servaddr, sizeof(servaddr)) != 0) {
        throw std::runtime_error("Unable to establish socket connection with Staubli robot TCP");
    } else std::cout << "Staubli robot TCP connection established" << std::endl;

    this->socket = sockfd;
    this->bEstablished = true;
}

void Staubli::disconnect(){
    if(!this->isEstablished()) return;
    ::close(this->socket);
    this->bEstablished = false;
}

void Staubli::connect(){
    if(this->isEstablished()) throw std::runtime_error("Unable to establish socket connection, which is established");
    try {
        establish_connection();
    } catch (...) {
        throw std::runtime_error("Unable to establish socket connection which is not guaranted");
    }

}

void Staubli::reconnect() {
    establish_connection();
    if(this->socket == -1) {
        throw std::runtime_error("Unable to establish socket connection with Staubli robot TCP");
    }
}

std::stringstream Staubli::sendPacket(const std::stringstream& msg){
    std::stringstream WBuffer, result;

    WBuffer << msg.str() << '\000';
    //write
    int write_status = write(this->socket, WBuffer.str().c_str(), WBuffer.str().size());
    if (write_status < 0) {
        std::cerr<<"Write failed with "<< write_status <<std::endl;
        reconnect();
    }
    //read
    while(1){
        u_char buffer[1];
        int read_status = read(this->socket, buffer, 1);
        if (read_status < 0) std::cerr<<"Read failed with "<< read_status <<std::endl;
        if(buffer[0] == '\000') break;
        result<<buffer;
    }
    return result;
}

std::string Staubli::getVersion(){
    std::string result;
    std::stringstream cmd;
    cmd << this->packet_pattern << "GetVersion:System";
    std::stringstream response(sendPacket(cmd).str());
    while(std::getline(response, result, ':')){}
    return result;
}

tcpc::sRobot_interface Staubli::getParameters(){
    return this->parameters;
}

void Staubli::setParameters(tcpc::sRobot_interface parameters){
    this->parameters = parameters;
}

ri::Point Staubli::getPosition(){
    std::string value;
    ri::Point result;
    std::stringstream cmd;
    cmd << this->packet_pattern << "HereExact:" <<  std::to_string(static_cast<uint8_t>(this->tool)) << ":" <<  std::to_string(static_cast<uint8_t>(this->frame));
    std::stringstream response(sendPacket(cmd).str());
    std::getline(response, value, ':');
        std::getline(response, value, ':');
        result.x = std::stod(value);
        std::getline(response, value, ':');
        result.y = std::stod(value);
        std::getline(response, value, ':');
        result.z = std::stod(value);
        std::getline(response, value, ':');
        result.rx = std::stod(value);
        std::getline(response, value, ':');
        result.ry = std::stod(value);
        std::getline(response, value, ':');
        result.rz = std::stod(value);

//        std::cout<<std::to_string(result.x)<<" "<<std::to_string(result.y)<<" "<<std::to_string(result.z)<<
//                   " "<<std::to_string(result.rx)<<" "<<std::to_string(result.ry)<<" "<<std::to_string(result.rz)<<" "<<std::endl;

    return result;
}

bool Staubli::setPosition(ri::Point position){
    std::stringstream cmd, test;
    cmd << this->packet_pattern << "MoveJ:";
    cmd << position.x << ":" << position.y << ":" << position.z << ":" <<
           position.rx << ":" << position.ry << ":" << position.rz << ":";
    cmd << std::to_string(static_cast<uint8_t>(this->tool)) << ":" <<  std::to_string(static_cast<uint8_t>(this->frame));
    std::stringstream response(sendPacket(cmd).str());
    //cmd.flush();
    test << this ->packet_pattern << "WaitEndMove";
    response << sendPacket(test).str();
    return (response.str() == "");
}

bool Staubli::isEstablished(){
    return this->bEstablished;
}

void Staubli::setTOOL(eRobot_TOOL tool){
    this->tool = tool;
}

eRobot_TOOL Staubli::getTOOL(){
    return this->tool;
}

void Staubli::setFRAME(eRobot_FRAME frame){
    this->frame = frame;
}

eRobot_FRAME Staubli::getFRAME(){
    return this->frame;
}

void Staubli::setHome(Point home_position){
    this->defaul_position = home_position;
}

bool Staubli::atPosition(Point position){
    bool result = false;
    Point actual_position = this->getPosition();

    result &= actual_position.x < position.x+ACCURACY && actual_position.x > position.x-ACCURACY;
    result &= actual_position.y < position.y+ACCURACY && actual_position.y > position.y-ACCURACY;
    result &= actual_position.z < position.z+ACCURACY && actual_position.z > position.z-ACCURACY;
    result &= actual_position.rx < position.rx+ACCURACY && actual_position.rx > position.rx-ACCURACY;
    result &= actual_position.ry < position.ry+ACCURACY && actual_position.ry > position.ry-ACCURACY;
    result &= actual_position.rz < position.rz+ACCURACY && actual_position.rz > position.rz-ACCURACY;

    return result;
}

bool Staubli::atHome(){
    return this->atPosition(this->defaul_position);
}

void Staubli::goHome(){
    if(this->getPosition().x==0) {
    throw std::runtime_error ("Default position is not setted");
    }
    this->setPosition(this->defaul_position);
}

bool Staubli::setTCP(Point TCP_coordinates, eRobot_TOOL TOOL){
    std::stringstream cmd_I, cmd_II;
    cmd_I << this->packet_pattern << "SetTool:";
    cmd_I << TCP_coordinates.x << ":" << TCP_coordinates.y << ":" << TCP_coordinates.z << ":" <<
           TCP_coordinates.rx << ":" << TCP_coordinates.ry << ":" << TCP_coordinates.rz << ":";
    cmd_I << std::to_string(static_cast<uint8_t>(TOOL));
    std::stringstream response(sendPacket(cmd_I).str());
    //cmd.flush();
    cmd_II << this ->packet_pattern << "WaitEndMove";
    response << sendPacket(cmd_II).str();
    return (response.str() == "");
}

bool Staubli::setPositionJ(float j1, float j2, float j3, float j4, float j5, float j6){
    std::stringstream cmd_I, cmd_II;
    cmd_I << this->packet_pattern << "MoveJJ:";
    cmd_I << j1 << ":" << j2 << ":" << j3 << ":" << j4 << ":" << j5 << ":" << j6;
    std::stringstream response(sendPacket(cmd_I).str());
    //cmd.flush();
    cmd_II << this ->packet_pattern << "WaitEndMove";
    response << sendPacket(cmd_II).str();
    return (response.str() == "");
}

bool Staubli::setPositionJ(std::vector<float> joints){
    if(joints.size()!=6) throw std::runtime_error("Bad size of Joints vector");
    return setPositionJ(joints.at(0),joints.at(1),joints.at(2),joints.at(3),joints.at(4),joints.at(5));
}

void Staubli::getPositionJ(float& j1, float& j2, float& j3, float& j4, float& j5, float& j6){
    std::string value;
    std::stringstream cmd;
    cmd << this->packet_pattern << "HereJ";
    std::stringstream response(sendPacket(cmd).str());
    std::getline(response, value, ':');
        std::getline(response, value, ':');
        j1 = std::stod(value);
        std::getline(response, value, ':');
        j2 = std::stod(value);
        std::getline(response, value, ':');
        j3 = std::stod(value);
        std::getline(response, value, ':');
        j4 = std::stod(value);
        std::getline(response, value, ':');
        j5 = std::stod(value);
        std::getline(response, value, ':');
        j6 = std::stod(value);
}

std::vector<float> Staubli::getPositionJ(){
    std::vector<float> result;
    std::string value;
    std::stringstream cmd;
    cmd << this->packet_pattern << "HereJ";
    std::stringstream response(sendPacket(cmd).str());
    std::getline(response, value, ':');
    for(int i=0;i<6;i++){
        std::getline(response, value, ':');
        result.push_back(std::stod(value));
    }
    return result;
}

void Staubli::setSpeed(float speed){
        std::stringstream cmd;
        cmd << this->packet_pattern << "MoveSpeed:";
        cmd << std::to_string(static_cast<uint8_t>(speed));
        std::stringstream response(sendPacket(cmd).str());
}

}//namespace_ri
