/*****************************************************************************/
/*                       Image processing functions                          */
/*  Source file : detection of the rectangle under perspective view          */
/*  Designed by Igor Potucek (potucek@fit.vutbr.cz)                          */
/*  created 2006                                                             */
/*****************************************************************************/
#include <memory.h>
#include "stdafx.h"
#include "rectangle.h"
#include <math.h>
#define M_PI 3.14159265358979323846
const float MINIMAL_SIZE = 0.3;
const float MAXIMAL_SIZE = 0.8;

int LinesIntersection(s_Line *Line1, s_Line *Line2, CvPoint2D32f *Point)
{  // 0 - OK, -1 - intersection does not exist
   bool Line1ver = (Line1->Alpha-(int)Line1->Alpha==0) && ((int)Line1->Alpha % 180)==0;
   bool Line2ver = (Line2->Alpha-(int)Line2->Alpha==0) && ((int)Line2->Alpha % 180)==0;
   bool Line1hor = (Line1->Alpha-(int)Line1->Alpha==0) && (((int)Line1->Alpha + 90) % 180)==0;
   bool Line2hor = (Line2->Alpha-(int)Line2->Alpha==0) && (((int)Line2->Alpha + 90) % 180)==0;
   // lines are both horizontal or vertical
   if ((Line1ver && Line2ver) || (Line1hor && Line2hor))
     return -1;
   else
   if ((Line1hor) && (Line2ver))
   {
     Point->x = Line2->R/cos(Line2->Alpha*M_PI/180.0);
     Point->y = Line1->R/sin(Line1->Alpha*M_PI/180.0);
   }
   else
   if ((Line2hor) && (Line1ver))
   {
      Point->x = Line1->R/cos(Line1->Alpha*M_PI/180.0);
      Point->y = Line2->R/sin(Line2->Alpha*M_PI/180.0);
   }
   else
   if (Line1hor)
   {
      Point->y = Line1->R/sin(Line1->Alpha*M_PI/180.0);
      Point->x = (Line2->R-sin(Line2->Alpha*M_PI/180.0)*Point->y)/cos(Line2->Alpha*M_PI/180.0);
   }
   else
   if (Line1ver)
   {
      Point->x = Line1->R/cos(Line1->Alpha*M_PI/180.0);
      Point->y = (Line2->R-cos(Line2->Alpha*M_PI/180.0)*Point->x)/sin(Line2->Alpha*M_PI/180.0);
   }
   else
   if (Line2hor)
   {
      Point->y = Line2->R/sin(Line2->Alpha*M_PI/180.0);
      Point->x = (Line1->R-sin(Line1->Alpha*M_PI/180.0)*Point->y)/cos(Line1->Alpha*M_PI/180.0);
   }
   else
   if (Line2ver)
   {
      Point->x = Line2->R/cos(Line2->Alpha*M_PI/180.0);
      Point->y = (Line1->R-cos(Line1->Alpha*M_PI/180.0)*Point->x)/sin(Line1->Alpha*M_PI/180.0);
   }
   else
   {
     double tmp = (sin(Line2->Alpha*M_PI/180.0)*cos(Line1->Alpha*M_PI/180.0) - sin(Line1->Alpha*M_PI/180.0)*cos(Line2->Alpha*M_PI/180.0));
     if (tmp==0) return -1;       // cary jsou rovnobezne
     Point->y = (Line2->R*cos(Line1->Alpha*M_PI/180.0)-Line1->R*cos(Line2->Alpha*M_PI/180.0))/tmp;
     tmp = cos(Line1->Alpha*M_PI/180.0);
     Point->x = (Line1->R - Point->y*sin(Line1->Alpha*M_PI/180.0))/tmp;
   }
   return 0;
}

