ariaUtil.h

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#ifndef ARIAUTIL_H
#define ARIAUTIL_H

#include <string>
#include <list>
#include <math.h>
#include <stdarg.h>
#ifdef WIN32
#include <sys/timeb.h>
#include <sys/stat.h>
#endif // win32
#ifndef WIN32
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <unistd.h>
#endif // ifndef win32
#include "ariaTypedefs.h"
#include "ArLog.h"
#include "ArFunctor.h"
#include "ArArgumentParser.h"
//#include "ariaInternal.h"
#include "ariaOSDef.h"

#ifndef M_PI
#define M_PI 3.1415927
#endif // of M_PI, windows has a function call instead of a define

class ArUtil
{
public:
  enum BITS { 
    BIT0 = 0x1, 
    BIT1 = 0x2, 
    BIT2 = 0x4, 
    BIT3 = 0x8, 
    BIT4 = 0x10, 
    BIT5 = 0x20, 
    BIT6 = 0x40, 
    BIT7 = 0x80, 
    BIT8 = 0x100, 
    BIT9 = 0x200, 
    BIT10 = 0x400, 
    BIT11 = 0x800, 
    BIT12 = 0x1000, 
    BIT13 = 0x2000, 
    BIT14 = 0x4000, 
    BIT15 = 0x8000, 
  };

  static void sleep(unsigned int ms);
  
  static unsigned int getTime(void);


  template<class T> static void deleteSet(T begin, T end)
    {
      for (; begin != end; ++begin)
      {
        delete (*begin);
      }
    }


  template<class T> static void deleteSetPairs(T begin, T end)
    {
      for (; begin != end; ++begin)
      {
        delete (*begin).second;
      }
    }

  static int findMin(int first, int second) 
    { if (first < second) return first; else return second; }
  static int findMax(int first, int second) 
    { if (first > second) return first; else return second; }

  static double findMin(double first, double second) 
    { if (first < second) return first; else return second; }
  static double findMax(double first, double second) 
    { if (first > second) return first; else return second; }

  static long sizeFile(const char *fileName);

  static long sizeFile(std::string fileName);

  static bool findFile(const char *fileName);

  // OS-independent way of stripping the directory from the fileName.
  // commented out with std::string changes since this didn't seem worth fixing right now
  //static bool stripDir(std::string fileIn, std::string &fileOut);

  // OS-independent way of stripping the fileName from the directory.
  // commented out with std::string changes since this didn't seem worth fixing right now
  //static bool stripFile(std::string fileIn, std::string &fileOut);

  static void appendSlash(char *path, size_t pathLength);

  static void fixSlashes(char *path, size_t pathLength);

  static void fixSlashesForward(char *path, size_t pathLength);

  static void fixSlashesBackward(char *path, size_t pathLength);

  static void addDirectories(char *dest, size_t destLength,
                                      const char *baseDir, 
                                      const char *insideDir);

  static int strcmp(std::string str, std::string str2);

  static int strcmp(std::string str, const char *str2);

  static int strcmp(const char *str, std::string str2);

  static int strcmp(const char *str, const char *str2);

  static int strcasecmp(std::string str, std::string str2);

  static int strcasecmp(std::string str, const char *str2);

  static int strcasecmp(const char *str, std::string str2);

  static int strcasecmp(const char *str, const char *str2);

  static void escapeSpaces(char *dest, const char *src, 
                                    size_t maxLen);

  static bool stripQuotes(char *dest, const char *src,size_t destLen);

  static void lower(char *dest, const char *src, 
                             size_t maxLen);
  static bool isOnlyAlphaNumeric(const char *str);

  static double atof(const char *nptr);

  static const char *convertBool(int val);

  static void functorPrintf(ArFunctor1<const char *> *functor,
                                     char *str, ...);

  static void writeToFile(const char *str, FILE *file);

  static bool getStringFromFile(const char *fileName, 
                                         char *str, size_t strLen);
  enum REGKEY {
    REGKEY_CLASSES_ROOT, 
    REGKEY_CURRENT_CONFIG, 
    REGKEY_CURRENT_USER, 
    REGKEY_LOCAL_MACHINE, 
    REGKEY_USERS 
  };

  static bool getStringFromRegistry(REGKEY root,
                                             const char *key,
                                             const char *value,
                                             char *str,
                                             int len);

  static bool findFirstStringInRegistry(const char* key, const char* value, char* str, int len) {
        if(!getStringFromRegistry(REGKEY_CURRENT_USER, key, value, str, len))
                return getStringFromRegistry(REGKEY_LOCAL_MACHINE, key, value, str, len);
        return true;
  }

