PositionReader.hpp

/* $Id$ */

#ifndef IMAGE_POSITION_READER_HPP
#define IMAGE_POSITION_READER_HPP

#include <string>

#include <kernel/jafarMacro.hpp>
#include <kernel/keyValueFile.hpp>

#include <jmath/jblas.hpp>

namespace jafar {
  namespace geom {
    class T3D;
  }
  namespace datareader {
    class Position;
    class PositionReader {
     private:
      std::string directory;
      std::string pattern;
      std::string patternTransfos;
      std::string patternTimes;
      jblas::vec robotToSensorCorrection;
      int first, last, step;

      geom::T3D *robotToSensor, *sensorToRobot;
      
      Position *cached_pos;
      unsigned int cached_pos_index;

     public:
      enum PosFileType {
        JAFAR,
        CALIFE,
        GENOM
      };
      enum TransfoSource {
        POS,
        T3D
      };

      PositionReader();
      ~PositionReader();
      
      /*      // for back compatibility purpose ; not compatible with swig for default parameters, ambiguity
      void setup(std::string directory_,
          std::string prefix_ = "image.",
          std::string suffix_ = ".pos",
          unsigned int indexDigits_ = 4,
          jblas::vec* robotToSensorCorrection_ = NULL) {
        setup(directory_, prefix_+std::string("%0")+std::string(1,'0'+indexDigits_)+std::string("d")+suffix_,
        robotToSensorCorrection_);
      }*/
  
      void setup(std::string directory_,
        std::string pattern_ = "image.pos.%04d.g.pos",
        std::string patternTransfos_ = "%04d_to_%04d.t3d",
        std::string patternTimes_ = "",
        jblas::vec* robotToSensorCorrection_ = NULL,
        int first_ = -1, int last_ = -1, int step_ = -1);

      Position loadPosition(unsigned int index_,
                            PosFileType fileType = JAFAR) const;
      void loadPosition(unsigned int index_, Position& pos) const;
      void loadT3D(unsigned int index1, unsigned int index2, jafar::geom::T3D &t3d) const;
      void loadTime(unsigned int index, double &time) const;
      jafar::geom::T3D* loadTransfo(unsigned int iFrom, unsigned int iTo, int *dt = NULL, TransfoSource source = POS);
    };
    
    
    class Position : public kernel::KeyValueFileLoad {
     public:
      Position() {}
      
     public:
      //<begin> to delete
      void sensorToMain(jafar::geom::T3D& pos) { robotToSensor(pos); }
      jafar::geom::T3D* sensorToMain() { return robotToSensor(); }
      void mainToBase(jafar::geom::T3D& pos) { baseToRobot(pos); }
      jafar::geom::T3D* mainToBase() { return baseToRobot(); }
      void mainToOrigin(jafar::geom::T3D& pos) { originToRobot(pos); }
      jafar::geom::T3D* mainToOrigin() { return originToRobot(); }
      //<end>
      void robotToSensor(jafar::geom::T3D& pos);
      jafar::geom::T3D* robotToSensor();
      void baseToRobot(jafar::geom::T3D& pos);
      jafar::geom::T3D* baseToRobot();
      void originToRobot(jafar::geom::T3D& pos);
      jafar::geom::T3D* originToRobot();

      inline int date() { return m_date; }
      static void fromCalifeToJafar(const jblas::vec src, jblas::vec& dst);
      static void fromJafarToCalife(const jblas::vec src, jblas::vec& dst);
      
     protected:
      virtual void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
      void fixRobotToSensor(const jblas::vec &robotToSensorCorrection);
      bool loadPomPosition(std::string filename);

     private:
      void loadStringInT3D(const std::string& , jafar::geom::T3D& pos);
      bool readline(std::string &line, char *sep, int index, int len, jblas::vec &res);
      jblas::vec m_robotToSensor, m_baseToRobot, m_originToRobot;
      int m_date;

      friend Position PositionReader::loadPosition(unsigned int index_,
              PositionReader::PosFileType fileType) const;
      friend void PositionReader::loadPosition(unsigned int index_, Position& pos) const;
    };

  }
}
#endif // DATAREADER_POSITION_READER_HPP