//## begin module%1.4%.codegen_version preserve=yes
//   Read the documentation to learn more about C++ code generator
//   versioning.
//## end module%1.4%.codegen_version

//## begin module%3D86D3570320.cm preserve=no
//	  %X% %Q% %Z% %W%
//## end module%3D86D3570320.cm

//## begin module%3D86D3570320.cp preserve=no
//## end module%3D86D3570320.cp

//## Module: SIR%3D86D3570320; Pseudo Package body
//## Source file: C:\Program Files\Rational\Rose\C++\source\SIR.cpp

//## begin module%3D86D3570320.additionalIncludes preserve=no
//## end module%3D86D3570320.additionalIncludes

//## begin module%3D86D3570320.includes preserve=yes
#include <stdlib.h>
#include <math.h>
#include "Version.h"
//## end module%3D86D3570320.includes

// SIR
#include "SIR.h"
//## begin module%3D86D3570320.additionalDeclarations preserve=yes
//## end module%3D86D3570320.additionalDeclarations


// Class SIR

SIR SIR::sirs[NUMBER_OF_ROBOTS];

//tables diff. sim. - physical robots
const double SIR::row2Meter[] =
  {1.000,                                                                          //border of image, not used
   1.0000, 1.0000, 1.0000, 1.0000, 1.0000,    1.0000, 0.9000, 0.9000, 0.9000, 0.8000, //rows 1
   0.8000, 0.7500, 0.7000, 0.6500, 0.6000,    0.5500, 0.5250, 0.5000, 0.4830, 0.4670,
   0.8000, 0.8000, 0.8000, 0.8000, 0.8000,    0.8000, 0.8000, 0.7300, 0.6600, 0.6200, //to 30 not used, too inaccurate
   0.5800, 0.5400, 0.5000, 0.4700, 0.4400,    0.4100, 0.3900, 0.3700, 0.3500, 0.3300,
   0.3050, 0.2900, 0.2750, 0.2550, 0.2400,    0.2300, 0.2200, 0.2150, 0.2100, 0.2000,
   0.1950, 0.1900, 0.1800, 0.1700, 0.1600,    0.1550, 0.1500, 0.1400, 0.1350, 0.0650,
   0.065}; //border of image, not used

const double SIR::yFactor[] =
  {0.0200,                                                                         //border of image, not used
   0.0200, 0.0200, 0.0200, 0.0200, 0.0200,    0.0200, 0.0200, 0.0200, 0.0200, 0.0200, //rows 1
   0.0180, 0.0140, 0.0105, 0.0095, 0.0090,    0.0085, 0.0080, 0.0073, 0.0069, 0.0067,
   0.0095, 0.0095, 0.0095, 0.0095, 0.0095,    0.0075, 0.0075, 0.0075, 0.0075, 0.0075, //to 30 not used, too inaccurate
   0.0055, 0.0039, 0.0039, 0.0037, 0.0036,    0.0036, 0.0035, 0.0034, 0.0034, 0.0033,
   0.0033, 0.0032, 0.0030, 0.0028, 0.0026,    0.0025, 0.0026, 0.0027, 0.0028, 0.0029,
   0.0030, 0.0029, 0.0028, 0.0026, 0.0023,    0.0022, 0.0020, 0.0018, 0.0016, 0.0015,
   0.0100};                                                                        //border of image, not used

const double SIR::yFactorMinus[] =
  {0.0100,                                                                         //border of image, not used
   0.0100, 0.0100, 0.0100, 0.0100, 0.0100,    0.0100, 0.0100, 0.0100, 0.0100, 0.0100, //rows 1
   0.0059, 0.0055, 0.0053, 0.0047, 0.0045,    0.0042, 0.0040, 0.0036, 0.0034, 0.0033,
   0.0015, 0.0015, 0.0014, 0.0014, 0.0013,    0.0013, 0.0013, 0.0013, 0.0013, 0.0013, //to 30 not used, too inaccurate
   0.0026, 0.0025, 0.0024, 0.0023, 0.0022,    0.0021, 0.0019, 0.0018, 0.0017, 0.0016,
   0.0015, 0.0014, 0.0013, 0.0012, 0.0011,    0.0011, 0.0010, 0.0011, 0.0010, 0.0010,
   0.0010, 0.0010, 0.0010, 0.0010, 0.0010,    0.0010, 0.0010, 0.0009, 0.0008, 0.0007,
   0.0010};                                                                        //border of image, not used