  static const char *COM1; 
  static const char *COM2; 
  static const char *COM3; 
  static const char *COM4; 
  static const char *TRUESTRING; 
  static const char *FALSESTRING; 
  
};

class ArMath
{
public:


  static double addAngle(double ang1, double ang2) 
    { return fixAngle(ang1 + ang2); }


  static double subAngle(double ang1, double ang2) 
    { return fixAngle(ang1 - ang2); }


  static double fixAngle(double angle) 
    {
      if (angle >= 360)
        angle = angle - 360.0 * (double)((int)angle / 360);
      if (angle < -360)
        angle = angle + 360.0 * (double)((int)angle / -360);
      if (angle <= -180)
        angle = + 180.0 + (angle + 180.0);
      if (angle > 180)
        angle = - 180.0 + (angle - 180.0);
      return angle;
    } 
  

  static double degToRad(double deg) { return deg * M_PI / 180.0; }


  static double radToDeg(double rad) { return rad * 180.0 / M_PI; }


  static double cos(double angle) { return ::cos(ArMath::degToRad(angle)); }


  static double sin(double angle) { return ::sin(ArMath::degToRad(angle)); }


  static double atan2(double y, double x) 
    { return ArMath::radToDeg(::atan2(y, x)); }
  static bool angleBetween(double angle, double startAngle, double endAngle)
    {
      angle = fixAngle(angle);
      startAngle = fixAngle(startAngle);
      endAngle = fixAngle(endAngle);
      if ((startAngle < endAngle && angle > startAngle && angle < endAngle) ||
          (startAngle > endAngle && (angle > startAngle || angle < endAngle)))
        return true;
      else
        return false;
    }


  static double fabs(double val) 
    {
      if (val < 0.0)
        return -val;
      else
        return val;
    }


  static int roundInt(double val) 
    { 
      val += .49;
      if (val > INT_MAX)
        return (int) INT_MAX;
      else if (val < INT_MIN)
        return (int) INT_MIN;
      else
        return((int) floor(val)); 
    }
    

  static short roundShort(double val) 
    { 
      val += .49;
      if (val > 32767)
        return (short) 32767;
      else if (val < -32768)
        return (short) -32768;
      else
        return((short) floor(val)); 
    }
    

  static void pointRotate(double *x, double *y, double th)
    {
      double cs, sn, xt, yt;
      cs = cos(th);
      sn = sin(th);
      xt = *x;  
      yt = *y;
      *x = cs*xt + sn*yt;
      *y = cs*yt - sn*xt;
    }
  
  static long random(void)
    {
#ifdef WIN32
      return(rand());
#else
      return(lrand48());
#endif
    }

  static double distanceBetween(double x1, double y1, double x2, double y2)
    { return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));  }


  static double squaredDistanceBetween(double x1, double y1, double x2, double y2)
    { return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);  }

  static double log2(double x)
  {
#ifdef WIN32
    return log10(x) / 0.3010303;  // that's log10(2.0);
#else
    return log2(x);
#endif
  }
};


class ArPose
{
public:

  ArPose(double x = 0, double y = 0, double th = 0)
    { myX = x; myY = y; myTh = th; }
  ArPose(const ArPose &pose) : 
    myX(pose.myX), myY(pose.myY), myTh(pose.myTh) {}

  virtual ~ArPose() {}

  virtual void setPose(double x, double y, double th = 0) 
    { setX(x); setY(y); setTh(th); }

  virtual void setPose(ArPose position)
    {
      setX(position.getX());
      setY(position.getY());
      setTh(position.getTh());
    }
  void setX(double x) { myX = x; }
  void setY(double y) { myY = y; }
  void setTh(double th) { myTh = ArMath::fixAngle(th); }
  void setThRad(double th) { myTh = ArMath::fixAngle(ArMath::radToDeg(th)); }
  double getX(void) const { return myX; }
  double getY(void) const { return myY; }
  double getTh(void) const { return myTh; }
  double getThRad(void) const { return ArMath::degToRad(myTh); }

  void getPose(double *x, double *y, double *th = NULL) const
    { 
      if (x != NULL) 
        *x = myX;
      if (y != NULL) 
        *y = myY; 
      if (th != NULL) 
        *th = myTh; 
    }

  virtual double findDistanceTo(ArPose position) const
    {
      return ArMath::distanceBetween(getX(), getY(), 
                                     position.getX(), 
                                     position.getY());
    }


  virtual double squaredFindDistanceTo(ArPose position) const
    {
      return ArMath::squaredDistanceBetween(getX(), getY(), 
                                            position.getX(), 
                                            position.getY());
    }

