/****************************************************************/
/**@file CameraTools.cpp: 
 * Desctiption: Higher level abstraction above raw camera driver.*
 *              This code acts like driver, that provides 
 *              RT point with ball position			*
 * Dependency:							*
 ****************************************************************/
#include "hardware/OsSupp.h"
#include "images/csext.h"

#include "images/raster.h"
#include "images/ras_comp.h"
#include "images/img_tool.h"

#include "CameraTools.h"

#include <strmif.h>


const int CameraTool::DEFAULT_THR = 220; ///< Default threshold value


/** Read input points from camera object */
Point *CameraTool::GetInput(int Index)
{
switch(Index)
  {
  case 0: return &VideoX;      //motor 1 position
  case 1: return &VideoY;      //motor 2 position  
  case 2: return &WrThreshold; ///< Get threshold
  default: return NULL;
  }
};


/** Periodical execution recalculates ball positions. */
int CameraTool::Execute(void)
{
int posX, posY;
 
  if(Camera==NULL) return -1;  
  
  try
    {
    //Camera->GrabImage();		//GrabImage();

      /*int BALL_STATE = */
    if(BalSearchAlloved && BallSearch(&posX,&posY,false,false)==BALL_OK)
      {
      VideoX.SetDouble((2*posX)/(double)iWidth - 1);
      VideoY.SetDouble((-2*posY)/(double)iHeight + 1);
      }
    else
      {
      InvalidateVideoPos();
      }
    }
  catch(...)
    {	// problem with grab
    InvalidateVideoPos();
    return -2;
    }

return 0;
}


void CameraTool::InvalidateVideoPos(void)
{
  double d=0;
  MakeNaN(&d);
  VideoX.SetDouble(d);	
  VideoY.SetDouble(d);
}


#include "LineConv.cpp"
////////////////////////////////////////////////////////////


/** This method formats camera raw image into required format. Moreover it
  also crops image to a rectangular shape. This makes signifficant speedup of whole
  application.
  @param[in] ImageBuf Buffer that receives image data in required format.
  @param[in] mode = 0 true color image; 1 true color as gray; 2 true color as mono
                     0x11 native gray 8bit;0x12 8 bit mono  */
