YARPOrientation::YARPOrientation(const YARPImageOf<YarpPixelMono> &image)
{
    nAng = image.GetHeight() ;
    nEcc = image.GetWidth() ;
    rfMin = 0.31 ;
    initialise() ;
}

int YARPLogpolar::ReconstructGrays (const YARPImageOf<YarpPixelMono>& in, YARPImageOf<YarpPixelMono>& out)
{
	using namespace _logpolarParams;

	Reconstruct_Grays((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), _srho, _stheta, _img.padding, _weightsMap, _img.Pix_Numb);
	return YARP_OK;
}

//
// only find object_no.
//
int YARPObjectContainer::Segment (int object_no, YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& xx, int& yy)
{
	if (!m_active)
	{
		printf ("YARPObjectContainer: need to update stats first\n");
		out.PeerCopy(scan);
		xx = yy = 0;
		return -1;
	}

	double x, y, quality;
	m_locator[object_no].BackProject (scan, m_backp[object_no]);

	double ex, ey;
	m_locator[object_no].GetExtent (ex, ey);

	ex *= SCALE;
	ey *= SCALE;

	bool valid = false;

	if (m_locator[object_no].Find (ex, ey, x, y, quality) >= 0)
	{
		double mean = 0, stddev = 0;
		const double THR = 4.0;		// it was 2.0
		m_locator[object_no].GetExpectancy (mean, stddev);

		valid = (fabs(quality - mean) < stddev * THR) ? true : false;

#ifdef _DEBUG
		printf ("object: %d location: %lf %lf q: %lf\n", object_no, x, y, quality);
#endif
	}

	if (valid)
	{
		YarpPixelBGR red;
		red.r = 255;
		red.g = red.b = 0;

		double betterx = x;
		double bettery = y;
		AddRectangle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));

		//AddCircleOutline (m_backp[object_no], red, int(max_x+.5), int(max_y+.5), 10);
		AddCircle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), 5);

		// return processed image.
		out.PeerCopy (m_backp[object_no]);

		xx = int (betterx + .5);
		yy = int (bettery + .5);
	}
	else
	{
		xx = yy = 0;
	}

	return (valid) ? 0 : -1;
}

void YARPLpShifter::GetShiftedImage(const YARPImageOf<YarpPixelRGB>& im1, YARPImageOf<YarpPixelRGB>& dst, double * shift)
{
	unsigned char **src1p0 = (unsigned char **)im1.GetArray();
	unsigned char **src2p0 = (unsigned char **)dst.GetArray();

	// shift (xi, eta);
	int xi = int (shift[0] + .5);
	int eta = int (shift[1] + .5);
	assert (xi >= 0 && xi < lut_size && eta >= 0 && eta < nAng);

	int i,j,i1,j1,jt;
	for(i=0; i<nEcc; i++)
		for(j=0; j<nAng; j++)
		{
			jt = (j + eta) % nAng;
		   	i1 = lut[xi][i][jt].ecc;
			j1 = lut[xi][i][jt].ang;
			j1=(j1 + nAng - eta) % nAng;
			if (i1>=0) 
			{
				src2p0[j][i*3] = src1p0[j1][i1*3];
				src2p0[j][i*3+1] = src1p0[j1][i1*3+1];
				src2p0[j][i*3+2] = src1p0[j1][i1*3+2];
			}
			else 
			{
				src2p0[j][i*3] = 0;
				src2p0[j][i*3+1] = 0;
				src2p0[j][i*3+2] = 0;
			}
		}
}

int 
YARPFlowTracker::CenterOfMass (YARPImageOf<YarpPixelMono>& in, int& x, int& y)
{
	double xx = 0, yy = 0;
	int count = 0;

	for (int i = 0; i < in.GetHeight(); i++)
		for (int j = 0; j < in.GetWidth(); j++)
		{
			if (in(j, i) != 0)
			{
				xx += j;
				yy += i;
				count ++;
			}
		}

	if (count != 0)
	{
		x = int(xx / count + .5);
		y = int(yy / count + .5);
	}
	else
	{
		x = in.GetWidth()/2;
		y = in.GetHeight()/2;
	}

	return 0;
}