  virtual double findAngleTo(ArPose position) const
    {
      return ArMath::radToDeg(atan2(position.getY() - getY(),
                                    position.getX() - getX()));
    }
  virtual void log(void) const
    { ArLog::log(ArLog::Terse, "%.0f %.0f %.1f", myX, myY, myTh); }
protected:
  double myX;
  double myY;
  double myTh;
};



class ArTime
{
public:
  ArTime() { setToNow(); }
  ~ArTime() {}
  
  long mSecSince(ArTime since) const 
    {
      long timeSince, timeThis;

      timeSince = since.getSec() * 1000 + since.getMSec();
      timeThis = mySec * 1000 + myMSec;
      return timeSince - timeThis;
    }
  long secSince(ArTime since) const
    {
      return mSecSince(since)/1000;
    }
  long mSecTo(void) const
    {
      ArTime now;
      now.setToNow();
      return -mSecSince(now);
    }
  long secTo(void) const
    {
      return mSecTo()/1000;
    }
  long mSecSince(void) const
    {
      ArTime now;
      now.setToNow();
      return mSecSince(now);
    }
  long secSince(void) const
    {
      return mSecSince()/1000;
    }
  bool isBefore(ArTime testTime) const
    {
      if (mSecSince(testTime) < 0)
        return true;
      else
        return false;
    }
  bool isAt(ArTime testTime) const
    {
      if (mSecSince(testTime) == 0)
        return true;
      else
        return false;
    }
  bool isAfter(ArTime testTime) const
    {
      if (mSecSince(testTime) > 0)
        return true;
      else
        return false;
    }
  void setToNow(void)
    {
#ifdef WIN32
      /* this should be the better way, but it doesn't really work...
         this would be seconds from 1970, but it is based on the
         hardware timer or something and so winds up not being updated
         all the time and winds up being some number of ms < 20 ms off
      struct _timeb startTime;
      _ftime(&startTime);
      mySec = startTime.time;
      myMSec = startTime.millitm;*/
      // so we're going with just their normal function, msec since boot
      long timeNow;
      timeNow = timeGetTime();
      mySec = timeNow / 1000;
      myMSec = timeNow % 1000;
#else // if not win32
      struct timeval timeNow;
      
      if (gettimeofday(&timeNow, NULL) == 0)
      {
        mySec = timeNow.tv_sec;
        myMSec = timeNow.tv_usec / 1000;
      }
      else
        ArLog::log(ArLog::Terse, "ArTime::setToNow: invalid return from gettimeofday.\n");
#endif //linux
    }
  void addMSec(long ms)
    {
      unsigned long timeThis;
      timeThis = mySec * 1000 + myMSec;
      if (ms < 0 && (unsigned)abs(ms) > timeThis)
      {
        ArLog::log(ArLog::Terse, "ArTime::addMsec: tried to subtract too many milliseconds, would result in a negative time.");
        mySec = 0;
        myMSec = 0;
      }
      else 
      {
        timeThis += ms;
        mySec = timeThis / 1000;
        myMSec = timeThis % 1000;
      }
    }
  void setSec(time_t sec) { mySec = sec; }
  void setMSec(time_t msec) { myMSec = msec; }
  time_t getSec(void) const { return mySec; }
  time_t getMSec(void) const { return myMSec; }
  void log(void) const
    { ArLog::log(ArLog::Terse, "Time: %ld.%ld", getSec(), getMSec()); }
protected:
  time_t mySec;
  time_t myMSec;

};


class ArPoseWithTime : public ArPose
{
public:
  ArPoseWithTime(double x = 0, double y = 0, double th = 0,
         ArTime thisTime = ArTime()) : ArPose(x, y, th)
    { myTime = thisTime; }
  virtual ~ArPoseWithTime() {}
  void setTime(ArTime newTime) { myTime = newTime; }
  void setTimeToNow(void) { myTime.setToNow(); }
  ArTime getTime(void) const { return myTime; }
protected:
  ArTime myTime;
};


class ArSectors
{
public:
  ArSectors(int numSectors = 8) 
    { 
      mySectorSize = 360/numSectors;
      mySectors = new int[numSectors]; 
      myNumSectors = numSectors; 
      clear();
    }
  virtual ~ArSectors() { delete mySectors; }
  void clear(void) 
    {
      int i;
      for (i = 0; i < myNumSectors; i++)
        mySectors[i] = false;
    }
  void update(double angle)
    {
      int angleInt;
      angleInt = ArMath::roundInt(ArMath::fixAngle(angle) + 180);
      mySectors[angleInt / mySectorSize] = true;
    }
  bool didAll(void) const
    {
      int i;
      for (i = 0; i < myNumSectors; i++)
        if (mySectors[i] == false)
          return false;
      return true;
    }
protected:
  int *mySectors;
  int myNumSectors;
  int mySectorSize;
};





