#include <iostream>
#include "behaviors.h"



#ifndef EFFECT_MAX_RANGE
    #define EFFECT_MAX_RANGE 2.0
#endif

using namespace std;

vec laserAvoid(LaserProxy *lp,double weight)
{
  vec totalVector(0,0),tempVector;
  totalVector.mag = totalVector.theta = 0;
  double fov = RTOD(lp->GetMaxAngle() - lp->GetMinAngle());
  double denominator = lp->GetCount() / fov;
  int i;

  cout <<"laserAvoid"<<endl;
  for(i=0;i<lp->GetCount();i++)
    {
      tempVector.mag = lp->GetRange(i);
      if(tempVector.mag <= EFFECT_MAX_RANGE)
	{
	  //tempVector.mag = (EFFECT_MAX_RANGE - tempVector.mag*tempVector.mag*.5)*weight/EFFECT_MAX_RANGE;
	  tempVector.mag = (EFFECT_MAX_RANGE/tempVector.mag -1)*weight;
	  tempVector.theta = (-90.0 + (((double)i)/2.0));
	  totalVector = reverse(tempVector)+totalVector;
	}
    }
  

  
  return totalVector;
}

vec sonarAvoid(SonarProxy *sp,double sonarAngles[],double weight)
{
  vec totalVector(0,0),tempVector;
  totalVector.mag = totalVector.theta = 0;
  int i;

  cout <<"sonarAvoid"<<endl;
  for(i = 9;i<15;i++)
    {
      tempVector.mag = sp->GetScan(i);
      if(tempVector.mag <= EFFECT_MAX_RANGE)
	{
	  tempVector.mag = (EFFECT_MAX_RANGE - tempVector.mag*tempVector.mag*.5)*weight/EFFECT_MAX_RANGE;
	  tempVector.theta = sonarAngles[i-9];
	  //tempVector.reverse();
	  totalVector = reverse(tempVector)+totalVector;
	}
      
    }
   
  return totalVector;
}

vec randomVector(double weight, int favoredSide)
{
  vec tempVector;

  cout <<"randomVector"<<endl;
  if(!favoredSide)
    {
      tempVector.theta = -90;
    }
  else 
    {
      tempVector.theta = 90;
    }
  
  tempVector.mag = ((rand()%100) +1) *weight/100;
  return tempVector;
}

vec wander(LaserProxy *lp, double weight)
{
  vec totalVector(0,0),tempVector;
  int i;

  cout <<"wander"<<endl;
  for(i=0;i<lp->GetCount();i++)
    {
      if(lp->GetRange(i) > EFFECT_MAX_RANGE)
	{
	  tempVector.theta = (-90.0 + (((double)i)/2.0));
	  tempVector.mag = weight;
	  totalVector = tempVector+totalVector;
	}
    }
  return totalVector;
}