/* 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_Utility_h
#define INCLUDED_Utility_h

#include <math.h>

#include "system_config.h"

#include "DogTypes.h"
#include "Geometry.h"

inline ulong SecToTime(double sec) {
  return (ulong)(sec*1.0e6);
}

inline double TimeToSec(ulong time) {
  return (time/1.0e6);
}

struct CylindricalCoordD{
  double angle,distance;
};

inline double deg2rad(double deg_angle) {
  return deg_angle*M_PI/180.0;
}

inline double rad2deg(double deg_angle) {
  return deg_angle*180.0/M_PI;
}

/*
// use norm_angle() in Util.h
inline double normAngle(double angle) {
  angle=fmod(angle,2*M_PI);
  if(angle < 0.0)
    angle+=2*M_PI;
  if(angle >= M_PI)
    angle-=2*M_PI;

  return angle;
}
*/

inline double
Distance(const vector2d &a, const vector2d &b) {
  double dx,dy;

  dx = a.x - b.x;
  dy = a.y - b.y;

  return sqrt(dx*dx + dy*dy);
}

inline vector2d
EgoToAllo(double theta,const vector2d &ego_vec) {
  return ego_vec.rotate(theta);
}

inline vector2d
AlloToWorld(const vector2d &our_loc, const vector2d &allo_vec) {
  return allo_vec+our_loc;
}

inline vector2d
WorldToAllo(const vector2d &our_loc, const vector2d &world_vec) {
  return world_vec-our_loc;
}

inline vector2d
AlloToEgo(double theta,const vector2d &allo_vec) {
  return allo_vec.rotate(-theta);
}

template <class coord>
vector2d ToVector(coord p)
{
  double x,y;

  x = cos(p.angle) * p.distance;
  y = sin(p.angle) * p.distance;

  return(vector2d(x,y));
}

inline
CylindricalCoordD ToCylindrical(vector2d v)
{
  CylindricalCoordD p;

  p.distance = v.length();
  p.angle = atan2(v.y,v.x);

  return(p);
}

inline double
ramp_up(double in, double zero_end, double one_start) {
  double out=0.0;

  if(in > one_start)
    out=1.0;
  else if(in < zero_end)
    out=0.0;
  else
    out=(in-zero_end)/(one_start-zero_end);
  
  return out;
}

inline double
ramp_dn(double in, double one_end, double zero_start) {
  double out=0.0;

  if(in > zero_start)
    return 0.0;
  else if(in < one_end)
    return 1.0;
  else
    out=(zero_start-in)/(zero_start-one_end);

  return out;
}

template <class num>
num square(num n)
{
  return(n * n);
}

#include "SystemUtility.h"

#endif