SIR::SIR ()
      : robotDetectedFlag(false),
        cubeDetectedFlag(false),
        minimumCubeSize(0.03),
#ifndef EYESIM_CUBECLUSTERING_PARAMETERS
        cubeColor(65),
        allowedCubeColorDeviation(15),
        obstacleThresholdSides(0.15)
#else
        cubeColor(42),
        allowedCubeColorDeviation(0),
        obstacleThresholdSides(0.25)
#endif
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
  ,obstacleThresholdFront(0.175)
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
  ,obstacleThresholdFront(0.11)
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
  ,obstacleThresholdFront(0.11)
#endif
  ,Thread("sir", 256*1024, MAX_PRI, 0)
{
	cubePositionX = 0.0;
	cubePositionY = 0.0;
}

void SIR::initialize(void)
{
  mes[OSMachineID()] = this;
	camera = Camera::getCamera();
	psds = PSDs::getPSDs();
  drive = Drive::getDrive();
}

//## Other Operations (implementation)
bool SIR::obstacleLeft ()
{
  //## begin SIR::obstacleLeft%3D9296DA015E.body preserve=yes
  return (getLeftMargin()<obstacleThresholdSides?true:false);
  //## end SIR::obstacleLeft%3D9296DA015E.body
}

bool SIR::obstacleRight ()
{
  //## begin SIR::obstacleRight%3D9296F8000A.body preserve=yes
  return (getRightMargin()<obstacleThresholdSides?true:false);
  //## end SIR::obstacleRight%3D9296F8000A.body
}

bool SIR::obstacleFront ()
{
  //## begin SIR::obstacleFront%3D9296F801C2.body preserve=yes
  return (getFrontMargin()<obstacleThresholdFront?true:false);
  //## end SIR::obstacleFront%3D9296F801C2.body
}

double SIR::getLeftMargin ()
{
  //## begin SIR::getLeftMargin%3D99481C028A.body preserve=yes
  return psds->readLeft();
  //## end SIR::getLeftMargin%3D99481C028A.body
}

double SIR::getRightMargin ()
{
  //## begin SIR::getRightMargin%3D9948230082.body preserve=yes
  return psds->readRight();
  //## end SIR::getRightMargin%3D9948230082.body
}

double SIR::getFrontMargin ()
{
  //## begin SIR::getFrontMargin%3D99482B0096.body preserve=yes
  return psds->readFront();
  //## end SIR::getFrontMargin%3D99482B0096.body
}

bool SIR::robotDetected ()
{
  //## begin SIR::robotDetected%3D99453C024E.body preserve=yes
  return robotDetectedFlag;
  //## end SIR::robotDetected%3D99453C024E.body
}

bool SIR::cubeDetected ()
{
  //## begin SIR::cubeDetected%3D994557000A.body preserve=yes
  return cubeDetectedFlag;
  //## end SIR::cubeDetected%3D994557000A.body
}

void SIR::takePicture ()
{
  //## begin SIR::takePicture%3D9948FB03D4.body preserve=yes
	camera->getFrame(currentImage);
  //## end SIR::takePicture%3D9948FB03D4.body
}

void SIR::takeColorPicture ()
{
  //## begin SIR::takeColorPicture%3D9FC00D014A.body preserve=yes
	camera->getColorFrame(currentColorImage);
  //## end SIR::takeColorPicture%3D9FC00D014A.body
}