void ResizeTwice(unsigned char *source, unsigned char *dst, int Width, int Height)
{   // the final size of output image is twice smaller, rounded down
    int newWidth = Width/2;
    int newHeight = Height/2;
    unsigned char *src = source;
    unsigned char *dest = dst;
    for (int j=0; j<newHeight; j+=1)
    {
      src = source+j*Width*2;
      for (int i=0; i<newWidth; i+=1)
      {
         int sum =(*src+*(src+1)+*(src+Width)+*(src+1+Width))/4;
//             sum+=*(src+Width)+*(src+Width+1)+*(src+Width+2)+*(src+Width+3);
//             sum+=*(src+2*Width)+*(src+2*Width+1)+*(src+2*Width+2)+*(src+2*Width+3);
//             sum+=*(src+3*Width)+*(src+3*Width+1)+*(src+3*Width+2)+*(src+3*Width+3);
         if (sum>255)
           *dest = 255;
         else
         if (sum<0)
           *dest=0;
         else
           *dest = sum;
         src+=2;
         dest+=1;
      }
      //  dest+=newWidth;
    }
}

// definition of the operator for list sorting 
bool operator<(const s_Line &x, const s_Line &y)
{
   return x.R < y.R;
}

void c_DetectRectangle::Init(int XSize, int YSize)
{
  VerticalLines.clear();
  HorizontalLines.clear();
  Width = XSize;
  Height = YSize;
}

void c_DetectRectangle::PutLine(float R, float Alpha, int Weight)
{
   s_Line Line;
     float angle = Alpha;
     while (angle < 0) angle += 180;
     while (angle > 180) angle -= 180;
   Line.Alpha = Alpha;
   Line.R = R;
   if ((angle>=45) && (angle<=135))
      HorizontalLines.push_back(Line);
   else
      VerticalLines.push_back(Line);
}

void c_DetectRectangle::Sort()
{
   VerticalLines.sort();
   HorizontalLines.sort();
   VerticalPoints = VerticalLines.size();
   HorizontalPoints = HorizontalLines.size();
}

bool TestIntersection(CvPoint2D32f *A,int xsize,int ysize)
{
  for (int i=0; i<4; i+=1)
  {
     if ((A[i].x<1) || (A[i].y<1) || (A[i].x>xsize-2) || (A[i].y>ysize-2))
       return false;
  }
  return true;
}

float c_DetectRectangle::FindRectangle(unsigned char *Image, int xsize, int ysize, CvPoint2D32f *BestA)
{
  //////////////////////// compute all intersections //////////////////////////
   t_LineList::iterator lbi = VerticalLines.begin(), lei = VerticalLines.end();
   t_LineList::iterator rbi = HorizontalLines.begin(), rei = HorizontalLines.end();
   t_LineList::iterator rbi2,rei2, lbi2, lei2;
   lei2 = VerticalLines.end();
   rei2 = HorizontalLines.end();
   // compute all intersections
   CvPoint2D32f A[4];
   float BestValue = 0;
   int k1=0, k2=0, k3=0, cnt=0;
   for (; rbi != rei; ++rbi)
   {
      lbi = VerticalLines.begin(); lei = VerticalLines.end();
	  k1++;
      for (; lbi != lei; ++lbi)
      {
         LinesIntersection(&(*lbi), &(*rbi), &A[0]);
         lbi++;
         lbi2 = lbi;
         lbi--;
//         lei2 = VerticalLines.end();
		 k2++;
         for (; lbi2 != lei2; ++lbi2)
		 {
           LinesIntersection(&(*lbi2), &(*rbi), &A[1]);
           rbi++;
           rbi2 = rbi;
           rbi--;
           //rei2 = HorizontalLines.end();
		   k3++;
           for (; rbi2 != rei2; ++rbi2)
		   {
             LinesIntersection(&(*lbi2), &(*rbi2), &A[2]);
             LinesIntersection(&(*lbi), &(*rbi2), &A[3]);
           //  if (TestIntersection(A, Width, Height))
             {
				 float value = HypothesisTest(Image, xsize, ysize, A, 4);
				 cnt++;
              // return 0;
               float Ratio1 = 0, Ratio2 = 0;
               if ((A[2].y-A[1].y)!=0) Ratio1 = (A[1].x-A[0].x)/(float)(A[2].y-A[1].y);
               if ((A[3].y-A[0].y)!=0) Ratio2 = (A[2].x-A[3].x)/(float)(A[3].y-A[0].y);
               if ((A[1].x-A[0].x>MINIMAL_SIZE*Width) && (A[1].x-A[0].x<MAXIMAL_SIZE*Width))
               if ((value>BestValue) && (Ratio1<2.0) && (Ratio1>0.8) && (Ratio2<2.0) && (Ratio2>0.8))
               {
                  BestA[0] = A[0]; BestA[1] = A[1]; BestA[2] = A[2]; BestA[3] = A[3];
                  BestValue = value;
               }
             } //endif
           }
         }
      }
   }
   return BestValue;
}


