#ifndef ROBOGLUE_COM_H
#define ROBOGLUE_COM_H

/*
 * roboglue_com.cpp
 *
 *  Created on: Jan 21, 2019
 *      Author: emanuele
 *
 *  Modulo di comunicazione del software roboglue, è un thread che continua a leggere le ibformazioni del robot
 * e le scrive nella memoria condivisa tra i moduli, rivece comandi sotto forma di segnali per inviare
 * comandi URscript al controller
 *
 *  20190121 - Prima Scittura, incorpora un thread che si occupa solo della lettura dati,
 *              ad ora non finzionante.
 *  20190122 - Il thread di lettura funziona correttamente ed è implementazione di QThread,
 *              ad ogni nuovo pacchetto letto viene emess il segnale newData();
 *              la classe readerThread contiene solo la funzione che viene spostata nel thread
 *              di lettura.
 *              Impostata la struttura comune a tutti i moduli.
 *  20190123 - Impostati i segnalli per connessione e disconnessione comandata da GUI.
 *  20190125 - Corretti alcuni bug che facevano crashare in caso di mancata connessione.
 *              Aggiunto un parametero nella configurazione che definisce il massimo raggio di reach
 *              del robot. Sarà sostituito da una tabella completa delle lunghezze dei bracci per
 *              il calcolo della cinematica inversa nel main.
 * 20190128 - Inizio scrittura modulo tcpsocket per lo scambio dei consensi.
 * 20190129 - Continua scrittura del modulo socket, le variabili di scambio con il robot sono
 *              in una mappa <NOME,VALORE> <std::string,int> per facilitaà di accesso
 *              (rif documenti URscript get_var(), set_var())
 * 20190131 - Modifiche alla parte di comunicazione per usare i comandi servoj del robot come
 *              suggerito nelle librerie di ROS industrial.
 *              Ricerche iniziate su come usare MoveIT e ros indusrtial con il driver UR per
 *              la pianificazione e l'invio delle traiettorie. Il problema principale è che
 *              il driver per UR di ROS è datato e non funziona con il controller versione 5.2
 *              ROS indusrtial inoltre non è ancora compatibile con l'ultima versione di  ROS
 *              e ubuntu 18.04. Altervativa valida porebbe essere usare solo MoveIT, vedrò.
 * 20190208 - Implementazione della comunicazione ai moduli ros con mqtt
 * 20190211 - Continua implementazione della comunicazione ai moduli ros (non ancora scritto)
 *              con mqtt. Tra Qt e ROS ci saranno 3 code "bidirezionali": COMANDO, COORDINATE, STATO
 *              per separare i flussi dei messaggi.
 *              I messaggi sono strutturati come json, fare riferimento al file roboglue_messaggi.txt
 *              per la specifica di formato.
 *              I seganli che arrivano portando i messaggi sono strutturati diversamente a seconda
 *              della coda di destinazione.
 * 20190311 - La pubblicazione dei messaggi di coordinate verso ros deve essere a rateo costante
 *              indipendentemente dalla frequenza di ingresso
 */

#include <QObject>
#include <QThread>
#include <QTcpServer>
#include <QTcpSocket>
#include <QJsonObject>
#include <QJsonDocument>
#include <QList>
#include <QTimer>
#include <ctime>
#include <arpa/inet.h>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/clamp.hpp>
#include <boost/chrono.hpp>
#include <spdlog/spdlog.h>
#include <spdlog/sinks/stdout_sinks.h>
#include <mqtt/async_client.h>
#include "mqtt_callback.h"
#include <libURcom/URCLinterface.h>
#include <shared/roboglue_shared.h>

#define COMMAND 0
#define VARNAME 1
#define VALUE   2

class RoboGlue_COM: public QThread {
  Q_OBJECT

  public:
      RoboGlue_COM(RoboGlue_SHARED* mem = nullptr);
      void run() override;

  private:
      shared_ptr<spdlog::logger> liblog;
      shared_ptr<spdlog::logger> modlog;

      RoboGlue_SHARED *m;
      clientData_t localData;
      QThread *readerThr = nullptr;
      QTcpServer *server = nullptr;
      QTcpSocket *robotIN = nullptr;
      mqtt_callback *callback = nullptr;
      mqtt::async_client* client = nullptr;
      json baseMsg;
      std::string robotRequest;
      UR_CLinterface *interface = nullptr;

      QTimer* coordSendTimer = nullptr;
      QList<double> lastCoord;
      bool isCoordNew = false;

      bool isRunning = true;
      bool isConnected = false;
      bool isReading = false;
      bool isNew = false;
      bool isFirstMessage =  true;
      bool robotConnected = false;

      void saveKinematicInfo();
      void saveConnectionInfo();
      void processRobotRequest(std::string);

      void MQTTpublish(json msg, std::string topic);

  signals:
      void com_newData();
      void com_heartBeat();
      void com_newROScommand(QString, QVariantMap);

  private slots:
      void on_internalRobotConnect();
      void on_internalNewData();
      void timed_sendCoordinates();

  public slots:
      void on_quit();
      void on_commonStatusChange(void);
      void on_connect(QString ip = "", uint port = 0, uchar retry = 1);
      void on_disconnect();
      void on_sendURscript(QString command);
      void on_sendROScommand(QString, QVariantMap);
      void on_sendROScoordinates(QList<double> sp);
      void on_sendROSstate(QMap<std::string, bool> digital, QMap<std::string, uint16_t> analog);

      //void on_sendSetpointOLD(QList<double> sp);
};


class readerThread: public QThread{
    Q_OBJECT

public:
    readerThread(bool* r, UR_CLinterface* i, clientData_t* d){
        this->isReading=r;
        this->interface=i;
        this->dataOut=d;
    }
    bool* isReading;
    UR_CLinterface* interface;
    clientData_t* dataOut;

    void run() override;
    void loopRead();

signals:
    void newDataAvailable();
};

#endif // ROBOGLUE_COM_H