#ifndef STAUBLI_H
#define STAUBLI_H

#pragma once
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <iostream>
#include <fstream>
#include <string>
#include <array>
#include <sstream>
#include <functional>
#include <stdio.h>
#include <vector>
#include <iomanip>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <assert.h>
#include "libs/tcpc/tcpc_defs.h"
#include "libs/robot/robot_defs.h"

#define SA struct sockaddr

#define ACCURACY 1 //accuracy 1mm
namespace ri{

class Staubli
{
public:
    Staubli();

    Staubli(tcpc::sRobot_interface interface);

    ~Staubli();

    std::string getVersion();

    ri::Point getPosition();

    bool setPosition(ri::Point position);

    bool setPositionJ(float j1, float j2, float j3, float j4, float j5, float j6);

    bool setPositionJ(std::vector<float> joints);

    void getPositionJ(float& j1, float& j2, float& j3, float& j4, float& j5, float& j6);

    std::vector<float> getPositionJ();

    bool isEstablished();

    void disconnect();

    void connect();

    tcpc::sRobot_interface getParameters();

    void setParameters(tcpc::sRobot_interface parameters);

    void setTOOL(eRobot_TOOL tool);

    eRobot_TOOL getTOOL();

    void setFRAME(eRobot_FRAME frame);

    eRobot_FRAME getFRAME();

    bool atHome();

    void setHome(Point home_position);

    bool atPosition(Point position);

    void goHome();

    bool setTCP(Point TCP_coordinates, eRobot_TOOL TOOL);

    void setSpeed(float speed);

private:
    const std::string packet_pattern = "EthernetVAL3:";
    Point defaul_position;
    eRobot_TOOL tool = eRobot_TOOL::FLANGE;
    eRobot_FRAME frame = eRobot_FRAME::WORLD;
    tcpc::sRobot_interface parameters;
    int socket;
    bool bEstablished = false;

    bool isAccesible();

    void establish_connection();

    void reconnect();

    std::stringstream sendPacket(const std::stringstream& msg);

};//class_Staubli

}//namespace_ri

#endif // STAUBLI_H
