/*****************************************************************************/
/*                       Image processing functions                          */
/*  Source file : detection teacher position                                 */
/*  Designed by Igor Potucek (potucek@fit.vutbr.cz)                          */
/*  created 2006                                                             */
/*****************************************************************************/

#include "teachertracking.h"
#include "highgui.h"

const int BORDER = 5;

bool IsPrezentation(CvPoint2D32f *Points, int x, int y)
{  // points representation   0 1
   //                         3 2
   int minx = (Points[0].x>Points[3].x)?(int)(Points[3].x+0.5):(int)(Points[0].x+0.5);
   int miny = (Points[0].y>Points[1].y)?(int)(Points[1].y+0.5):(int)(Points[0].y+0.5);
   int maxx = (Points[1].x>Points[2].x)?(int)(Points[1].x+0.5):(int)(Points[2].x+0.5);
   int maxy = (Points[2].y>Points[3].y)?(int)(Points[2].y+0.5):(int)(Points[3].y+0.5);
   return ((x>=minx-BORDER) && (x<=maxx+BORDER) && (y>=miny-BORDER) && (y<=maxy+BORDER));
}

void DeletePrezentation(IplImage *imgColor, CvPoint2D32f *Prezentation)
{
 for (int j = 0; j < imgColor->height; j++)
  {
    unsigned char *data = (unsigned char *)imgColor->imageData + imgColor->widthStep*j;
    for (int i = 0; i < imgColor->width; i++)
    {
	   if (IsPrezentation(Prezentation, i, j))
     {
		  data[0] = 0;
      data[1] = 0;
      data[2] = 0;
	   }
     data += 3;
    }
  }
}

bool c_BackGroundDetection::Init(unsigned char *img, int XSize, int YSize, CvPoint2D32f *Prezentation)
{
  imWidth = XSize;
  imHeight = YSize;
  PositionList.clear();
  if (OCimgColor!=NULL) cvReleaseImage(&OCimgColor);
      OCimgColor  = cvCreateImage(cvSize(imWidth,imHeight), IPL_DEPTH_8U, 3);
  // create  from color input image
  //DeletePrezentation(img, XSize, YSize, Prezentation);
  memcpy(OCimgColor->imageData, img, imWidth*imHeight*3*sizeof(unsigned char));
  memcpy(BestPoints, Prezentation, 4*sizeof(CvPoint2D32f));
  return true;
}

bool c_BackGroundDetection::Init(IplImage *imgColor, CvPoint2D32f *Prezentation)
{
  imWidth = imgColor->width;
  imHeight = imgColor->height;
  PositionList.clear();
  //DeletePrezentation((unsigned char *)imgColor->imageData, imWidth, imHeight, Prezentation);
  if (OCimgColor!=NULL) cvReleaseImage(&OCimgColor);
      OCimgColor  = cvCreateImage(cvSize(imWidth,imHeight), IPL_DEPTH_8U, 3);
  if (MaskImage!=NULL) cvReleaseImage(&MaskImage);
      MaskImage  = cvCreateImage(cvSize(imWidth,imHeight), IPL_DEPTH_8U, 1);
  memcpy(BestPoints, Prezentation, 4*sizeof(CvPoint2D32f));
  cvCopyImage(imgColor, OCimgColor);
  track_window = cvRect(0,0,imWidth,imHeight);
  return true;
}

void c_BackGroundDetection::Localize(IplImage *imgColor, double &x, double &y, double &divx, double &divy, int &Size, int Threshold)
{
  DeletePrezentation(imgColor, BestPoints);
  unsigned char *OCdata = (unsigned char *)OCimgColor->imageData;	// image from previous frame
  unsigned char *data = (unsigned char *)imgColor->imageData;		// current image
  unsigned char *mask = (unsigned char *)MaskImage->imageData;		// probability image with moving objects
  cvSetZero(MaskImage);
  for (int j = 0; j < imHeight; j++)
  {
    mask   =  (unsigned char *)MaskImage->imageData+MaskImage->widthStep*j;
    OCdata =  (unsigned char *)OCimgColor->imageData + OCimgColor->widthStep*j;
    data =    (unsigned char *)imgColor->imageData + imgColor->widthStep*j;
    for (int i = 0; i < imWidth; i++)
    {

   	   int difference = abs(*data-*OCdata)+abs(*(data+1)-*(OCdata+1))+abs(*(data+2)-*(OCdata+2));
	   if (!IsPrezentation(BestPoints, i, j) && (difference>Threshold))
     {
		  *mask = difference;
	   }
     else
      *mask = 0;


       OCdata+=3;
	   data+=3;
	   mask++;
    }
  }
	cvErode(MaskImage, MaskImage, 0, 1);
	cvDilate(MaskImage, MaskImage, 0, 1);
//        cvSaveImage("proc.jpg", MaskImage);
	cvCamShift(MaskImage, track_window,
                          cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                          &track_comp, &track_box );
  s_Position Pos;
  x = Pos.x = track_box.center.x;
  y = Pos.y = track_box.center.y;
  divx = Pos.divy = track_box.size.width;
  divy = Pos.divx = track_box.size.height;
  Size = Pos.Size = track_box.size.height*track_box.size.width;

  PositionList.push_back(Pos);
  cvCopyImage(imgColor, OCimgColor);
  // draw results
  data = (unsigned char *)imgColor->imageData;
  mask = (unsigned char *)MaskImage->imageData;
  for (int j = 0; j < imHeight; j++)
    for (int i = 0; i < imWidth*3; i++)
    {
       *data = mask[i/3+j*imWidth]*2;
	   //if (pole[i/3+j*imWidth]>0)  *data = 255;  else  *data = 0;
       data++;
	}
}

int c_BackGroundDetection::GetPosition(int Pos, float &x, float &y)
{
  if ((Pos<0) || (Pos>PositionList.size()))
    return -1;
  s_Position Position = PositionList[Pos];
  x = (float)Position.x;
  y = (float)Position.y;
  return Position.Size;

}
