/**
    05Robot - Computer Vision Module

    @author P.Kaczmarek,J.Tomczynski,T.Mankowski
    @version 1.0 2017
*/
#ifndef UR3INTERMEDIATOR_H
#define UR3INTERMEDIATOR_H

#pragma once
#include "UR3Message.h"
#include <QObject>
#include <QTcpSocket>
#include <QTcpServer>
#include <QMutex>
#include <QFile>
#include <QTextStream>
#include <QElapsedTimer>
#include <QNetworkInterface>


/** \addtogroup UR
 *  @{
 */
/**
 * @brief Base class for communication with UR robot using TCP/IP interface
 */
class UR3Intermediator: public QObject
{
    Q_OBJECT

public:
    /**
     * @brief default constructor, initializes class with starting properties
     * @param ipAddress Robot TCP/IP interface IP.
     * @param port Robot TCP/IP interface port. Default 30002.
     * @param ackServerPort Port for acknowledgment messages of command execution.
     */
    UR3Intermediator(QString ipAddress = "192.168.0.109", int port = 30002, int ackServerPort = 9090);


    /**
     * @brief Connects to a robot based on \ref IpAddress and \ref Port. Establishes acknowledgment server on port \ref ackServerPort
     * @return true on success, false otherwise
     */
    bool ConnectToRobot();
    /**
     * @brief Disconnects from robot
     * @return true on success, false otherwise
     */
    void DisconnectFromRobot();

    /**
     * @brief Generates command string for MoveJ command - move in robot joint space
     * @param Position target position as 6 element vector - joint configuration [rad] or TCP position [mm; RV]
     * @param isBaseCS specifies if position is given in joint configuration (false) or as TCP position (true)
     * @param Acceleration specifies acceleration as joint acceleration (\ref isBaseCS=false) or as TCP acceleration (\ref isBaseCS=true)
     * @param Speed specifies speed as joint speed (\ref isBaseCS=false) or as TCP speed (\ref isBaseCS=true)
     * @param blendRadius blend radius between specified and following motion [mm], omitted if 0
     * @return command string to be feed into \ref addCommandToExecuteList
     */
    QString MoveJ(QVector<double> Position, bool isBaseCS, double Acceleration= 2.0, double Speed = 4, double blendRadius = 0);
    /**
     * @brief Generates command string for MoveL command - linear move in TCP space
     * @param TargetPose target position as 6 element vector describing TCP position [mm; RV]
     * @param toolAcceleration tool acceleration
     * @param toolSpeed tool speed
     * @param time time
     * @param blendRadius blend radius between specified and following motion [mm]
     * @return command string to be feed into \ref addCommandToExecuteList
     */
    QString MoveL(QVector<double> TargetPose,double toolAcceleration=1.2,double toolSpeed=.25,double time=0, double blendRadius=0);
    /**
     * @brief Generates command string for setting Tool Center Point
     * @param pose pose describing transformation from robot flange to new TCP
     * @return command string to be feed into \ref addCommandToExecuteList
     */
    QString setTCP(QVector<double> pose);
    /**
     * @brief Generates command string for robot to sleep an amount of time
     * @param seconds time [s]
     * @return command string to be feed into \ref addCommandToExecuteList
     */
    QString robotSleep(float seconds);
    /**
     * @brief Generates command string to set tool digital output signal level
     * @param output id of the tool output (0 or 1)
     * @param state signal level
     * @return command string to be feed into \ref addCommandToExecuteList
     */
    QString setToolOutput(int output, bool state);

    /**
     * @brief main method for sending commands to robot. addCommandToExecuteList adds commands generated by MoveJ, MoveL, setTCP, robotSleep or setToolOutput to be executed
     * when robot is not running. Above commands can be concatenated to create a longer script to be send to robot at ounce. Internal UR3Intermediator mechanism queues
     * the commands and sends them when robot finished former command
     * @param cmd string containing a command to be executed
     */
    void addCommandToExecuteList(QString cmd);

    /**
     * @brief Robot TCP/IP interface port. Default 30002.
     */
    int Port;
    /**
     * @brief Robot TCP/IP interface IP.
     */
    QString IpAddress;
    /**
     * @brief Port for acknowledgment messages of command execution.
     */
    int ackServerPort;

    QString setOutput(int output, bool state);
signals:
    /**
     * @brief emitted when a new pose/data information has been received from robot
     * @param x new data received
     * @param flag determines type of message: 'j' - joint, 'p' - cartesian pose, 'f' - forces, 'c' - currents
     */
    void newPose(QVector<double> x, char flag);
    /**
     * @brief emitted on \ref ConnectToRobot()
     * @param Result true on success, false otherwise
     */
    void ConnectionAction(QString,bool Result);
    /**
     * @brief emitted on robot disconnection
     */
    void DisconnectionAction();
    /**
     * @brief emmited when new command from command list has been send to robot for execution
     */
    void CommandExecutionStart();
    /**
     * @brief emiited on command execution confirmation/acknowledgment reception
     */
    void CommandFinished();
    /**
     * @brief emiited when robot is in emergancy or protective stop state
     */
    void robotEmergancyStoped();


private:
    //Fields
    int current_timestamp; //< Licznik slużący do rysowania koła
    QElapsedTimer timerrr;
    bool _running = false;
    QVector<double> _moveJTargetPos;//< opis pola
    QVector<double> _moveLTargetPose;
    QVector<double> _lastJointPos;
    QVector<double> _lastPolozenie;
    QVector<double> _lastForceValue;
    QString SaveFileName; //< Ścieżka do pliku
    QVector<double> rotationVector;

    UR3Message ActualRobotInfo;
    char * _data;
    QByteArray _DataFlow;
    QTcpSocket* _socket;
    bool _connected = false;

    QString getAckServerIp(QString UR3IP);
    QTcpServer* _ackServer;
    QTcpSocket* _ackServerSocket = NULL;
    QString _ackServerIP;
    bool _ackLock = false;

    //Methods
    void Execute(QString command);
    void GetRobotData();
    void GetRobotMessage(char * data, unsigned int &offset, int size);
    void ReadDataFlow();

    void RealTime(unsigned int &offset);

    void timerEvent(QTimerEvent *event);

    bool CheckIfRunning();
    UR3Message GetActualUR3State() { return ActualRobotInfo; }

private slots:
    void OnSocketNewBytesWritten();
    void onSlotDisconnect();

    void ackServerSetup();
    void ackServerNewConnection();
    void ackServerNewMsg();      

private:
    QMutex mutex;
    QVector<QString> cmds;
    //Macierz *M;
};
/** @}*/

#endif // UR3INTERMEDIATOR_H
