#include <stdafx.h>
#include <windows.h>
#include <stdio.h>
#include "common.h"
#include "basic.h"

#include "totacd.h"

/* TOTACD(unsigned char**,unsigned char**,int,int,unsigned char**,int,int,int*,int*,double)*/
void TOTACD(unsigned char** img,int imgrow,int imgcol,unsigned char** subimg,
       unsigned char** map,int maprow,int mapcol,int* lur,int* luc,double w)
/*unsigned char** img;     /*  the current frame image  */
/*int imgrow;              /*  the image's row size     */
/*int imgcol;              /*  the image's column size  */
/*unsigned char** subimg;  /*  the subregion on the previous frame's gate position */  
/*unsigned char** map;     /*  the template used on the previous frame */
/*int maprow;              /*  the template's row size  */
/*int mapcol;              /*  the template's column size */
/*int* lur;            /*  the previous frame's gate position  */
/*int* luc;            /*  the previous frame's gate position  */
/*double w;            /*  weigthing coefficent                */
{
   int i,j;
   double dr;  /* the track error along row direction */
   double dc;  /* the track error along column direction */
   
   /* Update the template with the subregion of the previous frame image  */
   /* the template and the subregion have the same size                   */
   for(i=0; i<maprow; i++)
   for(j=0; j<mapcol; j++)
   {
      map[i][j] = (unsigned char)((1-w)*map[i][j] + w*subimg[i][j]);
   }

   /* get the subregion on the current frame image */
   GetSubimg(img,imgrow,imgcol,subimg,map,maprow,mapcol,*lur,*luc);
   
   /* calculate the track error   */
   LocateXY(subimg,map,maprow,mapcol,&dr,&dc);
   TRACE("dr=%f, dc=%f\n",dr,dc);
   /* update the current frame's gate position   */      
   *lur = (int)(*lur-dr);
   *luc = (int)(*luc-dc);
   
}

void GetSubimg(unsigned char** img,int imgrow,int imgcol,unsigned char** subimg,
       unsigned char** map,int maprow,int mapcol,int lur,int luc)
/*unsigned char** img;
int imgrow;
int imgcol;
unsigned char** subimg;
unsigned char** map;
int maprow;
int mapcol;
int lur;
int luc;   */
{
   int i,j;
   
   for(i=0; i<maprow; i++)
   for(j=0; j<mapcol; j++)
   {
      subimg[i][j] = img[i+lur][j+luc];
   }
}

/* Update the template(map) with the subregion(subimg) of the image  */
/* the template and the subregion have the same size                 */
void UpdateTemplate(unsigned char** map,unsigned char** subimg,int row,int col,double w)
/*unsigned char** map;      /* the template            */
/*unsigned char** subimg;   /* the subregion from which the update information come */
/*int row;                  /* template's row size     */
/*int col;                  /* template's column size  */
/*double w;                  /* weighting coefficent    */
{
   int i,j;             /* repetition variable   */
   
   /* update the template    */
   for(i=0; i<row; i++)
   for(j=0; j<col; j++)
   {
      map[i][j] = (unsigned char)((1-w)*map[i][j] + w*subimg[i][j]);
   }
}
  
int LocateXY(unsigned char** map,unsigned char** subimg,int row,int col,double *dr,double *dc)
/*unsigned char** map;    /* the template of the current frame */
/*unsigned char** subimg; /* the iamge signal to be locate accurately */
/*int row;   /* the template and image signal's row size  */
/*int col;    /* column size  */
/*double *dr;  /* the track error along row direction */
/*double *dc;  /* the track error along column direction */
{
   double** Wer; /* the derivative of the template along row direction */
   double** Wec; /* the derivative of the template along column direction */
   double Cer,Cec;   /* the Ce correspond to row & column direction respectively */
   int i,j;
   double tem;    /* temporary variable  */

   /* calculate the template's derivative along the two direction respectively */
   /* allocate the memmory to the array Wer&Wec  */
   Wer = (double**)fspace_2d(row,col,sizeof(double));
   if(!Wer) return 0;
   Wec = (double**)fspace_2d(row,col,sizeof(double));
   if(!Wec) return 0;
   Derivative(map,Wer,Wec,row,col);

   /* calculate the Ce  */
   Cer = Cec = 0.0;
   for(i=0; i<row; i++)
   for(j=0; j<col; j++)
   {
      Cer += Wer[i][j]*Wer[i][j];
      Cec += Wec[i][j]*Wec[i][j];
   }
   
   /* calculate the track error  */
   *dr = *dc = 0.0;
   for(i=0; i<row; i++)   
   for(j=0; j<col; j++)
   {
      tem = subimg[i][j]-map[i][j];
      *dr += -Wer[i][j]*tem;
      *dc += -Wec[i][j]*tem;
   }
   *dr = *dr/Cer;
   *dc = *dc/Cec;

   return 1;
}


void Derivative(unsigned char** img,double** Der,double** Dec,int row,int col)
/*unsigned char** img; /* the input image  */
/*double** Der;  /* the derivative of image along row direction */
/*double** Dec;  /* the derivative of image along column direction */
/*int row;       /* the image's row size */
/*int col;        /* the image's column size */
{
   int i,j;  /* the repetition vriables  */
   
   /* reset Der&Dec to zero  */
   for(i=0; i<row; i++)
   for(j=0; j<col; j++)
   {
      Der[i][j] = Dec[i][j] = 0.0;
   }
   
   /* calculate the derivative  */
   /* row direction.... */
   for(j=0; j<col; j++)
   {
      for(i=1; i<row-1; i++) Der[i][j] = img[i+1][j] - img[i][j];
   }
   /* column direction.... */
   for(i=0; i<row; i++)
   {
      for(j=1; j<col-1; j++) Dec[i][j] = img[i][j+1] - img[i][j];
   }

}