client.hpp

#ifndef CLIENT_HPP
#define CLIENT_HPP

#include <cstdlib>
#include <cstdio>
#include <sstream>
#include <fstream>

#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/filesystem.hpp>


#include <gdhe/GDHE_client_prot.h>

#include "kernel/IdFactory.hpp"
#include "jmath/misc.hpp"
#include "jmath/jblas.hpp"

#include "gdhe/gdheException.hpp"

#define ENABLE_CHILDREN 0

namespace jafar {
namespace gdhe {
  



  struct ColorRGB
  {
    unsigned char R, G, B;
    ColorRGB(unsigned char R_, unsigned char G_, unsigned char B_): R(R_), G(G_), B(B_) {}
    ColorRGB(): R(0), G(0), B(0) {}
//    std::ostream& operator<<(std::ostream& os)
//      { os << (int)R << " " << (int)G << " " << (int)B; return os; }
  };

  std::ostream& operator<<(std::ostream& os, const ColorRGB & c)
    { os << (int)c.R << " " << (int)c.G << " " << (int)c.B; return os; }
  
  extern const ColorRGB colorRed;
  extern const ColorRGB colorGreen;
  extern const ColorRGB colorBlue;



  class Object;
  
  typedef kernel::IdFactorySet MyIdFactory;
  
  // FIXME maybe ask Matthieu to add const the parameters when included in c++ ? what's the best solution vs const_cast ?
  class Client
  {
    protected:
      MyIdFactory idFactory;
      std::string host;
      bool host_is_local;
      ColorRGB backgroundColor;
    protected:
      void init();
      typedef std::pair<std::string,std::string> ConvertData;
      //std::list<ConvertData> convert_queue;
      ConvertData convert_data;
      boost::mutex convert_mutex;
      boost::condition_variable convert_condition;
      std::string convert_tmppath;
      bool convert_done;
      bool convert_end;
      boost::thread *convert_thread;
      void convert()
      {
        std::cout << "starting conversion task !" << std::endl;
        convert_done = false;
        convert_end = false;
        while(true)
        {
          // wait for a task
          boost::unique_lock<boost::mutex> convert_lock(convert_mutex);
          while(convert_done && !convert_end) convert_condition.wait(convert_lock);
          if (convert_done && convert_end) break;
          convert_lock.unlock();
          
          // wait that the input file exists and is writable
          std::fstream f;
          while (!f.is_open())
          {
            f.open(convert_data.first.c_str(), std::fstream::out | std::fstream::app);
            usleep(100*1000);
          }
          f.close();
          
          // then convert
          std::ostringstream oss; oss << "convert " << convert_data.first << " " << convert_data.second << " ; rm -f " << convert_data.first;
          int r = system(oss.str().c_str());
          if (r == -1) std::cout << "conversion from " << convert_data.first << " to " << convert_data.second << " failed." << std::endl;
          
          // and notify that has finished
          convert_lock.lock();
          convert_done = true;
          convert_condition.notify_all();
        }
      }
    public:
      Client(): idFactory(), host("localhost"), host_is_local(true), backgroundColor(194,184,176), convert_thread(NULL) {}
      Client(std::string host_): idFactory(), host(host_), host_is_local(host_ == "localhost"), backgroundColor(194,184,176), convert_thread(NULL) {}
      ~Client()
      {
        // wait the convert thread
        if (convert_thread)
        {
          boost::unique_lock<boost::mutex> convert_lock(convert_mutex);
          convert_end = true;
          convert_thread->join();
          delete convert_thread;
        }
      }
      void setHost(std::string host_) { host = host_; }
      int connect(bool wait = true);
      int disconnect() { return ::disconnect(); }
      int eval(std::string exp) 
      {
        int r;
        r = ::eval_expression(const_cast<char*>(exp.c_str()));
        //JFR_DEBUG("evaluated [" << r << "] '" << exp << "'");
        return r;
      }
      int launch_server()
        { return system("gdhe&"); }
      int dump(std::string const& filename)
      {
        int r;
        boost::filesystem::path filepath(filename);
        std::string filebasepath = filepath.branch_path().string();
        std::string filebasename = boost::filesystem::basename(filepath); //filename.substr(0, filename.size()-4);
        std::string extension = boost::filesystem::extension(filepath); //filename.substr(filename.size()-3, 3);
        std::ostringstream oss; oss << "$gdheBase.gdhe.ogl dumpPpm -color ";
//std::cout << "extension gdhe dump is '" << extension << "'"<< std::endl;
        if (host_is_local && extension == ".png")
        {
//std::cout << "converting to png!" << std::endl;
          //-- dump
          std::string tmpname = 
            (convert_tmppath == "" ? filebasepath + std::string("/") : convert_tmppath + std::string("/gdhe.")) + 
            filebasename + std::string(".ppm");
          oss << tmpname;
          r = eval(oss.str());
          
          //-- ask conversion
          boost::unique_lock<boost::mutex> convert_lock(convert_mutex);
          // wait that the previous conversion has finished
          if (convert_thread != NULL)
            while (!convert_done) convert_condition.wait(convert_lock);
          // update data
//std::cout << "DUMP: previous conversion has finished, changing data" << std::endl;
          convert_data = ConvertData(tmpname, filebasepath+std::string("/")+filebasename+std::string(".png"));
          convert_done = false;
          // start thread if first time
          if (convert_thread == NULL)
            convert_thread = new boost::thread(boost::bind(&Client::convert,this));
          // notify
          convert_lock.unlock();
          convert_condition.notify_all();
          
        } else
        {
          oss << filebasepath << "/" << filebasename << ".ppm"; 
          r = eval(oss.str());
        }
        return r;
      }
      void setConvertTempPath(std::string convert_tmppath_) { convert_tmppath = convert_tmppath_; }
      void redraw()
        { eval("redrawAllWindows"); }
      void clear() 
        { eval("set robots(test) {sphere 0 0 0 0};unset robots;set pos(test) {0 0 0 0 0 0};unset pos;"); }
      