float c_DetectRectangle::HypothesisTest(unsigned char *Image, int xsize, int ysize, CvPoint2D32f *Lines, int nr)
{  // dodelat testovani mimo okraj obrazku // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   float final = 0;
   float constant = 0.4;
   float Rx = (float)xsize/Width;
   float Ry = (float)ysize/Height;
   for (int i=0; i<nr; i++)
   {
      CvPoint2D32f PointX1 = Lines[i];
      CvPoint2D32f PointX2 = Lines[(i+1) % 4];
      int length = 0;
      int count = 0;
      int x1 = PointX1.x*Rx;
      int y1 = PointX1.y*Ry;
      int x0 = PointX2.x*Rx;
      int y0 = PointX2.y*Ry;

      int dx, dy;
      int xinc, yinc;
      register int res1;
      int res2;
      xinc = 1;
      yinc = 1;
       if ((dx = x1-x0) < 0)
       {
           xinc = -1;
           dx = -dx;
       }
       if ((dy = y1-y0) < 0)
       {
           yinc = -1;
           dy = -dy;
       }
       res1 = 0; res2 = 0;
       if (dx > dy)
       {
          while (x0 != x1)
           {
             if ((x0>0) && (y0>0) && (x0<xsize-1) && (y0<ysize-1))
			 {
				 unsigned char *src =  Image+x0+xsize*y0;
	/*             int sum =*src;
				 if (sum>0) { count+=1; }//*src=200;
	*/             int sum =*src+*(src+1)+*(src-1);
				 sum+=*(src+xsize)+*(src+xsize+1)+*(src+xsize-1);
				 sum+=*(src-xsize)+*(src-xsize+1)+*(src-xsize-1);
				 if (sum>256) count+=1;
				 length+=1;
			 }
              if (res1 > res2)
              {
                  res2 += dx - res1;
                  res1 = 0;
                  y0 += yinc;
              }
              res1 += dy;
              x0 += xinc;
          }
      }
      else if (dx < dy)
      {
         while (y0 != y1)
         {
             if ((x0>0) && (y0>0) && (x0<xsize-1) && (y0<ysize-1))
			 {
				 unsigned char *src =  Image+x0+xsize*y0;
	 /*            int sum =*src;
				 if (sum>0) { count+=1; } //*src=200;
	*/           int sum =*src+*(src+1)+*(src-1);
				 sum+=*(src+xsize)+*(src+xsize+1)+*(src+xsize-1);
				 sum+=*(src-xsize)+*(src-xsize+1)+*(src-xsize-1);
				 if (sum>256) count+=1;
				 length+=1;
			 }
              if (res1 > res2)
              {
                  res2 += dy - res1;
                  res1 = 0;
                  x0 += xinc;
              }
               res1 += dx;
               y0 += yinc;
           }
       }
       else
       {
           while (x0 != x1)
           {
             if ((x0>0) && (y0>0) && (x0<xsize-1) && (y0<ysize-1))
			 {
				 unsigned char *src =  Image+x0+xsize*y0;
	/*             int sum =*src;
				 if (sum>0) { count+=1; } //*src=200;
	*/           int sum =*src+*(src+1)+*(src-1);
				 sum+=*(src+xsize)+*(src+xsize+1)+*(src+xsize-1);
				 sum+=*(src-xsize)+*(src-xsize+1)+*(src-xsize-1);
				 if (sum>256) count+=1;
				 length+=1;
			 }
               y0 += yinc;
               x0 += xinc;
           }
       }
       if (length>0)
         final += (float)count/length;
   }
     return final/4.0;
}