bool CameraTool::ReadImageAs(Raster2DAbstract *ImageBuf, int mode, bool VFlip, bool HFlip, const unsigned char *BufferData)
{
int Width, Height;
char destplanes;
int y;
int OffsetX;

  if(Camera==NULL) return false;
  if(BufferData==NULL) return false;
  if(ImageBuf==NULL) return false;
  
  Width = Camera->GetWidth();
  if(Width<0) {Width=-Width;HFlip=!HFlip;}
  Height = Camera->GetHeight();
  if(Height<0) {Height=-Height;VFlip=!VFlip;}

  iWidth = Width;
  iHeight = Height;

  if(Width<=0 || Height<=0) 
	return false;  

  OffsetX = 0;

  if(mode & 0x80)
  {
    mode &= ~0x80;
    if(Width>Height)	//cut image to a rectangle
    {
      OffsetX = (Width-Height) / 2;
      iWidth = Height;
    }
  }

  destplanes = (mode<0x10) ? 24 : 8;
  if(destplanes!=ImageBuf->GetPlanes())
    return false;
  if(ImageBuf->Size1D!=Width || ImageBuf->Size2D!=Height)
  {
    if(ImageBuf->UsageCount<READ_BARIER)
    {
      InterlockedExchangeAdd(&ImageBuf->UsageCount, WRITE_BARIER);
      if(ImageBuf->UsageCount < WRITE_BARIER+READ_BARIER)
      {
        ImageBuf->Allocate2D(Width,Height);
      }
      InterlockedExchangeAdd(&ImageBuf->UsageCount, -WRITE_BARIER);
    }
    else
    {
      return false;	// rather exit than overflow.
    }
  }  
  
	// recompute the image to monochrome according to mode    

  const int depth = Camera->GetDepth();
  AConvertVideoLine *pConvertVideoLine = NULL;

  switch(depth)
  {
    case 64:							//ARGB camera 64 bit
            if(Camera->VideoFormat==vfA16R16G16B16 || Camera->VideoFormat==vfA16B16G16R16)
            {
              switch(mode)
              {
                case 0: if(Camera->VideoFormat==vfA16R16G16B16)
                            pConvertVideoLine = new ConvertVideoLine_00_A16R16G16B16;
                        else
                            pConvertVideoLine = new ConvertVideoLine_00_A16B16G16R16;
                        break;
		case 1: pConvertVideoLine = new ConvertVideoLine_01_A16x16G16x16; break;
                case 2: pConvertVideoLine = new ConvertVideoLine_02_A16x16G16x16(threshold); break;
                case 0x11: pConvertVideoLine = new ConvertVideoLine_11_A16x16G16x16; break;
                case 0x12: pConvertVideoLine = new ConvertVideoLine_12_A16x16G16x16(threshold); break;
	      }
            }
            break;       

    case 48:							//RGB camera 48 bit
            if(Camera->VideoFormat==vfR16G16B16 || Camera->VideoFormat==vfB16G16R16)
            {
              switch(mode)
              {
                case 0: if(Camera->VideoFormat==vfR16G16B16)
                            pConvertVideoLine = new ConvertVideoLine_00_R16G16B16;
                        else
                            pConvertVideoLine = new ConvertVideoLine_00_B16G16R16;
                        break;
		case 1: pConvertVideoLine = new ConvertVideoLine_01_x16G16x16; break;
                case 2: pConvertVideoLine = new ConvertVideoLine_02_x16G16x16(threshold); break;
                case 0x11: pConvertVideoLine = new ConvertVideoLine_11_x16G16x16; break;
                case 0x12: pConvertVideoLine = new ConvertVideoLine_12_x16G16x16(threshold); break;
	      }
            }
            break;       

    case 32:if(Camera->VideoFormat==vfR210)
            {
              switch(mode)
              {
                    case 0: pConvertVideoLine = new ConvertVideoLine_00_R210; break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_R210; break;
                    case 2: pConvertVideoLine = new ConvertVideoLine_02_R210(threshold); break;
                    case 0x11:pConvertVideoLine = new ConvertVideoLine_11_R210; break;
                    case 0x12:pConvertVideoLine = new ConvertVideoLine_12_R210(threshold); break;
              }
              break;
            }
            if(Camera->VideoFormat==vfAYUV)
            {
              switch(mode)
              {
                    case 0: pConvertVideoLine = new ConvertVideoLine_00_AYUV; break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_AYUV; break;
                    case 2: pConvertVideoLine = new ConvertVideoLine_02_AYUV(threshold); break;
                    case 0x11:pConvertVideoLine = new ConvertVideoLine_11_AYUV; break;
                    case 0x12:pConvertVideoLine = new ConvertVideoLine_12_AYUV(threshold); break;
              }
              break;
            }           
            if(Camera->VideoFormat==vfA2B10G10R10 || Camera->VideoFormat==vfA2R10G10B10) 
            {
              switch(mode)
              {
                    case 0: if(Camera->VideoFormat==vfA2R10G10B10)
                              pConvertVideoLine = new ConvertVideoLine_00_vfA2R10G10B10;
                            else
                              pConvertVideoLine = new ConvertVideoLine_00_vfA2B10G10R10;
                            break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_vfA2x10G10x10; break;
		    case 2: pConvertVideoLine = new ConvertVideoLine_02_vfA2x10G10x10(threshold); break;
		    case 0x11:pConvertVideoLine = new ConvertVideoLine_11_vfA2x10G10x10; break;
		    case 0x12:pConvertVideoLine = new ConvertVideoLine_12_vfA2x10G10x10(threshold); break;
              }
            }
            switch(mode)
            {
                  case 0: pConvertVideoLine = new ConvertVideoLine_00_ARGB; break;
		  case 1: pConvertVideoLine = new ConvertVideoLine_01_ARGB; break;
                  case 2: pConvertVideoLine = new ConvertVideoLine_02_ARGB(threshold); break;
                  case 0x11:pConvertVideoLine = new ConvertVideoLine_11_ARGB; break;
                  case 0x12:pConvertVideoLine = new ConvertVideoLine_12_ARGB(threshold); break;
            }
            break;
    case 24:
	    switch(mode)
            {
                  case 0: pConvertVideoLine = new ConvertVideoLine_00_RGB; break;
		  case 1: pConvertVideoLine = new ConvertVideoLine_01_RGB; break;
                  case 2: pConvertVideoLine = new ConvertVideoLine_02_RGB(threshold); break;
                  case 0x11:pConvertVideoLine = new ConvertVideoLine_11_RGB; break;
                  case 0x12:pConvertVideoLine = new ConvertVideoLine_12_RGB(threshold); break;
            }
            break;
    case 16:	
	    if(Camera->VideoFormat == vfRGB565)		// https://chowdera.com/2021/12/202112271902236648.html
            {
              switch(mode)
              {
                    case 0: pConvertVideoLine = new ConvertVideoLine_00_RGB565; break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_RGB565; break;
                    case 2: pConvertVideoLine = new ConvertVideoLine_02_RGB565(threshold); break;
                    case 0x11: pConvertVideoLine = new ConvertVideoLine_11_RGB565; break;
                    case 0x12: pConvertVideoLine = new ConvertVideoLine_12_RGB565(threshold); break;
              }
              break;
            }
            if(Camera->VideoFormat == vfRGB555)
            {
              switch(mode)
              {
                    case 0: pConvertVideoLine = new ConvertVideoLine_00_RGB555; break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_RGB555; break;
                    case 2: pConvertVideoLine = new ConvertVideoLine_02_RGB555(threshold); break;
                    case 0x11: pConvertVideoLine = new ConvertVideoLine_11_RGB555; break;
                    case 0x12: pConvertVideoLine = new ConvertVideoLine_12_RGB555(threshold); break;
              }
              break;
            }
            if(Camera->VideoFormat==vfY422)
            {
              switch(mode)
              {                 
                 case 1: pConvertVideoLine = new ConvertVideoLine_00_Gray8; break;
                 case 2: pConvertVideoLine = new ConvertVideoLine_02_Gray8(threshold); break;
                 case 0x11: pConvertVideoLine = new ConvertVideoLine_11_Gray8; break;
                 case 0x12: pConvertVideoLine = new ConvertVideoLine_12_Gray8(threshold); break;
              }
              break;
            }            
            if(Camera->VideoFormat==vfUYVY || Camera->VideoFormat==vfYVYU || Camera->VideoFormat==vfYUYV)
            {				// YUY2 same as YUYV
              switch(mode)
              {
                case 0:if(Camera->VideoFormat == vfUYVY)
                           {pConvertVideoLine = new ConvertVideoLine_00_UYVY; break;}
                       if(Camera->VideoFormat == vfYVYU)
                           {pConvertVideoLine = new ConvertVideoLine_00_YVYU; break;}
                       pConvertVideoLine = new ConvertVideoLine_00_YUYV;
                       break;
                case 1:if(Camera->VideoFormat == vfUYVY)
                            pConvertVideoLine = new ConvertVideoLine_01_UYVY;
                       else
                            pConvertVideoLine = new ConvertVideoLine_01_YxYx;
		       break;
                case 2:if(Camera->VideoFormat == vfUYVY)
                            pConvertVideoLine = new ConvertVideoLine_02_UYVY(threshold);
                       else
                            pConvertVideoLine = new ConvertVideoLine_02_YxYx(threshold);
		       break;
                case 0x11:if(Camera->VideoFormat == vfUYVY)
                            pConvertVideoLine = new ConvertVideoLine_11_UYVY;
                       else
                            pConvertVideoLine = new ConvertVideoLine_11_YxYx;
		       break;
                case 0x12:if(Camera->VideoFormat == vfUYVY)
                            pConvertVideoLine = new ConvertVideoLine_12_UYVY(threshold);
                       else
                            pConvertVideoLine = new ConvertVideoLine_12_YxYx(threshold);
		       break;
              }

            }
            break;
    case 12:if(Camera->VideoFormat!=vfI420 && Camera->VideoFormat!=vfNV12) break;	// I420 has 8bpp Y block
            if(mode==0) break;								// do not use 8bpp handling her
    case 8: if(Camera->VideoFormat==vfRGB8 && Camera->pCameraPalette!=NULL)
            {
              switch(mode)
              {
                    case 0: pConvertVideoLine = new ConvertVideoLine_00_RGB8(Camera->pCameraPalette); break;
		    case 1: pConvertVideoLine = new ConvertVideoLine_01_RGB8(Camera->pCameraPalette); break;
                    case 2: pConvertVideoLine = new ConvertVideoLine_02_RGB8(Camera->pCameraPalette,threshold); break;
                    case 0x11: pConvertVideoLine = new ConvertVideoLine_11_RGB8(Camera->pCameraPalette); break;
                    case 0x12: pConvertVideoLine = new ConvertVideoLine_12_RGB8(Camera->pCameraPalette,threshold); break;
              }
              break;
            }
            switch(mode)
            {
                 case 0: if(Camera->VideoFormat==vfI420 || Camera->VideoFormat==vfNV12)
                         {
                           break;
                         }
                 case 1: pConvertVideoLine = new ConvertVideoLine_00_Gray8; break;
                 case 2: pConvertVideoLine = new ConvertVideoLine_02_Gray8(threshold); break;
                 case 0x11: pConvertVideoLine = new ConvertVideoLine_11_Gray8; break;
                 case 0x12: pConvertVideoLine = new ConvertVideoLine_12_Gray8(threshold); break;
            }
            break;

    default:	// bpp varies from 21 to 22.
            if(Camera->VideoFormat==vfV210) // https://wiki.multimedia.cx/index.php/V210
            {
              switch(mode)
              {
                case 0: pConvertVideoLine = new ConvertVideoLine_00_V210; break;
                case 1: pConvertVideoLine = new ConvertVideoLine_01_V210; break;
                case 2: pConvertVideoLine = new ConvertVideoLine_02_V210(threshold); break;
                case 0x11: pConvertVideoLine = new ConvertVideoLine_11_V210; break;
                case 0x12: pConvertVideoLine = new ConvertVideoLine_12_V210(threshold); break;
             }
             break;
           }
           break;
  }

  InterlockedExchangeAdd(&ImageBuf->UsageCount, READ_BARIER);
  for(y=0; y<Height; y++)
  {
    if(pConvertVideoLine)
    {
      pConvertVideoLine->ConvertData((unsigned char *)ImageBuf->GetRow(y), BufferData+pConvertVideoLine->RowStart(y,Width), Width);
    }
    else
    {
      if(mode==0)
      {
	OffsetX &= ~1;
        unsigned char *j = (uint8_t *)ImageBuf->GetRow(y);
        if(Camera->VideoFormat==vfNV12)	// NV12 mode 0 needs special handling.
        {          			//calculate buffer positions          
          const unsigned char *i = BufferData + (OffsetX+y*Width);	// Y canal is like 8 gray      
          Y_UV_RGB(j, i,
		    BufferData + (Height*Width) + OffsetX/2 + (y/2)*Width,
		    Width);	//converts to RGB
        }
        else if(Camera->VideoFormat==vfI420)	// I420 mode 0 needs special handling.
        {				//calculate buffer positions
          const unsigned char *i = BufferData + (OffsetX+y*Width);	// Y canal is like 8 gray      
          YUV_RGB(j, i,
                    BufferData + 5*(Height*Width)/4 + OffsetX/2 + (y/2)*(Width/2),
		    BufferData + (Height*Width) + OffsetX/2 + (y/2)*(Width/2),
		    Width);	//converts to RGB
        }
        else if(Camera->VideoFormat==vfY422)	// Y422 mode 0 needs special handling.
        {				//calculate buffer positions
          const unsigned char *i = BufferData + (OffsetX+y*Width);	// Y canal is like 8 gray      
          YUV_RGB(j, i,
		    BufferData + (Height*Width) + OffsetX/2 + y*(Width/2),		    
                    BufferData + 3*(Height*Width)/2 + OffsetX/2 + y*(Width/2),		    
		    Width);	//converts to RGB        
        }
      }
    }
  }
	// Solve vertical orientation once per frame, it is faster.
  if(pConvertVideoLine)
  {
    if(VFlip ^ (pConvertVideoLine->Orientation==1)) // F,0-flip; F,1-nothing; -,0-nothing; -,1-flip
        Flip2D(ImageBuf);
    delete(pConvertVideoLine);
  }
  else
    if(VFlip) Flip2D(ImageBuf);
 
  if(HFlip) Flip1D(ImageBuf);	// flip image contents.
  InterlockedExchangeAdd(&ImageBuf->UsageCount, -READ_BARIER);
  return true;
}