      void setBackgroundColor(ColorRGB &_color) 
        { backgroundColor = _color; }
      void setBackgroundColor(unsigned char R_, unsigned char G_, unsigned char B_) 
        { backgroundColor.R = R_; backgroundColor.G = G_; backgroundColor.B = B_; }
      
      void addObject(Object *object, bool visible = true);
      void addSubObject(Object *object, Object *parent, std::string suffix, bool visible = true);
      void removeObject(Object *object);
      void setCameraTarget(double _x, double _y, double _z);
      void setCameraPos(double _yaw, double _pitch, double _dist);
      
    protected:
    public:
      struct SendToServer {};
      static SendToServer sendToServer;
      std::ostringstream oss;
//      friend template<typename T> Client& operator<<(Client& oc, const T & element);
//      friend template<> Client& operator<<(Client& oc, Client::SendToServer element);
  };
  
  template<typename T> Client& operator<<(Client& oc, const T & element)
    { oc.oss << element; return oc; }
  template<> Client& operator<<(Client& oc, const Client::SendToServer & element)
    { oc.eval(oc.oss.str()); oc.oss.str(""); return oc; }
  


  class Label;
  
  class Object
  {
    protected:
      double x, y, z;
      double yaw, pitch, roll;
      ColorRGB color;
      bool poseModified, attributesModified;
      
      Label *label;
      void createLabel();
      void updateLabelPose();
      
