/*****************************************************************************/
/*                       Image processing functions                          */
/*  Source file : hough transform for line detection                         */
/*  Designed by Igor Potucek (potucek@fit.vutbr.cz)                          */
/*  created 2005                                                             */
/*****************************************************************************/
#include "stdafx.h"
#include "linehough.h"
#include <math.h>
#include "cv.h"


#define M_PI 3.14159265358979323846
#define max(a,b) ((a)>(b))?(a):(b)

// size of matrix axis with angles in hough space
const int AXIS_ALPHA_SIZE = 300;
// number of cells in x & y direction for line aproximation
const int MATRIX_SURROUND = 3;
// max distance from line
const int LINE_DIST = 2;
// number of cells in x & y direction for erasing the values
const float MATRIX_ZERO_SURROUND_X = 0.07;
const float MATRIX_ZERO_SURROUND_Y = 0.05;
// size of the step in ortogonal direction to the line
const float STEP = 0.8;
// coeficient for min value of the cell in hough space for LSM
const float HOUGH_THRESHOLD = 0.0002;     // result = XSize * YSize * HOUGH_THRESHOLD

void c_LineHoughTransform::Init(int X, int Y, int Angleparts, float AspectRatio)
// initialization accumulator array
// Xs - maximal size of the r, Ys - maximal count of steps 0 - 2Pi
{
   int Xs=RmaxValue(X,Y)*AspectRatio*2;
   ZeroWidth  = MATRIX_ZERO_SURROUND_X * Xs;
   ZeroHeight = MATRIX_ZERO_SURROUND_Y * Angleparts;
   Aspect=AspectRatio;
   Angleparts=Angleparts*AspectRatio;
   if ((Array!=NULL) && ((XSize!=Xs) || (YSize!=Angleparts)))
   {
     delete [] Array;  Array=NULL;
   }
   if (Array==NULL)
   {
     Array = new int[Xs*Angleparts];
   }
   // initialize and compute auxiliary sin table
   if ((SinFun!=NULL) && (YSize!=Angleparts))
   {
      delete [] SinFun;
      SinFun=NULL;
   }
   if (SinFun==NULL)
   {
      SinFun=new float[Angleparts];
      for (int i=0; i<Angleparts; i++)
        SinFun[i]=sin(i*M_PI/(float)(Angleparts));
   }
   // initialize and compute auxiliary cos table
   if ((CosFun!=NULL) && (YSize!=Angleparts))
   {
      delete [] CosFun;
      CosFun=NULL;
   }
   if (CosFun==NULL)
   {
      CosFun=new float[Angleparts];
      for (int i=0; i<Angleparts; i++)
        CosFun[i]=cos(i*M_PI/(float)(Angleparts));
   }
   XSize=Xs;
   YSize=Angleparts;
   Count=0;
   memset(Array, 0, XSize*YSize*sizeof(int));
}

bool c_LineHoughTransform::AddValue(int x, int y)
{  // transform position in image into position in hough space
  Count++;
  for (int i=0; i<YSize; i++)
  {
    // +0.5 do round
    float tmp = (x*CosFun[i]+y*SinFun[i])*Aspect+XSize/2;
    int Rpos=int((tmp<0)?(tmp-0.5):(tmp+0.5));
    if ((Rpos>=0) && (Rpos<XSize))
      Array[Rpos+i*XSize]+=1;
  }
  return true;
}

int c_LineHoughTransform::RmaxValue(int ImageXsize, int ImageYSize)
{ // return size of the image diagonal
  return sqrt((float)(ImageXsize*ImageXsize+ImageYSize*ImageYSize));
}