int YARPLogpolar::Uniform2Sawt(const YARPImageOf<YarpPixelBGR>& in, YARPImageOf<YarpPixelBGR>& out)
{
	using namespace _logpolarParams;

	uniform2Sawt((unsigned char *)out.GetRawBuffer(), (unsigned char *)in.GetRawBuffer(), &_img, _padMap);
	return YARP_OK;
}

void YARPImageTrackTool::Apply(YARPImageOf<YarpPixelBGR>& src)
{
	YARPImageOf<YarpPixelBGR> nextImg, blah;
	int ox, oy;

	if (src.GetWidth() != IMG_W || src.GetHeight() != IMG_H)
	{
		printf("Image tracking code is old, and specific to %dx%d images\n", IMG_W, IMG_H);
		exit(1);
	}

	nextImg.Refer(src);
	if (first)
	{
		first = 0;
		ResetXY();
		prevImg3.PeerCopy(nextImg);
		delta = 0;
	}
	else
	{
		ox = tx;
		oy = ty;
		ImgInt3& ii1 = *((ImgInt3 *)prevImg3.GetRawBuffer());
		ImgInt3& ii2 = *((ImgInt3 *)src.GetRawBuffer());
		Apply(ii1, ii2, tx, ty);
		if (ox != tx || oy != ty || delta)
		{
			prevImg3.PeerCopy(nextImg);
			delta = 0;
		}
	}
}

void YARPLpHistoSegmentation::Apply(YARPImageOf<YarpPixelHSV> &src)
{
	int i;
	int j;
	unsigned char *h;
	unsigned char *s;
	unsigned char *v;
			
	for(i = 0; i < src.GetHeight(); i++)
	{
		h = (unsigned char *) src.GetArray()[i];
		s = h+1;
		v = h+2;

		double w = pSize(1, i);
		for(j = 0; j < src.GetWidth(); j++)
		{
			if (_checkThresholds(*h,*s,*v))
				YARP3DHistogram::Apply(*h, *s, 0, w);
						
			h += 3;
			s += 3;
			v += 3;
		}
	}
}

int main(int argc, const char *argv[])
{
  const char *name = DEFAULT_NAME;
  char buf[256];
  if (argc>1)
    {
      name = argv[1];
    }
  sprintf(buf, "%s/i:img", name);
  in_img.Register(buf);
  sprintf(buf, "%s/o:img", name);
  out_img.Register(buf);

  sprintf(buf, "%s/i:mix", name);
  in_mix.Register(buf);
  sprintf(buf, "%s/o:box", name);
  out_data.Register(buf);
  
  box_thread.Begin();
  
  while(1)
    {
      in_img.Read();
      YARPImageOf<YarpPixelBGR> in;
      YARPImageOf<YarpPixelBGR> out;
      in.Refer(in_img.Content());
      out_img.Content().SetID(YARP_PIXEL_BGR);
      SatisfySize(in,out_img.Content());
      out.Refer(out_img.Content());
      Filter(in,out);
      out_img.Write();
//      YARPTime::DelayInSeconds(1000000);
    }
  return 0;
}

//
// helper.
int 
YARPFlowTracker::GrowMask (const YARPImageOf<YarpPixelMono>& src, YARPImageOf<YarpPixelMono>& dest)
{
	// block filter 7x7
	const int w = segmentation_mask.GetWidth();
	const int h = segmentation_mask.GetHeight();
	unsigned char *ss = (unsigned char *)src.GetRawBuffer();

	dest.Zero();

	// save in +2,+2...
	unsigned char *s[7];
	for (int ll = 0; ll < 7; ll++)
		s[ll] = ss+ll*w;

	int accum = 0;

	for (int i = 0; i < h-7; i++)
	{
		for (int j = 0; j < w-7; j++)
		{
			accum = 0;
			for (int k = 0; k < 7; k++)
			{
				accum += s[0][k];
				accum += s[1][k];
				accum += s[2][k];
				accum += s[3][k];
				accum += s[4][k];
				accum += s[5][k];
				accum += s[6][k];
			}

			accum /= 49;
			accum = (accum >= 64) ? 255 : 0;
		
			dest (j+3, i+3) = accum;

			s[0] ++;
			s[1] ++;
			s[2] ++;
			s[3] ++;
			s[4] ++;
			s[5] ++;
			s[6] ++;
		}

		ss += w;
		s[0] = ss;
		s[1] = s[0]+w;
		s[2] = s[1]+w;
		s[3] = s[2]+w;
		s[4] = s[3]+w;
		s[5] = s[4]+w;
		s[6] = s[5]+w;
	}

	return 0;
}