bool SIR::detectCubes ()
{
  //## begin SIR::detectCubes%3D99479F0168.body preserve=yes
  image hues; //to store the hues of the currentColorImage

  takeColorPicture();

  double robotPositionXAtPictureTime, robotPositionYAtPictureTime, phiAtPictureTime;
  drive->getPosition(robotPositionXAtPictureTime, robotPositionYAtPictureTime, phiAtPictureTime);



  //used to set up tables (row2Meter, yFactor, yFactorMinus)
  //also uncomment code below
  /*for (int row = 1; row <=imagerows-2; row++)
      imageFilter.calculateHue(currentColorImage, row, hues);
    for (int row = 1; row <=imagerows-2; row++)
      for (int column = 1; column <=imagecolumns-2; column++)
        if (hasCubeColor(hues[row][column]))
          hues[row][column] = 30;
        else
          hues[row][column] = 255;
  LCDPutGraphic(&hues);*/
  //LCDPutColorGraphic(&currentColorImage);

  int row = imagerows-2; //start at the bottom row
  while(row>=31) //rows 1 to 30 are not considered, the results are too inaccurate //diff. sim. - physical robots
  {

  	//the desired cube size in pixel is calculated
		//(it depends on minimum cube size in meter and distance to robot)
		int minimumSize = 0;
    if (yFactor[row] != 0.0)
    {
      minimumSize = (int) (minimumCubeSize / yFactor[row]);
      if (minimumSize > imagecolumns - 2)
        minimumSize = imagecolumns - 2;
    }
    else
      minimumSize = imagecolumns - 2;

    if (minimumSize < 8) //enough?? //Note: minumumSize must be >= 1, see code below (first+=minimumSize)
    	minimumSize = 8;

    imageFilter.calculateHue(currentColorImage, row, hues);

    int first = 1; //index of first pixel with object color
		int last = -1; //used to store: (index of last pixel with object color)+1

		while(first<=(imagecolumns-2)-minimumSize+1) //this while loop must have the same exit criterion as the (next) inner while loop, otherwise this method might not terminate
		{
    	// find left 'edge', first pixel with object color in the current row
    	while ((first<=(imagecolumns-2)-minimumSize+1) && !hasCubeColor(hues[row][first]))  //this while loop must have the same exit criterion as the (previous) outer while loop, otherwise this method might not terminate
      	first+=minimumSize; // //Note: minumumSize must be >= 1
			//check if we really found a 'first' pixel
    	if ((first<=(imagecolumns-2)-minimumSize+1) && hasCubeColor(hues[row][first]))
    	{
				while ((first-1>=1) && hasCubeColor(hues[row][first-1]))
					first--;
  	  	//we found a 'first' pixel, now we want to know the size of the object
  	  	//find right 'edge'
				last = first + 1;
	  		while ((last<=imagecolumns-2) && hasCubeColor(hues[row][last]))
    			last++;
		    if ((last - first) >= minimumSize) // check if object big enough
    		{
			    //int cubeMiddleY = first + (last-first)/2;

          //add up all pixels in this row with cube color.
          //Maybe a better estimation for cubeMiddleY then
          //first+(last-first)/2?
          //Problems occur if two cubes are in the same row and far apart
          int sum = 0;
          int numberOfCubePixels = 0;
          for (int column = 1; column <= imagecolumns-2; column++)
          {
            if (hasCubeColor(hues[row][column]))
            {
              sum += column;
              numberOfCubePixels++;
            }
          }
          int cubeMiddleY = (int)floor(sum/numberOfCubePixels + 0.5);

			    int deviationFromImageMiddleY = (imagecolumns-2)/2 - cubeMiddleY; //Note: as the robot coordinate system (x up, y to the left) and the image coordinate system (x down, y to the right) have opposite orientations, we turn around y by calculating imageMiddleY-cubeMiddleY instead of cubeMiddleY-imageMiddleY
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
					cubeDistanceX = row2Meter[row];
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
					cubeDistanceX = row2Meter[row];
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
					cubeDistanceX = row2Meter[row];
#endif
          if (deviationFromImageMiddleY >=0)
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactor[row] * 0.33;
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactor[row];
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactor[row] * 2;
#endif
          else
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactorMinus[row] * 2.5;
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactorMinus[row];
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
					  cubeDistanceY = double(deviationFromImageMiddleY) * yFactorMinus[row] * 4;
#endif
          cubePositionX   = robotPositionXAtPictureTime + cos(2*PI*phiAtPictureTime/360.0)*cubeDistanceX + sin(2*PI*phiAtPictureTime/360.0)*cubeDistanceY;
          cubePositionY   = robotPositionYAtPictureTime + cos(2*PI*phiAtPictureTime/360.0)*cubeDistanceY + sin(2*PI*phiAtPictureTime/360.0)*cubeDistanceX;

					cubeDetectedFlag = true; //Note: needs to be set after the cube position to prevent race conditions

          //used to set up tables (row2Meter, yFactor, yFactorMinus)
          /*LCDSetPos(0,0);
          LCDPrintf("row %d\n", row);
          LCDPrintf("rel x %f\n", cubeDistanceX);
          LCDPrintf("rel y %f\n", cubeDistanceY);
          for (int jj=1; jj<=imagecolumns-2; jj++)
            LCDSetPixel(row, jj, 2);
          for (int jj=1; jj<=imagerows-2; jj++)
            LCDSetPixel(jj, cubeMiddleY, 2);*/
        	return cubeDetectedFlag;
				} //found object too small
			  first = last + 1; //we know that last does not have cube color

			} //no 'first' pixel found
   	} //while(first<=imagecolumns-2)
		row--;
	} //while(row>=1)

	cubeDetectedFlag = false;
  return cubeDetectedFlag; //no cube found
  //## end SIR::detectCubes%3D99479F0168.body
}

