#ifndef _TOOLS_H
#define _TOOLS_H

#include <stdlib.h>
#include <math.h>

#ifndef new
	void* operator new(size_t sz);
	void operator delete(void* p);
#endif

#define max(a, b) (((a) > (b)) ? (a) : (b))
#define min(a, b) (((a) < (b)) ? (a) : (b))
#define sign(a) (((a) < 0) ? (-1) : (1))

inline void sleep(int sec)
{
  for (int s=0; s<sec; s++) {
    for (long int i=0; i<1405678; i++) {
      asm volatile("nop"); 
    }
  }
};

inline void msleep(int msec)
{
  for (int s=0; s<msec; s++) {
    for (long int i=0; i<1405; i++) {
      asm volatile("nop"); 
    }
  }
};

inline void usleep(int usec)
{
	usec *= 100;
  for (int s=0; s<usec; s++) {
    for (long int i=0; i<1405; i++) {
      asm volatile("nop"); 
    }
  }
};

inline float distance2d(float x1, float y1, float x2, float y2)
{
	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
}

inline float direction2d(float x1, float y1, float x2, float y2)
{
	return atan((y2 - y1) / (x2 - x1));
}

#endif
