accelmotion.hpp

#ifndef DDF_ACCELMOTION_HPP
#define DDF_ACCELMOTION_HPP

#include <iostream>
#include <vector>
#include <algorithm>
#include <sstream>

#include "accprofileitem.hpp"
#include "ddf/debugging.hpp"

namespace jafar
{
  namespace ddfsimu
  {

    typedef std::vector<ddfsimu::AccProfileItem> AccelProfile;

    typedef struct {
      double A_max;
      double A_min;
      double V_max;
      double V_min;
      double P_max;
      double P_min;
      bool   computed;
    }ExtValStruct;

    class AccelMotion
    {
    protected:
      VEC          m_vel_init;  // initial velocity
      VEC          m_pos_init;  // initial position

    private:
      AccelProfile m_accel_profile;  // store the acceleration profile for all the dimensions
      bool         m_profile_sorted; // say if the profile is sorted chronologically
      ExtValStruct m_extval[4];      // store the extremal values for all axis for x, y, z and max amongst all axis
      std::string  m_name;           // name of the instance

    public:
      typedef enum{SAMPLE_ACC = 1 << 0, SAMPLE_VEL = 1 << 1, SAMPLE_POS = 1 << 2} SAMPLE_TYPE;
      typedef enum{SV_X, SV_VX, SV_AX, SV_Y, SV_VY, SV_AY, SV_Z, SV_VZ, SV_AZ, SV_BAX, SV_BAY, SV_BAZ} SV_IDX_TYPE;
      
      AccelMotion(std::string name);
      virtual ~AccelMotion();
      
      std::string const& GetName() const { return m_name; }
      void GetName(char *name) const { strcpy(name, m_name.c_str()); }
      
      void * GetInstance() { return (void*) this; }

      void SetInitialConditions(VEC const& v0, VEC const& p0) {
        m_pos_init = p0;
        m_vel_init = v0;
      }
      
      void GetInitialPosition(VEC &p) const { p = m_pos_init; }
      void GetInitialVelocity(VEC &v) const { v = m_vel_init; }
      VEC const& GetInitialPosition() const { return m_pos_init; }
      VEC const& GetInitialVelocity() const { return m_vel_init; }
      VEC  GetFinalPosition() { return ComputePositionAtTimeT(GetMaxTime()); }
      
      unsigned short GetProfileSize() const { return m_accel_profile.size(); }
      VEC const& GetAccel(unsigned short idx) const { JFR_PRECOND(idx < m_accel_profile.size(), "index"); return m_accel_profile[idx].GetAccel(); }
      AccProfileItem const* GetAccelItem(unsigned short idx) const
      {
        JFR_PRECOND(idx < m_accel_profile.size(), "index");
        return &m_accel_profile[idx];
      }
      
      void AddToAccelProfile(AccProfileItem const &item) {
        m_accel_profile.push_back(item);
        ToggleExtComputedFlag(false); }
      
      void AddToAccelProfile(double t, VEC const& a) {
        time  ti(t);
        AccProfileItem item(ti,a);
        AddToAccelProfile(item);
      }

      void SortAccelProfile () {
        sort(m_accel_profile.begin(),m_accel_profile.end());
        JFR_PRECOND (m_accel_profile[0].GetTimeDbl() == 0.0, "!= 0.0"); // make sure that the simulation starts at time 0.0
        m_profile_sorted = true;
      }

      bool IsProfileSorted() const { return m_profile_sorted; }
      void ComputeVelocityAndPos (double time, VEC & velocity, VEC & position) const;

      void PrintInitialConditions() const {
        std::ostringstream stream;

        stream << "\n* Initial conditions for " << GetName() << "\n   velocity :" << m_vel_init << "\n   position :" << m_pos_init << "\n";
        JFR_DEBUG(stream.str());
      }

      void PrintAccelProfile();
      double FillArrayWithSamples(float *pArray, unsigned short samples, SAMPLE_TYPE type, AXIS_TYPE axis);
      VEC GetAccelAtTimeT(double t) const;
      VEC GetInitialAccel() const { return GetAccelAtTimeT(GetMinTimeSec()); }
      VEC ComputePositionAtTimeT(double t) const;

      double GetMaxTime() const {
        JFR_PRECOND(m_profile_sorted == true, "not sorted yet");
        return m_accel_profile[m_accel_profile.size()-1].GetTimeDbl();
      }
      
      double GetMinTimeSec() const { JFR_PRECOND(m_profile_sorted == true, "not sorted yet"); return m_accel_profile[0].GetTimeDbl(); }
      time GetMinTime() const { JFR_PRECOND(m_profile_sorted == true, "not sorted yet"); return m_accel_profile[0].GetTime(); }
      double GetTimeSpan() const { return GetMaxTime() - GetMinTimeSec();}
      virtual double GetMinTimeSlice() const;
      virtual unsigned short GetMinSamplesNr() const;
      void ComputeExtremeValues();
      bool IsExtremeComputed();
      
      ExtValStruct GetExtremeValues(AXIS_TYPE type) {
        JFR_PRECOND(IsExtremeComputed() == true, "extremal values are not computed");
        return m_extval[type];
      }
        
      ExtValStruct GetExtremeValues(AccelMotion & r, AXIS_TYPE type = AXIS_TYPE(AXIS_Z+1));
      void PrintExtreme(AXIS_TYPE type);
      void PrintExtreme(ExtValStruct &extval);

      void FillProfile(unsigned short buf_size, double data[][4]);
      void ClearProfile();
      void GdheTrajCmd(unsigned short nbpoints, std::string *str);
      unsigned short FillGdheTrajectory(unsigned short nbpoints, float traj[][3], bool purge = true, double time_span = -1.0);
      void FillState(VEC const& pos, VEC const& vel, VEC const& acc, VEC & state);
      void FillState(time const& t, VEC & state);
      void FillStateVelocity(time const& t, VEC & state);

    private:
      void ComputeVelAndConstAcc(double const time, VEC const& a, VEC const& v_init, VEC const& p_init, VEC *vel, VEC *pos = NULL);
      VEC ComputeVelocityAtTimeT (AccProfileItem const& acc_t, AccProfileItem const& acc_t1, VEC const& v0, double T) const;
      VEC ComputePositionAtTimeT (AccProfileItem const& acc_t, AccProfileItem const& acc_t1, VEC const& v0, VEC const& x0, double T) const;
      void SetVarVECtor(SAMPLE_TYPE type, double x, double y, double z);
      void ToggleExtComputedFlag(bool val) { for (int i = 0; i < 4; i++) m_extval[i].computed = val; }
      void UpdateMinMax(ExtValStruct *pextval, VEC &accel, VEC &vel, VEC &pos);
    };

  } // namespace ddfsimu
} // namespace jafar
#endif