bool SIR::detectRobots ()
{
  //## begin SIR::detectRobots%3D9947AD00C8.body preserve=yes
  return false;
  //## end SIR::detectRobots%3D9947AD00C8.body
}

void SIR::getRobotPosition (double& x, double& y, double& phi)
{
  //## begin SIR::getRobotPosition%3D99488A033E.body preserve=yes
  //## end SIR::getRobotPosition%3D99488A033E.body
}

void SIR::getGlobalCubePosition (double& x, double& y)
{
  //## begin SIR::getGlobalCubePosition%3D9948740168.body preserve=yes
		x = cubePositionX;
		y = cubePositionY;
  //## end SIR::getGlobalCubePosition%3D9948740168.body
}

void SIR::getDensity (double& density, double& centerOfDensityX, double& centerOfDensityY)
{
  //## begin SIR::getDensity%3D9948C90294.body preserve=yes
	image hues;

	colimage aColorImage;
	camera->getColorFrame(aColorImage);

  //LCDPutColorGraphic(&aColorImage);

	int numberOfCubePixels = 0;
  int rowSum = 0;
  int columnSum = 0;
	for (int row=21; row<=imagerows-2; row++)  //the camera tables for rows < 21 are unreliable, so we might underestimate the distances. But we will approach the correct values steadily as the robot will make a better estimation next time when it is closer to the new cluster point, and so on
	{
		imageFilter.calculateHue(aColorImage, row, hues);
		for (int column=11; column<=imagecolumns-12; column++) //from 11 to 70 //diff. sim. - physical robots
			if (hasCubeColor(hues[row][column]))
      {
				numberOfCubePixels++;
        rowSum += row;
        columnSum += column;
      }
	}

  density = double(numberOfCubePixels)/double((imagerows-2)*(imagecolumns-2));

  if (density != 0.0)
  {
    int centerRow = rowSum/numberOfCubePixels;
    int centerColumn = columnSum/numberOfCubePixels;
    int deviationFromImageMiddleY = (imagecolumns-2)/2 - centerColumn; //Note: as the robot coordinate system (x up, y to the left) and the image coordinate system (x down, y to the right) have opposite orientations, we turn around y by calculating imageMiddleY-centerMiddleY instead of centerMiddleY-imageMiddleY

    if (centerRow<31)
      centerRow = 31; //the camera tables for rows < 31 are unreliable, so we might underestimate the distances. But we will approach the correct values steadily as the robot will make a better estimation next time when it is closer to the new cluster point, and so on

#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
    centerOfDensityX = row2Meter[centerRow] * 0.8; //as we use the whole cubes instead of just the cube bottom to estimate the position here in this method the estimations for the x positions are generally to high, we correct them by a factor
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
    centerOfDensityX = row2Meter[centerRow] * 0.8; //as we use the whole cubes instead of just the cube bottom to estimate the position here in this method the estimations for the x positions are generally to high, we correct them by a factor
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
    centerOfDensityX = row2Meter[centerRow] * 0.8; //as we use the whole cubes instead of just the cube bottom to estimate the position here in this method the estimations for the x positions are generally to high, we correct them by a factor
#endif
    if (deviationFromImageMiddleY >=0)
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactor[centerRow] * 0.33;
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactor[centerRow];
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactor[centerRow] * 2;
#endif
    else
#ifdef EYEBOT8_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactorMinus[centerRow] * 2.5;
#endif
#ifdef EYEBOT3_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactorMinus[centerRow];
#endif
#ifdef EYESIM_CUBECLUSTERING_PARAMETERS
      centerOfDensityY = double(deviationFromImageMiddleY) * yFactorMinus[centerRow] * 4;
#endif
  }
  else
  {
    centerOfDensityX = 0;
    centerOfDensityY = 0;
  }
  //## end SIR::getDensity%3D9948C90294.body
}