void YARPImageUtils::PasteInto (const YARPImageOf<YarpPixelMono>& src, int x, int y, int zoom, YARPImageOf<YarpPixelMono>& dst)
{
	char *bs = dst.GetRawBuffer ();

	IplImage *ipl = src.GetIplPointer ();
	const int dh = ipl->height;
	const int dw = ipl->width;
	char *dsY = ipl->imageData;

	int depth = dst.GetPixelSize ();
	ACE_ASSERT (depth == ipl->nChannels);	// same # of chan.

	const int h = dst.GetHeight();
	ACE_ASSERT (h >= dh);			// same height.
    const int w = dst.GetWidth();
	ACE_ASSERT (w >= dw);			// same width.

	const int rem_w = w - dw;

	// crude limit check.
	ACE_ASSERT (dw * zoom + x < w);
	ACE_ASSERT (dh * zoom + y < h);

	if (zoom == 1)
	{
		bs += (y * w);
		for (int i = 0; i < dh; i++)
		{
			memcpy (bs + x, dsY, dw);

			bs += w;
			dsY += dw;
		}
	}
	else
	{
		bs += (y * w);
		for (int i = 0; i < dh; i++)
		{
			char * st_row = bs;
			bs += x;
			for (int j = 0; j < dw; j++)
			{
				for (int k = 0; k < zoom; k++)
				{
					*bs++ = *dsY;
				}
				dsY++;
			}

			for (int k = 1; k < zoom; k++)
				memcpy (st_row + x + w * k, st_row + x, dw * zoom); 

			bs = st_row + w * zoom;
		}
	}
}

void YARPGaussianFeatures::Apply (const YARPImageOf<YarpPixelMono>& in)
{
	assert (in.GetPadding() == 0);

	for (int i = 1; i <= m_sigmas; i++)
	{
		m_features(i) = SpecialConvolveX (m_coeffs[i-1], (const unsigned char *)in.GetAllocatedArray());
		m_features(i+m_sigmas) = SpecialConvolveY (m_coeffs[i-1], (const unsigned char *)in.GetAllocatedArray());
	}
}

int YARPOrientation::Resize(const YARPImageOf<YarpPixelMono> &image)
{
    cleanup() ;

    nEcc = image.GetWidth() ;
    nAng = image.GetHeight() ;

    initialise() ;

    return true ;
}

bool FindCentroid (YARPImageOf<YarpPixelMono>& img, int *x, int *y)
{
	char *ptri = img.GetRawBuffer ();
	const int w = img.GetWidth ();
	const int h = img.GetHeight ();

	unsigned char max = 0;

	unsigned char *tmp = (unsigned char *)ptri;
	for (int i = 0; i < w * h; i++, tmp++)
		if (*tmp > max)
		{
			max = *tmp;
		}

	int count = 0;
	*x = 0;
	*y = 0;

	for (i = 0; i < h; i++)
	{
		unsigned char *row = (unsigned char *)ptri + i * w;
		for (int j = 0; j < w; j++, row++)
		{
			if (*row == max)
			{
				*x += j;
				*y += i;
				count ++;
			}
		}
	}

	if (count != 0)
	{
		*x /= count;
		*y /= count;
	}
	else
	{
		*x = img.GetWidth() / 2;
		*y = img.GetHeight() / 2;
	}

	if (max > 0)
		return true;
	else
		return false;
}

void Filter(YARPImageOf<YarpPixelBGR>& src,
	    YARPImageOf<YarpPixelBGR>& dest)
{
  FiveBoxesInARow& boxes = out_data.Content();
  YARPImageOf<YarpPixelMono> mono;
  mono.CastCopy(src);
  dest.PeerCopy(src);
  trackers.Update(mono,dest,boxes);
  out_data.Write();
  /*
  static ImgTrackTool track;
  dest.PeerCopy(src);
  track.Apply(dest);
   */
}