class ArLine
{
public:
  ArLine() {}
  ArLine(double a, double b, double c) { newParameters(a, b, c); }
  ArLine(double x1, double y1, double x2, double y2) 
  { newParametersFromEndpoints(x1, y1, x2, y2); }
  virtual ~ArLine() {}
  void newParameters(double a, double b, double c) 
    { myA = a; myB = b; myC = c; }
  void newParametersFromEndpoints(double x1, double y1, double x2, double y2)
    { myA = y1 - y2; myB = x2 - x1; myC = (y2 *x1) - (x2 * y1); }
  const double getA(void) const { return myA; }
  const double getB(void) const { return myB; }
  const double getC(void) const { return myC; }

  bool intersects(const ArLine *line, ArPose *pose) 
    {
      double x, y;
      double n;
      n = (line->getB() * getA()) - (line->getA() * getB());
      // if this is 0 the lines are parallel
      if (fabs(n) < .0000000000001)
      {
        return false;
      }
      // they weren't parallel so see where the intersection is
      x = ((line->getC() * getB()) - (line->getB() * getC())) / n;
      y = ((getC() * line->getA()) - (getA() * line->getC())) / n;
      pose->setPose(x, y);
      return true;
    }
  void makeLinePerp(const ArPose *pose, ArLine *line) const
    {
      line->newParameters(getB(), -getA(), 
                          (getA() * pose->getY()) - (getB() * pose->getX()));
    }
protected:
  double myA, myB, myC;
};

class ArLineSegment
{
public:
#ifndef SWIG

  ArLineSegment() {}
  ArLineSegment(double x1, double y1, double x2, double y2)
    {   newEndPoints(x1, y1, x2, y2); }
  ArLineSegment(ArPose pose1, ArPose pose2)
    {   newEndPoints(pose1.getX(), pose1.getY(), pose2.getX(), pose2.getY()); }
#endif // SWIG

  virtual ~ArLineSegment() {}
  void newEndPoints(double x1, double y1, double x2, double y2)
    {
      myX1 = x1; myY1 = y1; myX2 = x2; myY2 = y2; 
      myLine.newParametersFromEndpoints(myX1, myY1, myX2, myY2);
    }
  ArPose getEndPoint1(void) const { return ArPose(myX1, myY1); }
  ArPose getEndPoint2(void) const { return ArPose(myX2, myY2); }

  bool intersects(const ArLine *line, ArPose *pose) 
    {
      // see if it intersects, then make sure its in the coords of this line
      if (myLine.intersects(line, &myIntersection) &&
          linePointIsInSegment(&myIntersection))
      {
        pose->setPose(myIntersection);
        return true;
      }
      else
        return false;
    }


  bool intersects(ArLineSegment *line, ArPose *pose) 
    {
      // see if it intersects, then make sure its in the coords of this line
      if (myLine.intersects(line->getLine(), &myIntersection) &&
          linePointIsInSegment(&myIntersection) &&
          line->linePointIsInSegment(&myIntersection))
      {
        pose->setPose(myIntersection);
        return true;
      }
      else
        return false;
    }
#ifndef SWIG


  bool getPerpPoint(ArPose pose, ArPose *perpPoint) 
    {
      myLine.makeLinePerp(&pose, &myPerpLine);
      return intersects(&myPerpLine, perpPoint);
    }
#endif


  bool getPerpPoint(const ArPose *pose, ArPose *perpPoint) 
    {
      myLine.makeLinePerp(pose, &myPerpLine);
      return intersects(&myPerpLine, perpPoint);
    }

  double getPerpDist(const ArPose pose)
    {
      ArPose perpPose;
      myLine.makeLinePerp(&pose, &myPerpLine);
      if (!intersects(&myPerpLine, &perpPose))
        return -1;
      return (perpPose.findDistanceTo(pose));
    }