void SIR::setCubeSize (int size)
{
  //## begin SIR::setCubeSize%3D9972CE0334.body preserve=yes
  //## end SIR::setCubeSize%3D9972CE0334.body
}

void SIR::setCubeColor (int color)
{
  //## begin SIR::setCubeColor%3D9972EE00F0.body preserve=yes
  //## end SIR::setCubeColor%3D9972EE00F0.body
}

// Additional Declarations
  //## begin SIR%3D86D3570320.declarations preserve=yes
	SIR* SIR::getSIR ()
	{
  	return &sirs[OSMachineID()];
	}


	bool SIR::hasCubeColor (BYTE hue)
	{
		if (hue != imageFilter.NO_HUE)
		{
			//hue and cubeColor can have values between 0..252
			//we have 253 hues in total and 126 is the
			//middle
			//we need to calculate the 'distance' between
			//these two hues
			int distance = abs((int)hue-cubeColor);
			//if the distance is larger than 126 than the other
			//way round is shorter
			if (distance > 126)
				distance = 253 - distance;

			if (distance <= allowedCubeColorDeviation)
      {
				return true;
      }
		}
		return false;
	}

	void SIR::run (void)
	{
    //int oldCount = 0;
    //int newCount = 0;
	  while(1)
	  {
      //newCount = OSGetCount();
      //LCDPrintf("sir %d\n", newCount-oldCount);
      //oldCount = newCount;
      detectCubes();
      //reschedule();
      //detectRobots();
      //reschedule();
	  }
	}

	bool SIR::obstacle ()
	{
	  return (obstacleFront() || obstacleRight() || obstacleLeft());
	}

  bool SIR::spawn ()
  {
    threadControlBlock = OSSpawn(name, staticRun, stackSize, priority, OSMachineID()*10+id);
    if (threadControlBlock)
      return true;
    else
      return false;
  }

  void SIR::staticRun ()
  {
    mes[OSMachineID()]->run();
  }

  SIR* SIR::mes[NUMBER_OF_ROBOTS];

  void SIR::getLocalCubePosition (double& x, double& y)
  {
      x = cubeDistanceX;
      y = cubeDistanceY;
  }


  //## end SIR%3D86D3570320.declarations

//## begin module%3D86D3570320.epilog preserve=yes
//## end module%3D86D3570320.epilog