/*========================================================================
    cmv_threshold.h : Color threshold support for CMVision2
  ------------------------------------------------------------------------
    Copyright (C) 1999-2002  James R. Bruce
    School of Computer Science, Carnegie Mellon University
  ------------------------------------------------------------------------
    This software is distributed under the GNU General Public License,
    version 2.  If you do not have a copy of this licence, visit
    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
    in the hope that it will be useful, but WITHOUT ANY WARRANTY,
    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  ========================================================================*/

#ifndef __CMV_THRESHOLD_H__
#define __CMV_THRESHOLD_H__

#include "../headers/system_config.h"

#if defined(PLATFORM_APERIOS) && !defined(PLATFORM_APERIOS_SDK)
#include <OFS/OFS.h>
#endif

#include "cmv_types.h"

#include "../headers/CircBufPacket.h"
#include "../headers/FileSystem.h"

//#define CALC_AVG_IMG_COLOR

extern double AvgImgColor[3];

extern PacketStream *TextOutputStream;

namespace CMVision{

template<int x,int y,int z>
class DummyI3 {
};

template<class T,int x,int y,int z>
class DummyT1I3 {
};

template <class cmap_t,class image,int bits_y,int bits_u,int bits_v>
void ThresholdImage(cmap_t *cmap,image &img,cmap_t *tmap,DummyI3<bits_y,bits_u,bits_v> dummy=DummyI3<bits_y,bits_u,bits_v>())
{
  // yuyv *buf,p;
  uyvy *buf,p;
  int i,m,size;

  int rshift_y,rshift_u,rshift_v;
  int lshift_y,lshift_u,lshift_v;

  rshift_y = 8 - bits_y;
  rshift_u = 8 - bits_u;
  rshift_v = 8 - bits_v;

  lshift_y = bits_u + bits_v;
  lshift_u = bits_v;
  lshift_v = 0;

  size = img.width * img.height;
  buf  = img.buf;

  for(i=0; i<size; i++){
    p = buf[i / 2];
    m = ((p.u >> rshift_u) << lshift_u) +
        ((p.v >> rshift_v) << lshift_v);
    cmap[i + 0] = tmap[m + ((p.y1 >> rshift_y) << lshift_y)];
    cmap[i + 1] = tmap[m + ((p.y1 >> rshift_y) << lshift_y)];
  }
}

#ifdef PLATFORM_LINUX
template <class cmap_t,class image,int bits_r,int bits_g,int bits_b>
void ThresholdImageRGB24(cmap_t *cmap,image &img,cmap_t *tmap)
{
  rgb *buf,p;
  int i,size;

  int rshift_r,rshift_g,rshift_b;
  int lshift_r,lshift_g,lshift_b;

  rshift_r = 8 - bits_r;
  rshift_g = 8 - bits_g;
  rshift_b = 8 - bits_b;

  lshift_r = bits_g + bits_b;
  lshift_g = bits_b;
  lshift_b = 0;

  size = img.width * img.height;
  buf  = img.buf;

  for(i=0; i<size; i++){
    p = buf[i];
    cmap[i] = tmap[((p.red >> rshift_r) << lshift_r) + ((p.green >> rshift_g) << lshift_g) + ((p.blue >> rshift_b) << lshift_b)];
  }
}
#endif

template <class cmap_t,class image>
void ThresholdImageRGB16(cmap_t *cmap,image &img,cmap_t *tmap)
{
  unsigned short *buf;
  int i,size;

  size = img.width * img.height;
  buf  = (unsigned short*)img.buf;

  for(i=0; i<size; i++){
    cmap[i] = tmap[buf[i]];
  }
}

// This function fixes the image to remove the information encoded in the bottom left of the image
// on the AIBOs by copying the line of the image directly above the bogus row onto the bogus row.
template <class image,class element>
void FixImageYUVPlanar(image &img)
{
  element *row_bogus,*row_good;

  row_good  = img.buf_y + (img.height-2)*img.row_stride;
  row_bogus = img.buf_y + (img.height-1)*img.row_stride;
  for(int col=0; col<16; col++){
    row_bogus[col] = row_good[col];
  }

  row_good  = img.buf_u + (img.height-2)*img.row_stride;
  row_bogus = img.buf_u + (img.height-1)*img.row_stride;
  for(int col=0; col<16; col++){
    row_bogus[col] = row_good[col];
  }

  row_good  = img.buf_v + (img.height-2)*img.row_stride;
  row_bogus = img.buf_v + (img.height-1)*img.row_stride;
  for(int col=0; col<16; col++){
    row_bogus[col] = row_good[col];
  }
}

template <class cmap_t,class image,class element,int bits_y,int bits_u,int bits_v>
void ThresholdImageYUVPlanar(cmap_t *cmap,image &img,cmap_t *tmap,DummyT1I3<element,bits_y,bits_u,bits_v> dummy=DummyT1I3<element,bits_y,bits_u,bits_v>())
{
  //int nonzero_cnt=0;

  // element *buf_y,*buf_u,*buf_v;
  int row,col;
  int width,height;
  int py,pu,pv;
  int tmap_idx;
#ifdef CALC_AVG_IMG_COLOR
  ulong total_y;
  ulong total_u;
  ulong total_v;
#endif

  int rshift_y,rshift_u,rshift_v;
  int lshift_y,lshift_u,lshift_v;

  element *row_y,*row_u,*row_v;
  cmap_t *row_cmap;

  rshift_y = 8 - bits_y;
  rshift_u = 8 - bits_u;
  rshift_v = 8 - bits_v;

  lshift_y = bits_u + bits_v;
  lshift_u = bits_v;
  lshift_v = 0;

  width  = img.width;
  height = img.height;
#ifdef CALC_AVG_IMG_COLOR
  total_y = 0;
  total_u = 0;
  total_v = 0;
#endif

  for(row=0; row<height; row++) {
    row_y = img.buf_y + row*img.row_stride;
    row_u = img.buf_u + row*img.row_stride;
    row_v = img.buf_v + row*img.row_stride;
    row_cmap = cmap + row*width;

    for(col=0; col<width; col++) {
      py = row_y[col] >> rshift_y;
      pu = row_u[col] >> rshift_u;
      pv = row_v[col] >> rshift_v;
      tmap_idx = 
        (py << lshift_y) +
        (pu << lshift_u) +
        (pv << lshift_v);
      row_cmap[col] = tmap[tmap_idx];
#ifdef CALC_AVG_IMG_COLOR
      total_y += row_y[col];
      total_u += row_u[col];
      total_v += row_v[col];
#endif

      /*
      if(row==height/2 && col==width/2) {
        pprintf(TextOutputStream,"py=%u pu=%u pv=%u tmap_idx=%d tmap val=%u\n",py,pu,pv,tmap_idx,tmap[tmap_idx]);
      }
      */
    }
  }

#ifdef CALC_AVG_IMG_COLOR
 {
   double size=(double)(width*height);
   AvgImgColor[0] = total_y/size;
   AvgImgColor[1] = total_u/size;
   AvgImgColor[2] = total_v/size;
 }
#endif

  //for(row=0; row<height; row++) {
  //  for(col=0; col<width; col++) {
  //    uchar c;
  //    c = cmap[row*width + col];
  //    if(c >= 9) {
  //      cout << "p" << (void *)&cmap[row*width+col] << " r" << row << " c" << col << " color" << c << endl;
  //    }
  //  }
  //}
}

template<class cmap_t>
void ColorizePixel(cmap_t *cmap,int i,int j,int width,int height)
{
  int toplx = i-1, toply = j-1;
  int x,y, len = 3, c=0;
  int p1count = 0, p2count = 0;

  cmap_t val;
  uchar dval1,dval2; // definite values matching possible values
  uchar pval1,pval2;

  val = cmap[j*width + i];
  pval1 = val & 0xF;
  pval2 = val >> 4;

  dval1 = pval1 << 4 | pval1;
  if(dval1 == 0)
    dval1 = 0xFFU;
  dval2 = pval2 << 4 | pval2;
  if(dval2 == 0)
    dval2 = 0xFFU;
  
  while(c++ < 10){
    //Scan horizontally
    for(x = toplx; x < toplx+len; x++){
      //Scan top row
      y = toply;

      //In bounds
      if(x >= 0 && x < width && y >= 0 && y < height){
        cmap_t pix_val;
        pix_val = cmap[y*width + x];
        if(pix_val == dval1){
          p1count++;
          if(p1count > 5){
            cmap[j*width + i] = dval1;
            return;
          }
        }
        if(pix_val == dval2){
          p2count++;
          if(p2count > 5){
            cmap[j*width + i] = dval2;
            return;
          }
        }
      }

      //Scan bottom row
      y = toply+len-1;

      //In bounds
      if(x >= 0 && x < width && y >= 0 && y < height){
        cmap_t pix_val;
        pix_val = cmap[y*width + x];
        if(pix_val == dval1){
          p1count++;
          if(p1count > 5){
            cmap[j*width + i] = dval1;
            return;
          }
        }
        if(pix_val == dval2){
          p2count++;
          if(p2count > 5){
            cmap[j*width + i] = dval2;
            return;
          }
        }
      }
    }

    //Scan vertically
    for(y = toply; y < toply+len; y++){
      //Scan left side
      x = toplx;

      //In bounds
      if(x >= 0 && x < width && y >= 0 && y < height){
        cmap_t pix_val;
        pix_val = cmap[y*width + x];
        if(pix_val == dval1){
          p1count++;
          if(p1count > 5){
            cmap[j*width + i] = dval1;
            return;
          }
        }
        if(pix_val == dval2){
          p2count++;
          if(p2count > 5){
            cmap[j*width + i] = dval2;
            return;
          }
        }
      }

      //Scan right side
      x = toplx+len-1;

      //In bounds
      if(x >= 0 && x < width && y >= 0 && y < height){
        cmap_t pix_val;
        pix_val = cmap[y*width + x];
        if(pix_val == dval1){
          p1count++;
          if(p1count > 5){
            cmap[j*width + i] = dval1;
            return;
          }
        }
        if(pix_val == dval2){
          p2count++;
          if(p2count > 5){
            cmap[j*width + i] = dval2;
            return;
          }
        }
      }
    }

    toplx -= 1; toply -= 1;
    len += 2;
  }
}

template<class cmap_t>
void Disambiguate(cmap_t *cmap,int width,int height)
{
  int i,j;

  for(i=0;i<width;i++){
    for(j=height-1;j>=0;j--){
      cmap_t val;
      uchar pval1,pval2;
      val = cmap[j*width + i];
      pval1 = val & 0xF;
      pval2 = val >> 4;
      if(pval1 != pval2)
        ColorizePixel(cmap,i,j,width,height);
    }
  }
}

template<class cmap_t>
void HardDisambiguate(cmap_t *cmap,int width,int height)
{
  int i,j;

  for(i=0; i<width; i++){
    for(j=height-1; j>=0; j--){
      cmap_t val;
      val = cmap[j*width + i];
      val &= 0xF;
      cmap[j*width + i] = val;
    }
  }
}

template <class cmap_t,class image,class element,int bits_y,int bits_u,int bits_v>
void ThresholdImageYUVPlanarWithAmbiguous(cmap_t *cmap,image &img,cmap_t *tmap,DummyT1I3<element,bits_y,bits_u,bits_v> dummy=DummyT1I3<element,bits_y,bits_u,bits_v>())
{
  ThresholdImageYUVPlanar<cmap_t,image,element,bits_y,bits_u,bits_v>(cmap,img,tmap);
 
  for(int i=0; i<5; i++)
    Disambiguate(cmap,img.width,img.height);

  HardDisambiguate(cmap,img.width,img.height);
}

template <class cmap_t,class image,class element,int bits_y,int bits_u,int bits_v>
void ThresholdAndCorrectImageYUVPlanar(cmap_t *cmap, // buffer to write the color map into
                                       image &img,   // the source image
                                       image &mask,  // A mask to correct for color distortion
                                       cmap_t *tmap, // Our lookup table of (y,u,v) => symbolic color
                                       DummyT1I3<element,bits_y,bits_u,bits_v> dummy =
                                       DummyT1I3<element,bits_y,bits_u,bits_v>())
{
  int row,col;
  int width,height;
  int py,pu,pv;
  int tmap_idx;

  // Shift <pixel value> * <mask> right this many bits to convert back
  // to an integer value after approximating a floating point multiply
  // with integers. 2^mask_shift = 1.0 in the mask.
  const int mask_shift = 7;

#ifdef CALC_AVG_IMG_COLOR
  ulong total_y;
  ulong total_u;
  ulong total_v;
#endif

  int rshift_y,rshift_u,rshift_v;
  int lshift_y,lshift_u,lshift_v;

  element *row_y,*row_u,*row_v;
  element *mask_y,*mask_u,*mask_v;
  cmap_t *row_cmap;

  // How far we shift the values of Y, U, and V right
  // in order to get the portion that will go into
  // calculating our offset in the lookup table
  rshift_y = 8 - bits_y;
  rshift_u = 8 - bits_u;
  rshift_v = 8 - bits_v;

  // How far we shift each component to the left in order
  // to OR them together and get our offset into the 16-bit
  // table (or however many bits you want to use)
  lshift_y = bits_u + bits_v;
  lshift_u = bits_v;
  lshift_v = 0;

  width  = img.width;
  height = img.height;

#ifdef CALC_AVG_IMG_COLOR
  total_y = 0;
  total_u = 0;
  total_v = 0;
#endif

  // Remember that we're dealing with planar images. We have runs
  // of width Y values, width U values, width V values that
  // repeat.

  for(row=0; row<height; row++) {
    // Get pointers to the start of the runs for the
    // current row.
    row_y = img.buf_y + row*img.row_stride;
    row_u = img.buf_u + row*img.row_stride;
    row_v = img.buf_v + row*img.row_stride;

    // We'll also need to grab mask values for our multiplication.
    mask_y = mask.buf_y + row*mask.row_stride;
    mask_u = mask.buf_u + row*mask.row_stride;
    mask_v = mask.buf_v + row*mask.row_stride;

    // This is our place in the output buffer for this
    // particular row.
    row_cmap = cmap + row*width;
    
    // mask_col should be equal to 2*col at the top of
    // the loop each time. We use a separate counter
    // because the mask is twice as wide as the image.
    int mask_col = 0;

    for(col=0; col<width; col++) {
      // Grab our components
      py = row_y[col];
      pu = row_u[col];
      pv = row_v[col];

      // Now we correct for color distortion by multiplying
      // by a different factor for each pixel. We are using
      // an integer multiplication for speed. In our little
      // world, 128 is equal to 1.0. (We'll right shift by 7
      // bits at the end to cancel this out).

      // Note that we multiply our column by 2 because the mask is
      // twice the width of the image. Odd columns are the additive
      // factor. Even multiplicative.
      py *= mask_y[mask_col]; // [2*col];
      pu *= mask_u[mask_col]; // [2*col];
      pv *= mask_v[mask_col]; // [2*col];

      // Move mask_col over to get at the B bit of the correction
      mask_col++;

      // Divide by 128 to get back to the appropriate result after
      // the multiply.
      py = py >> mask_shift;
      pu = pu >> mask_shift;
      pv = pv >> mask_shift;

      // Now we add in our additive bit. These values are also
      // encoded. Y and U are signed chars, so we need to make
      // sure we get their sign back from the mask. V is
      // unsigned, but it's been negated - it should range from
      // -255 to 0.
      char y_b = (char)mask_y[mask_col];  // [2*col+1];
      char u_b = (char)mask_u[mask_col];  // [2*col+1];
      int v_b = -mask_v[mask_col];  // [2*col+1];
      py += y_b;
      pu += u_b;
      pv += v_b;

      // We'll want mask_col to point at the A term for the next
      // iteration of the loop
      mask_col++;

      // We need to clamp our values back in the normal range for
      // a byte. Please, Mr. Compiler, please use a conditional
      // move instead of a jump.

      if(py < 0) py = 0;
      if(py > 255) py = 255;

      if(pu < 0) pu = 0;
      if(pu > 255) pu = 255;

      if(pv < 0) pv = 0;
      if(pv > 255) pv = 255;

      // From here on out, this is normal planar thresholding.
      // We right shift to get the correct number of significant
      // bits and mush them all together to get an index into
      // our color table.

      py = py >> rshift_y;
      pu = pu >> rshift_u;
      pv = pv >> rshift_v;

      // Lookup the symbolic color
      tmap_idx = 
        (py << lshift_y) +
        (pu << lshift_u) +
        (pv << lshift_v);
      
      // Write the symbolic color into our buffer
      row_cmap[col] = tmap[tmap_idx];

#ifdef CALC_AVG_IMG_COLOR
      total_y += row_y[col];
      total_u += row_u[col];
      total_v += row_v[col];
#endif
    }
  }

#ifdef CALC_AVG_IMG_COLOR
 {
   double size=(double)(width*height);
   AvgImgColor[0] = total_y/size;
   AvgImgColor[1] = total_u/size;
   AvgImgColor[2] = total_v/size;
 }
#endif
}

//#define ENABLE_JOIN_NEARBY

template <class rle_t,class color_class_state_t>
void RmapToRgb(rgb *img,rle_t *map,int last_run,int width,int height,
		color_class_state_t *color,int num)
{
  int i,x,y=0,next_x;

  i=0;
  next_x=0;
#ifdef ENABLE_JOIN_NEARBY
  i=AdvanceToNextRun(i,map);
#endif  
  while(i < last_run) {
    rle_t *run;
    run = &map[i];
    
    y=run->y;
    if(y>=height) {
#ifdef PLATFORM_LINUX
      printf("run past height, i=%d last_run=%d y=%d\n",i,last_run,y);
#endif
      return;
    }

    x=run->x;
    if(x<next_x) {
#ifdef PLATFORM_LINUX
      printf("run jumps backwards, i=%d last_run=%d min_x %d x %d y=%d\n",i,last_run,next_x,x,y);
#endif
      return;
    }

    if(x!=next_x) {
      for(x=next_x; x<run->x; x++)
        img[y*width + x] = color[0].color;
    }

    next_x = run->x+run->width;
    for(x=run->x; x<next_x; x++)
      img[y*width + x] = color[run->color].color;

    if(next_x == width) {
      y++;
      next_x = 0;
    }

    i=i+1;//AdvanceToNextRun(i,map);
  }
  if(y < height)
    for(x=next_x; x<width; x++)
      img[y*width + x] = color[0].color;
}

template <class cmap_t>
void RgbToIndex(cmap_t *map,rgb *img,int width,int height,
		rgb *colors,int num)
{
  int i,j,size;

  size = width * height;

  j = 0;
  for(i=0; i<size; i++){
    if(img[i] != colors[j]){
      j = 0;
      while(j<num && img[i]!=colors[j]) j++;
#ifdef PLATFORM_LINUX
      //if(j==num){
      //  printf("unknown pixel found at x %d y %d r %d g %d b %d\n",i%width,i/width,
      //         img[i].red,img[i].green,img[i].blue);
      //  j=0;
      //  while(j<num && img[i]!=colors[j]){
      //    printf("unknown pixel found at (%d,%d)=(%d,%d,%d), comparing with j=%d (%d,%d,%d)\n",
      //           i%width,i/width,
      //           img[i].red,img[i].green,img[i].blue,
      //           j,colors[j].red,colors[j].green,colors[j].blue);
      //    j++;
      //  }
      //}
#endif
      if(j==num)
        j = 0;
    }
    map[i] = j;
  }
}

template <class cmap_t,class color_class_state_t>
void IndexToRgb(rgb *img,cmap_t *map,int width,int height,
		color_class_state_t *color,int num)
{
  int i,size;

  size = width * height;

  for(i=0; i<size; i++){
    img[i] = color[map[i]].color;
  }
}

template <class cmap_t>
void IndexToRgb(rgb *img,cmap_t *map,int width,int height,
		rgb *colors,int num)
{
  int i,size;

  size = width * height;

  for(i=0; i<size; i++){
    img[i] = colors[map[i]];
  }
}

template <class data>
data Get3D(data *arr,int num_i,int num_j,int num_k,int i,int j,int k)
{
  int l;
  l = i*num_j*num_k + j*num_k + k;
  return(arr[l]);
}

template <class data>
void Set3D(data *arr,int num_i,int num_j,int num_k,int i,int j,int k,data v)
{
  int l;
  l = i*num_j*num_k + j*num_k + k;
  arr[l] = v;
}

template <class tmap_t>
int RemapTMapColor(tmap_t *tmap,int num_y,int num_u,int num_v,int src_id,int dest_id)
{
  int i,n,size;

  size = num_y * num_u * num_v;
  n = 0;

  for(i=0; i<size; i++){
    if(tmap[i] == src_id){
      tmap[i] = dest_id;
      n++;
    }
  }

  return(n);
}

template <class tmap_t>
int CheckTMapColors(tmap_t *tmap,int num_y,int num_u,int num_v,int colors,int default_id)
{
  int i,n,size;

  size = num_y * num_u * num_v;
  n = 0;

  for(i=0; i<size; i++){
    if(tmap[i] >= colors){
      tmap[i] = default_id;
      n++;
    }
  }

  return(n);
}

template <class tmap_t>
bool LoadThresholdFile(tmap_t *tmap,int num_y,int num_u,int num_v,char *filename)
{
  HFS::FILE *in;
  char buf[256];
  int ny,nu,nv;
  int size,read;

  in = HFS::fopen(filename,"r");
  if(!in) return(false);

  // read magic
  if(!HFS::fgets(buf,256,in)) goto error;
  buf[4] = 0;
  if(strcmp(buf,"TMAP")) goto error;

  // read type (ignore for now)
  if(!HFS::fgets(buf,256,in)) goto error;

  // read size
  if(!HFS::fgets(buf,256,in)) goto error;
  ny = nu = nv = 0;
  sscanf(buf,"%d %d %d",&ny,&nu,&nv);
  if(num_y!=ny || num_u!=nu || num_v!=nv) goto error;

  size = num_y * num_u * num_v;
  read = HFS::fread(tmap,sizeof(tmap_t),size,in);

  HFS::fclose(in);

  return(read == size);
error:
  if(in) HFS::fclose(in);
  return(false);
}

template <class tmap_t>
bool SaveThresholdFile(tmap_t *tmap,int num_y,int num_u,int num_v,char *filename)
{
  FILE *out;
  int size,wrote;

  out = fopen(filename,"w");
  if(!out) return(false);

  fprintf(out,"TMAP\nYUV%d\n%d %d %d\n",
          sizeof(tmap_t),num_y,num_u,num_v);
  size = num_y * num_u * num_v;
  wrote = fwrite(tmap,sizeof(tmap_t),size,out);
  fclose(out);

  return(wrote == size);
}

} // namespace

#endif