    protected:
      bool ownId;
      MyIdFactory::storage_t id; 
      std::string ids;
      Client *client;
      void registerClient(Client *client_);
      void setId(MyIdFactory::storage_t id_, std::string suffix = "", bool ownId_ = true) 
        { ownId = ownId_; id = id_; ids = jmath::toStr(id)+suffix; }
      void touch() { attributesModified = true; }
      #if ENABLE_CHILDREN
      std::list<Object*> children;
      #endif
    public:
      Object(): 
        x(0), y(0), z(0), yaw(0), pitch(0), roll(0), color(), poseModified(true), attributesModified(true), label(NULL), ownId(false), id(0), ids(""), client(NULL)
        { }
      Object(double _x, double _y, double _z, double _yaw, double _pitch, double _roll): 
        poseModified(true), attributesModified(true), label(NULL), ownId(false), id(0), ids(""), client(NULL) 
        { setPose(_x,_y,_z,_yaw,_pitch,_roll); }
      Object(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned char R_, unsigned char G_, unsigned char B_): 
        poseModified(true), attributesModified(true), label(NULL), ownId(false), id(0), ids(""), client(NULL) 
        { setPose(_x,_y,_z,_yaw,_pitch,_roll); setColor(R_,G_,B_); }
      virtual ~Object();

      virtual const std::string construct_string() const = 0;
      virtual const std::string move_string() const
      {
        std::ostringstream oss;
        oss << "set pos(" << ids << ") {" << yaw << " " << pitch << " " << roll << " " << x << " " << y << " " << z << "};";
        return oss.str();
      }
      virtual const std::string remove_string() const
      {
        std::ostringstream oss;
        oss << "unset robots(" << ids << ");unset pos(" << ids << ");";
        return oss.str();
      }
      virtual void move_command(bool send = false) const
      {
        *client << "set pos(" << ids << ") {" << yaw << " " << pitch << " " << roll << " " << x << " " << y << " " << z << "};";
        if (send) *client << Client::sendToServer;
      }
      virtual void remove_command(bool send = false) const
      {
        *client << "unset robots(" << ids << ");unset pos(" << ids << ");";
        if (send) *client << Client::sendToServer;
      }
      
      void remove()
      {
        if (!client) JFR_ERROR(GdheException, GdheException::NOT_ADDED_TO_CLIENT, "This object was not added to a client");
        client->removeObject(this);
      }
      void hide()
      {
        if (!client) JFR_ERROR(GdheException, GdheException::NOT_ADDED_TO_CLIENT, "This object was not added to a client");
        std::ostringstream oss; oss << "unset pos(" << ids << ");"; client->eval(oss.str());
        poseModified = true;
      }
      void show()
      {
        if (!client) JFR_ERROR(GdheException, GdheException::NOT_ADDED_TO_CLIENT, "This object was not added to a client");
        client->eval(move_string());
        poseModified = false;
      }
      void refresh();
      
      void setPose(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
        { x = _x; y = _y; z = _z; yaw = _yaw; pitch = _pitch; roll = _roll; poseModified = true; updateLabelPose(); }
      void setPose(jblas::vec &pose) 
        { setPose(pose(0),pose(1),pose(2),pose(3),pose(4),pose(5)); }
      void setPose(jblas::vec &position, jblas::vec &euler)
        { setPose(position(0),position(1),position(2),euler(0),euler(1),euler(2)); }
      
      void setColor(ColorRGB &_color) 
        { color = _color; touch(); }
      void setColor(unsigned char R_, unsigned char G_, unsigned char B_) 
        { color.R = R_; color.G = G_; color.B = B_; touch(); }
      
      void setLabel(std::string text);
      void setLabelColor(ColorRGB &_color);
      void setLabelColor(unsigned char R_, unsigned char G_, unsigned char B_);
      void setLabelShift(double x_, double y_, double z_);
      
      friend class Client;
  };
  
  
  



  class Label: public Object
  {
    protected:
      std::string text;
      double shiftX, shiftY, shiftZ;
    public:
      Label(): text("") {}
      Label(std::string text_): text(text_) {}
      void setText(std::string text_) { text = text_; touch(); }
      void setShift(double x_, double y_, double z_) { shiftX = x_; shiftY = y_; shiftZ = z_; touch(); }
      virtual const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "drawString " << shiftX << " " << shiftY << " " << shiftZ << " \"" << text << "\"};";
        return oss.str();
      }
  };
  
