/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef INCLUDED_SystemUtility_h
#define INCLUDED_SystemUtility_h

#include "system_config.h"

#include <math.h>
#include <string.h>

#include "DogTypes.h"

#ifdef PLATFORM_APERIOS
#include <MCOOP.h>
#endif

#ifdef PLATFORM_APERIOS
inline char *strdup(char *src)
{
  char *dst;
  dst=new char[strlen(src)+1];
  strcpy(dst,src);
  return dst;
}
#endif

#ifdef PLATFORM_APERIOS
inline ulong GetTime() {
  static struct SystemTime time;
  
  GetSystemTime(&time);
  
  ulong seconds =time.seconds;
  ulong useconds=seconds*1000000+time.useconds;

  return useconds;
}
#else
#include <sys/time.h>

inline ulong GetTime() {
  static struct timeval time={0,0};
  static struct timezone tz={0,0};

  gettimeofday(&time,&tz);
  
  ulong seconds =time.tv_sec;
  ulong useconds=seconds*1000000+time.tv_usec;

  return useconds;
}
#endif

#ifdef PLATFORM_APERIOS
template<class T>
T *NewLarge(T **dst,int count) {
  sError result;

  // + 8096 is workaround for NewRegion rounding down amount of memory requested
  result = NewRegion(sizeof(T)*count+8096, reinterpret_cast<void **>(dst));
  if(result != sSUCCESS)
    *dst = NULL;

  return *dst;
}

template<class T>
void DeleteLarge(T *dst) {
  DeleteRegion(dst);
}
#else
template<class T>
T *NewLarge(T **dst,int count) {
  *dst = new T[count];

  return *dst;
}

template<class T>
void DeleteLarge(T *dst) {
  delete[] dst;
}
#endif

inline double round(double x) {
  x += .5;
  return floor(x);
}

inline void Die(unsigned short code) {
  unsigned long val = 0xDEAD0000UL | (unsigned long)code;
  *((long*)val)=1;
}

#endif