void c_LineHoughTransform::GetLineSurround(int r, int a, unsigned char *dst, int imXSize, int imYSize)
{ // fill dst matrix with 1 where lie the lines
  r = r + XSize/2;
  int threshold = XSize*YSize*HOUGH_THRESHOLD;
  int xmin=r-MATRIX_SURROUND;
  int xmax=r+MATRIX_SURROUND;
  int ymin=a-MATRIX_SURROUND;
  int ymax=a+MATRIX_SURROUND;
  if (xmin<0) xmin=0;
  if (xmax>XSize) xmax=XSize;
  if (ymin<0) ymin=0;
  if (ymax>YSize) ymax=YSize;
  for (int i=xmin; i<xmax; i++)
    for (int j=ymin; j<ymax; j++)
    {
       // takes only lines with big probability (length)
       int value=GetValue(i,j);
       if (value>threshold)
       {
         // takes lines in LINE_DIST distance from aproximated line
         for (float R=i-LINE_DIST; R<i+LINE_DIST; R+=STEP)
         {
            if (((j>=0) && (j<YSize/8.0)) || ((j>3*YSize/8.0) && (j<5*YSize/8.0)) || ((j>7*YSize/8.0) && (j<YSize)))
            {
               for (int y=0; y<imYSize; y++)
               {
                  int x=0; if (CosFun[j]!=0) x=(R-(float)y*SinFun[j])/CosFun[j];
                  if ((x>=0) && (x<imXSize))
                    dst[x+(imYSize-y-1)*imXSize]=1;
               }
            }
            else
            {
               for (int x=0; x<imXSize; x++)
               {
                  int y=0; if (SinFun[j]!=0) y=(R-(float)x*CosFun[j])/SinFun[j];
                  if ((y>=0) && (y<imYSize))
                    dst[x+(imYSize-y-1)*imXSize]=1;
               }
            }
         } // endfor R
       } // endif
    } // endfor MATRIX_SURROUND
  xmin=r-MATRIX_ZERO_SURROUND_X;
  xmax=r+MATRIX_ZERO_SURROUND_X;
  ymin=a-MATRIX_ZERO_SURROUND_Y;
  ymax=a+MATRIX_ZERO_SURROUND_Y;
  if (xmin<0) xmin=0;
  if (xmax>=XSize) xmax=XSize-1;
  if (ymin<0) ymin=0;
  if (ymax>=YSize) ymax=YSize-1;  
  for (int i=xmin; i<xmax; i++)
    for (int j=ymin; j<ymax; j++)
    {
       Array[i+j*XSize]=0;
    }
}

void c_LineHoughTransform::GetLine(float r, float alpha, int Ximage, int Yimage,int &x1, int &y1, int &x2, int &y2)
{
   bool first=true, second=true;
   r=r/Aspect;
   alpha=M_PI*alpha/YSize;
   // x = (r-y*sin(alpha))/cos(alpha)
   // y = (r-x*cos(alpha))/sin(alpha)
   // try to find intersections with image border
   int xmin=-1;   if (cos(alpha)!=0) xmin=r/cos(alpha);
   int xmax=-1; if (cos(alpha)!=0) xmax=(r-Yimage*sin(alpha))/cos(alpha);
   int ymin=-1;     if (sin(alpha)!=0) ymin=r/sin(alpha);
   int ymax=-1; if (sin(alpha)!=0) ymax=(r-Ximage*cos(alpha))/sin(alpha);
   if ((xmin>=0) && (xmin<Ximage))
   {
      x1=xmin; y1=0;
      first=false;
   }
   if ((xmax>=0) && (xmax<Ximage))
   {  if (first)
        { x1=xmax; y1=Yimage; first=false;  }
      else
        { x2=xmax; y2=Yimage; second=false; }
   }
   if ((ymin>0) && (ymin<Yimage))
   {  if (first)
        { x1=0; y1=ymin; first=false;  }
      else
      if (second)
        { x2=0; y2=ymin; second=false; }
   }
   if ((ymax>0) && (ymax<Yimage))
   {
      if (second)
      { x2=Ximage; y2=ymax; }
   }
}

 int c_LineHoughTransform::GetValue(int x, int y)
 {
   if ((x>=0) && (x<XSize) && (y>=0) && (y<YSize))
     return Array[x+y*XSize];
   else
     return -1;
 }
 int c_LineHoughTransform::FindMax(int &x, int &y)
 {  // normalize by maximal value
    int great=1;
    // search maximum
    for (int j=0; j<YSize; j++)
      for (int i=0; i<XSize; i++)
      {
         if (great<Array[j*XSize+i])
         {
            great=Array[j*XSize+i];
            x = i;  y = j;
         }
      }
      return great;
    //  x = x - XSize/2;
 }

unsigned char* c_LineHoughTransform::NormalizedHoughArray()
{
  unsigned char *hough=new unsigned char[XSize*YSize];
  int great=1;
  for (int i=0; i<XSize; i++)
    for (int j=0; j<YSize; j++)
    {
       great=max(great,Array[j*XSize+i]);
    }
  for (int i=0; i<XSize; i++)
    for (int j=0; j<YSize; j++)
    {
         int value=255*Array[j*XSize+i]/great;
         hough[j*XSize+i]=value;
    }
  return hough;
}


//------------------------------------------------------------------------------
  long c_LineHoughTransform::DrawArray(unsigned char *src, int Width, int Height, int Mult)
 {  // normalize by maximal value
    int great=1;
    // search maximum
    for (int j=0; j<YSize; j++)
      for (int i=0; i<XSize; i++)
      {
         great=max(great,Array[j*XSize+i]);
      }
    for (int j=0; j<Height; j++)
      for (int i=0; i<Width; i++)
      {
         
         if ((i*Mult<XSize) && (j*Mult<YSize))
       {
           int value = 255*Array[i*Mult+(j*Mult)*XSize]/great;
		   if (value>0)
		      *src = value;
       }

       src++;
      }
    return great;
 }