/** THIS METHOD IS A PRETTY HACK. It was dependent on a screen resolution very much.
*I released this limitation a litle bit, but I am still unsure whether this works fine.
*   @param[out] x  - x coordinate of ball (In a bitmap. No further transform!)
*   @param[out] y  - y coordinate of ball (In a bitmap. No further transform!)   */
CameraTool::BALLSTATE CameraTool::BallSearch(int* x, int* y, bool VFlip, bool HFlip)
{
  int  i, j, k, imin, jmin, imax, jmax, isize, jsize, iedge, jedge, q;
  bool around;

  int size;
  IMediaSample *tmpMediaSample = NULL;
  const unsigned char *psample = (const unsigned char *)Camera->PeekImediaSample(&size,&tmpMediaSample);
  if(tmpMediaSample==NULL) return BALL_FAIL;
  if(psample==NULL || size<=0 ||
     size < (labs(Camera->GetWidth())*labs(Camera->GetHeight())*Camera->GetDepth())/8) // Check for insufficient buffer size.)
  {
    tmpMediaSample->Release();
    return BALL_FAIL;
  }
  if(!ReadImageAs(&IntImage,0x12,VFlip,HFlip,psample))
  {
    tmpMediaSample->Release();
    return BALL_FAIL;
  }
  // IntImage.Height = iHeight; IntImage.Width = iWidth;  

  const int STEP    =   5;                 //< ball search step
  const int AROUND  = (60*iWidth)/480;	   //< size of expected position area (was set to 30 for resolution 255x255)  
  const int BALLMIN = (12*iWidth)/480;     //< minimum ball size (was set to 6 for resolution 255x255)
  const int BALLMAX = (75*iWidth)/480;     //< maximum ball size (was set to 30 for resolution 255x255)
  const int IGNOR   = (iWidth<=480) ? 2 : ((2*iWidth)/480); //< max. black area inside ball

  const int XMIN =  0;                     //< ball search range
  int XMAX = IntImage.Size1D;
  const int YMIN =   0;
  int YMAX = IntImage.Size2D;

  around=found;
  found=0;

  IntImage.CheckX(lastx);
  IntImage.CheckY(lasty);
  *x=lastx;                                /*  return values if not found */
  *y=lasty;

  if (around)                              /* first try around expected position */
  {
    imin = expx-AROUND;
    jmin = expy-AROUND;
    imax = expx+AROUND;
    jmax = expy+AROUND;
  }

  do
  {
    if (!around)                             /* if not around, try whole plain */
    {
      imin=XMIN;
      imax=XMAX;
      jmin=YMIN;
      jmax=YMAX;
    }

    IntImage.CheckX(imin);		/* sanity check */
    IntImage.CheckX(imax);
    IntImage.CheckY(jmin);
    IntImage.CheckY(jmax);

    for (j=jmin; j<jmax && !found; j+=STEP)
    {
      Raster1DAbstract *R1Row = IntImage.GetRowRaster(j);
      if(R1Row==NULL) continue;

      for (i=imin; i<imax && !found; i+=STEP)      
      {							/* find first white point */
        if(!(found=(R1Row->GetValue1D(i)>0))) continue;
                                                         /* get ball size & centre */
        for (k=0,q=j; (k<IGNOR) && (q>YMIN); q--)        /* y <- */
        {
          if(IntImage.iswhite(i,q)) k=0;
          else
          {
            if(IntImage.iswhite(i+1,q)) continue;
            if(IntImage.iswhite(i-1,q)) continue;
            k++;
          }
        }
        jedge = q + IGNOR;

        for (k=0,q=j; (k<IGNOR) && (q<YMAX); q++)        /* y -> */
        {
          if(IntImage.iswhite(i,q)) k = 0;
          else
          {
            if(IntImage.iswhite(i+1,q)) continue;
            if(IntImage.iswhite(i-1,q)) continue;          
            k++;
          }
        }
        jsize = (q-IGNOR)-jedge+1;

        if (jsize>BALLMAX)                               /* if too big exit */
        {
          found=0;
          tmpMediaSample->Release();
          return(BALL_BIG);
        }

        for (k=0,q=i; (k<IGNOR) && (q>XMIN); q--)        /* x <- */
        {
          if(R1Row->GetValue1D(q)>0) k=0;
          else
          {
            if(IntImage.iswhite(q,j+1)) continue;
            if(IntImage.iswhite(q,j-1)) continue;
            k++;
          }
        }
        iedge = q+IGNOR;

        for(k=0,q=i; (k<IGNOR) && (q<XMAX); q++)        /* x -> */
        {
          if(R1Row->GetValue1D(q)>0) k=0;
          else
          {
            if(IntImage.iswhite(q,j+1)) continue;
            if(IntImage.iswhite(q,j-1)) continue;
            k++;
          }
        }
        isize = (q-IGNOR)-iedge+1;

        if (isize>BALLMAX)                               /* if too big exit */
        {
          found=0;
          tmpMediaSample->Release();
          return(BALL_BIG);
        }        

        if (isize<BALLMIN || jsize<BALLMIN) found=0;     /* if too small try again */
      }
    }
    around=!around;
  }
  while(!around);

  if (!found) {tmpMediaSample->Release();return(BALL_NONE);}

  i = iedge+isize/2;                                       /* compute ball centre to [i,j] */
  j = jedge+jsize/2;

  expx = i+(i-lastx);
  if(expx>XMAX-AROUND) expx=XMAX-AROUND;                /* compute expected position */
  if(expx<XMIN+AROUND) expx=XMIN+AROUND;

  expy = j+(j-lasty);
  if(expy>YMAX-AROUND) expy=YMAX-AROUND;
  if(expy<YMIN+AROUND) expy=YMIN+AROUND;

  lastx=i;
  lasty=j;

  *x=i;                             /* return values */
  *y=j;

  tmpMediaSample->Release();
  return(BALL_OK);
}
