/**
 * Some simple vision behaviors
 */


#ifndef VISION_BEHAVIORS
#define VISION_BEHAVIORS

#include "behavior/behavior.hh"
#include "behavior/linktypes.hh"
#include "input/eyebot_sensors.hh"
#include "output/eyebot_actors.hh"
#include "eyebot_behavior_types.h"


/* Some constants and macros related to pixels on the image */
#define ALLPIXELS imagerows*imagecolumns*3
#define RED 0
#define GREEN 1
#define BLUE 2


/* macros for referencing a pixels in relation to the current pixel
   eg: NORTH(k) is the memory address within the image BYTE array for the
       pixel above pixel k
*/
//// Use inline functions if you really need it.
#define SOUTH(x) ((x)+(imagecolumns*3))
#define NORTH(x) ((x)-(imagecolumns*3))
#define EAST(x) ((x)-3)
#define WEST(x) ((x)+3)

#define div(x,y) (x/y)

namespace Soccer03 {

using namespace EyeMind;

//// didn't you mean unsigned?
static const int lookup_range[19] = {164,108,85,71,62,56,51,47,44,41,39,37,35,33,32,31,30,29,28};

/**
 * A behavior to take a picture with the camera.
 * NOTE: NO PROCESSING IS DONE ON THE IMAGE.
 * TO PROCESS THE IMAGE, DEFINE ANOTHER BEHAVIOR.
 */
class Look : public Behavior
{
private:
	Camera* camera;

public:
	Look()
	{
		camera = &Camera::I();
	}

	int Execute()
	{
		camera->Update();
		return 1;
	}

	colimage* Image()
	{
		if(camera!=NULL)
		{
			return camera->Image();
		}
		else
			return NULL;
	}
};


/// Connection functions
inline ImageLink& operator>>(Look& n, ImageLink& o)
{
	o.Image(n.Image());
	BehaviorOutputToSignalLink(n,o);
	return o;
}


/**
 * A behavior to put an image on the screen
 */
class ShowImage : public Behavior
{
private:
	colimage* buffer;

public:
	ShowImage()
	{}

	int Execute()
	{
		ImageLink *l = (ImageLink*) inlinks.Begin();
		if(l!=NULL)
		{
			LCDPutColorGraphic(l->Image());
		}
		return 0;
	}

	void Image(colimage* buf){buffer=buf;}
};

inline ShowImage& operator>>(ImageLink& l, ShowImage& o)
{
	o.Image(l.Image());
	SignalLinkOutputToBehavior(l,o);
	return o;
}
inline ImageLink& operator<<(ShowImage& n, ImageLink& i)
{
	n.Image(i.Image());
	BehaviorInputToSignalLink(n,i);
	return i;
}



/**
 * Filter the image
 */

class FindBall : public Behavior
{
private:
	BallType ball;
	int ball_found;
	colimage* buffer;

	int hue;
	int threshold;
	int intensity_lo;
	int intensity_hi;
	int K;
	int intensity_thresh;
	int req_matches;

	int image_to_polar(int x, int y);
	int RGBtoHue(BYTE r, BYTE g, BYTE b);
	int CheckHue(BYTE r, BYTE g, BYTE b);
	int CheckIntensity(BYTE r, BYTE g, BYTE b);
	
public:
 	FindBall();

	int Execute();

	void Image(colimage* buf){buffer=buf;}
	
	void Feed(SignalLink &l);
};

inline FindBall& operator>>(ImageLink& l, FindBall& o)
{
	o.Image(l.Image());
	SignalLinkOutputToBehavior(l,o);
	return o;
}
inline ImageLink& operator<<(FindBall& n, ImageLink& i)
{
	n.Image(i.Image());
	BehaviorInputToSignalLink(n,i);
	return i;
}


inline FindBall& operator<<(BallLink& l, FindBall& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}
inline BallLink& operator>>(FindBall& n, BallLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

//// Not that important, but really nice: use #if 0 ... #endif instead of
//// metre-long comment chains for deactivating code
/*
class SearchBall : public Behavior
{
 private:
  Servo* servo;
  //FindBall* fb;

 public:
  SearchBall(Servo* s)
    {
      servo = s;
      //fb = b;
    }

  int Execute()
  {
    int a;

    //if(!fb->FoundBall())
    //  {
    a = servo->Angle();
    a+=5;
    if(a>255)
      a = 0;

    servo->Angle(a);
    servo->Update();
    //}
    return 1;
  }
};*/

}; // namespace Soccer03


#endif