void YARPLpConicFitter::plotCircle(int T0, int R0, double R, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
	int theta;
	int rho;
	double r0;
	r0 = _moments.CsiToRo(R0);
	for(theta = 0; theta < _logpolarParams::_stheta; theta++)
	{
		double c = cos((theta-T0)/_q);
		double DELTA = (r0*r0*(c*c-1) + R*R);
		if (DELTA>=0)
		{
			int r = (int) ((r0*c+sqrt(DELTA)) + 0.5);
			if (r > 0)
			{
				rho = _moments.RoToCsi(r);
				if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
					output(theta,rho) = v;
			}
			
			r = (int) ((r0*c-sqrt(DELTA)) + 0.5);
			if (r > 0)
			{
				rho = _moments.RoToCsi(r);
				if ( (rho<=(_logpolarParams::_srho-1)) && (rho>0) )
						output(theta,rho) = v;
			}
		}
	}
	// plot center
	output.SafePixel(T0, R0) = v;
}

int YARPOrientation::centroid(const YARPImageOf<YarpPixelMono> &image,
                              double &x, double &y)
{
    x = 0 ;
    y = 0 ;
    
    char**
        pixels = image.GetArray() ;

    double
        numeratorX = 0.0,
        numeratorY = 0.0,
        area = 0.0 ;
    
    for (int i = 0; i < nAng; i++)
    {
        for (int j = 0; j < nEcc; j++)
        {
            if ((unsigned char)pixels[i][j] > 250)
            {
                area += fabs(jacobian[j]) ;
                numeratorX += power[j] * ac[i] * jacobian[j] ;
                numeratorY += power[j] * as[i] * jacobian[j] ;
            }
        }
    }

    x = numeratorX / area ;
    y = numeratorY / area ;

    return true ;
}

void YARPConicFitter::plotEllipse(int X0, int Y0, double a11, double a12, double a22, YARPImageOf<YarpPixelBGR> &output, const YarpPixelBGR &v)
{
	int t;
	float theta = 0.0;
	const int nThetaS = 200;
	const float deltaTh= (float) PI/ (double) nThetaS;
	float x,y,r;
	
	if ( !_checkDet(a11, a12, a22) )
	{
		// sorry, not an ellipse...
		return;
	}

	for(t = 0; t < nThetaS; t++)
	{
		theta = deltaTh*t;
		
		double c = cos(theta);
		double s = sin(theta);

		double A = a11*c*c+2*a12*c*s+a22*s*s;
			
		if (A > 0)
		{
			r = sqrt(1/A);

			int xx = int (r*c+0.5);
			int yy = int (r*s+0.5);

			x = X0 + xx;
			y = Y0 - yy;
			output.SafePixel(x,y) = v;
						
			x = X0 - xx;
			y = Y0 + yy;
			
			output.SafePixel(x,y) = v;
		}

	}
	// plot center
	output.SafePixel(X0, Y0) = v;

}

// very very stupid hash generator.
NetInt32 YARPImageHash::GetHash(YARPImageOf<YarpPixelBGR>& src)
{
  NetInt32 key = 0;
  int y = src.GetHeight()/2;
  int ent = 0;
  for (int x=src.GetWidth()-10; x>=10 && ent<5; x--)
    {
      YarpPixelBGR& pix = src.SafePixel(x,y);
      if ((pix.r>=10 && pix.r<=200) || x<=15)
	{
	  key *= 17;
	  key += pix.r;
	  key *= 17;
	  key += pix.g;
	  key *= 17;
	  key += pix.b;
	  ent++;
	}
    }
  return key;
}

void YARPBlobFinder::DrawBoxes (YARPImageOf<YarpPixelMono>& id)
{
	for (int i = 0; i < MaxBoxes; i++)
	{
		if (m_attn[i].valid)
		{
			DrawBox (id.GetIplPointer(), 
				m_attn[i].xmin,
				m_attn[i].ymin,
				m_attn[i].xmax,
				m_attn[i].ymax);
		}
	}
}