  double getDistToLine(const ArPose pose)
    {
      ArPose perpPose;
      myLine.makeLinePerp(&pose, &myPerpLine);
      if (!intersects(&myPerpLine, &perpPose))
      {
        return ArUtil::findMin(
                ArMath::roundInt(getEndPoint1().findDistanceTo(pose)),
                ArMath::roundInt(getEndPoint2().findDistanceTo(pose)));
      }
      return (perpPose.findDistanceTo(pose));
    }
  const double getX1(void) const { return myX1; }
  const double getY1(void) const { return myY1; } 
  const double getX2(void) const { return myX2; }
  const double getY2(void) const { return myY2; }
  const double getA(void) const { return myLine.getA(); }
  const double getB(void) const { return myLine.getB(); }
  const double getC(void) const { return myLine.getC(); }
  const bool linePointIsInSegment(ArPose *pose) const
    {
      return (((myX1 == myX2) || 
               (pose->getX() >= myX1 && pose->getX() <= myX2) || 
               (pose->getX() <= myX1 && pose->getX() >= myX2)) &&
              ((myY1 == myY2) || 
               (pose->getY() >= myY1 && pose->getY() <= myY2) || 
               (pose->getY() <= myY1 && pose->getY() >= myY2)));
    }
  const ArLine *getLine(void) const { return &myLine; }
protected:
  double myX1, myY1, myX2, myY2;
  ArLine myLine;
  ArPose myIntersection;
  ArLine myPerpLine;
};

class ArRunningAverage
{
public:
  ArRunningAverage(size_t numToAverage);
  ~ArRunningAverage();
  double getAverage(void) const;
  void add(double val);
  void clear(void);
  size_t getNumToAverage(void) const;
  void setNumToAverage(size_t numToAverage);
protected:
  size_t myNumToAverage;
  double myTotal;
  size_t myNum;
  std::list<double> myVals;
};


//class ArStrCaseCmpOp :  public std::binary_function <const std::string&, const std::string&, bool> 
struct ArStrCaseCmpOp 
{
public:
  bool operator() (const std::string &s1, const std::string &s2) const
  {
    return strcasecmp(s1.c_str(), s2.c_str()) < 0;
  }
};

#if !defined(WIN32) && !defined(SWIG)

class ArDaemonizer
{
public:
  ArDaemonizer(int *argc, char **argv);
  ~ArDaemonizer();
  bool daemonize(void);
  bool forceDaemonize(void);
  void logOptions(void);
  bool isDaemonized(void) { return myIsDaemonized; }
protected:
  ArArgumentParser myParser;
  bool myIsDaemonized;
};
#endif // !win32 && !swig



class ArPriority
{
public:
  enum Priority 
  {
    IMPORTANT, 
    NORMAL, 
    DETAILED, 
    TRIVIAL = DETAILED, 
    
    LAST_PRIORITY = DETAILED 
  };

  enum {
    PRIORITY_COUNT = LAST_PRIORITY + 1 
  };

  static const char * getPriorityName(Priority priority);
protected:
  static bool ourStringsInited;
  static std::map<Priority, std::string> ourPriorityNames;
  static std::string ourUnknownPriorityName;
};


class ArStringInfoHolder
{
public:
  ArStringInfoHolder(const char *name, ArTypes::UByte2 maxLength, 
                     ArFunctor2<char *, ArTypes::UByte2> *functor)
    { myName = name; myMaxLength = maxLength; myFunctor = functor; }
  virtual ~ArStringInfoHolder() {}
  const char *getName(void) { return myName.c_str(); }
  ArTypes::UByte2 getMaxLength(void) { return myMaxLength; }
  ArFunctor2<char *, ArTypes::UByte2> *getFunctor(void) { return myFunctor; }
protected:
  std::string myName;
  ArTypes::UByte2 myMaxLength;
  ArFunctor2<char *, ArTypes::UByte2> *myFunctor;
};

class ArStringInfoHolderFunctions
{
public:
  static void intWrapper(char * buffer, ArTypes::UByte2 bufferLen, 
                         ArRetFunctor<int> *functor, const char *format)
    { snprintf(buffer, bufferLen, format, functor->invokeR()); }
  static void doubleWrapper(char * buffer, ArTypes::UByte2 bufferLen, 
                            ArRetFunctor<double> *functor, const char *format)
    { snprintf(buffer, bufferLen, format, functor->invokeR()); }
  static void boolWrapper(char * buffer, ArTypes::UByte2 bufferLen, 
                          ArRetFunctor<bool> *functor, const char *format)
    { snprintf(buffer, bufferLen, format, 
               ArUtil::convertBool(functor->invokeR())); }
  static void stringWrapper(char * buffer, ArTypes::UByte2 bufferLen, 
                            ArRetFunctor<const char *> *functor, 
                            const char *format)
    { snprintf(buffer, bufferLen, format, functor->invokeR()); }

};

#endif // ARIAUTIL_H

---