  class Robot: public Object
  {
    protected:
      std::string model;
    public:
      Robot(std::string model_):
        model(model_) {}
      Robot(std::string model_, double x_, double y_, double z_, double yaw_, double pitch_, double roll_):
        model(model_)
      {
        setPose(x_, y_, z_, yaw_, pitch_, roll_);
      }
      
      virtual const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        if (model != "") oss << model; else oss << "color 255 255 255;sphere 0.001 12";
        oss << "};";
        return oss.str();
      }
      
  };
  
  
  class Sphere: public Object
  {
    protected:
      double radius;
      int facets;
      
    public:
      Sphere(double radius_, double facets_ = 12):
        Object(), radius(radius_), facets(facets_)
        { }

      void setRadius(double _radius) { radius = _radius; touch(); if (label) label->setShift(0,0,_radius*1.3); }
      void setFacets(double _facets) { facets = _facets; touch(); }

      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "sphere " << 0 << " " << 0 << " " << 0 << " " << radius << " " << facets << "};";
        return oss.str();
      }
  };
  

  class Ellipsoid: public Object
  {
    protected:
      double dx, dy, dz;
      int facets;
      
    public:
      Ellipsoid(double facets_ = 12):
        Object(), dx(0), dy(0), dz(0), facets(facets_)
        { }
      Ellipsoid(double _dx, double _dy, double _dz, double facets_ = 12):
        Object(), dx(_dx), dy(_dy), dz(_dz), facets(facets_)
        { }
      Ellipsoid(double x0_, double y0_, double z0_, double _dx, double _dy, double _dz, unsigned char R_, unsigned char G_, unsigned char B_, double facets_ = 12):
        Object(x0_, y0_, z0_, 0, 0, 0, R_, G_, B_), dx(_dx), dy(_dy), dz(_dz), facets(facets_)
        { }
      Ellipsoid(jblas::vec3 _x, jblas::sym_mat33 _xCov, double _scale = 1)
        { set(_x, _xCov, _scale); }
      void set(jblas::vec3 _x, jblas::sym_mat33 _xCov, double _scale = 1);
      void setCompressed(jblas::vec3 _x, jblas::sym_mat33 _xCov, double _scale = 1);

      void setRads(double _dx, double _dy, double _dz) { dx = _dx; dy = _dy; dz = _dz; touch(); if (label) label->setShift(0,0,_dz*1.3); }
      void setFacets(double _facets) { facets = _facets; touch(); }

      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "ellipsoid " << 0 << " " << 0 << " " << 0 << " " << dx << " " << dy << " " << dz << " " << facets << "};";
        return oss.str();
      }
  };

  class EllipsoidWire: public Ellipsoid
  {
    public:
      EllipsoidWire(double facets_ = 12): Ellipsoid(facets_) {}
      EllipsoidWire(double _dx, double _dy, double _dz, double facets_ = 12): Ellipsoid(_dx,_dy,_dz, facets_) {}
      EllipsoidWire(double x0_, double y0_, double z0_, double _dx, double _dy, double _dz, unsigned char R_, unsigned char G_, unsigned char B_, double facets_ = 12): Ellipsoid(x0_, y0_, z0_, _dx, _dy, _dz, R_, G_, B_, facets_) {}
      EllipsoidWire(jblas::vec3 _x, jblas::sym_mat33 _xCov, double _scale = 1): Ellipsoid(_x, _xCov, _scale) {}
      
      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        #if 0 // waiting approval of a commit in gdhe
        oss << "ellipse " << 0 << " " << 0 << " " << 0 << " z " << 2*dx << " " << 2*dy << " " << facets << ";";
        oss << "ellipse " << 0 << " " << 0 << " " << 0 << " y " << 2*dx << " " << 2*dz << " " << facets << ";";
        oss << "ellipse " << 0 << " " << 0 << " " << 0 << " x " << 2*dy << " " << 2*dz << " " << facets << ";";
        #endif
        oss << "polyline 2 " << -dx << " " <<  0  << " " <<  0  << " " << +dx << " " <<  0  << " " <<  0  << ";";
        oss << "polyline 2 " <<  0  << " " << -dy << " " <<  0  << " " <<  0  << " " << +dy << " " <<  0  << ";";
        oss << "polyline 2 " <<  0  << " " <<  0  << " " << -dz << " " <<  0  << " " <<  0  << " " << +dz << "};";
        return oss.str();
        // TODO also draw the angle uncertainty with an ellipsoidic cone (an ellipse which is the intersection
        // of an axis when the rotation around the two other axes vary, and 4 segments which are this axis at extrema)
      }
  };

  
  class Grid: public Object
  {
    protected:
      double extent;
      double size;
      
    public:
      Grid(double _extent, double _size):
        Object(), extent(_extent), size(_size)
        { }

      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "grille " << -extent << " " << -extent << " " << extent << " " << extent << " " << size << "};";
        return oss.str();
      }
  };
  
  struct Point3D
  {
    double x, y, z;
    Point3D(double x_, double y_, double z_): x(x_), y(y_), z(z_) {}
  };
  
  class Polyline: public Object
  {
    protected:
      std::vector<Point3D> line;
      
    public:
      Polyline(): Object() { }
      Polyline(double x0, double y0, double z0, double x1, double y1, double z1):
        Object()
      {
        line.push_back(Point3D(x0,y0,z0));
        line.push_back(Point3D(x1,y1,z1));
      }

      void addPoint(const Point3D &point) { line.push_back(point); touch(); }
      void addPoint(jblas::vec point) { addPoint(Point3D(point(0),point(1),point(2))); }
      void addPoint(double x, double y, double z) { addPoint(Point3D(x,y,z)); }
      void clear() { line.clear(); touch(); }
      Point3D const& back() { return line.back(); }
      int size() { return line.size(); }

      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "polyline " << line.size();
        for(std::vector<Point3D>::const_iterator it = line.begin(); it != line.end(); ++it)
          oss << " " << it->x << " " << it->y << " " << it->z;
        oss << "};";
        return oss.str();
      }
  };
  
  // TODO maybe compress a little bit, we'll lose detail but it's really big at 60Hz...
  class Trajectory: public Object
  {
    protected:
      std::vector<Polyline*> traj;
      int poly_size;
      Polyline *last_poly;
    public:
      Trajectory(): Object(), poly_size(20), last_poly(NULL) {}
      ~Trajectory()
      {
        for(std::vector<Polyline*>::iterator it = traj.begin(); it != traj.end(); ++it)
          delete *it;
      }
      void addPoint(const Point3D &point)
      {
        if (last_poly == NULL || last_poly->size() >= poly_size)
        {
          Polyline *prev_last_poly = last_poly;
          traj.push_back(new Polyline());
          last_poly = traj.back();
          last_poly->setColor(color);
          std::ostringstream oss; oss << "-" << traj.size();
          client->addSubObject(last_poly, this, oss.str(), false);
          poseModified = true;
          if (prev_last_poly)
            last_poly->addPoint(prev_last_poly->back());
        }
        last_poly->addPoint(point);
        touch();
      }
      void addPoint(jblas::vec point) { addPoint(Point3D(point(0),point(1),point(2))); }
      void addPoint(double x, double y, double z) { addPoint(Point3D(x,y,z)); }
      void clear() { traj.clear(); touch(); }
    
      const std::string move_string() const
        { if (last_poly) return last_poly->move_string(); else return ""; }
      const std::string remove_string() const
        { if (last_poly) return last_poly->remove_string(); else return ""; }
      const std::string construct_string() const
        { if (last_poly) return last_poly->construct_string(); else return ""; }
  };
  
  
  class Frame: public Object
  {
    protected:
      double length;
    public:
      Frame(): Object() { }
      Frame(double length_):
        Object(), length(length_) {}
      
      const std::string construct_string() const
      {
        std::ostringstream oss;
        oss << "set robots(" << ids << ") {";
        oss << "color " << color << ";";
        oss << "repere " << length << "};";
        return oss.str();
      }
  };
  
  
}}

#endif