#include "cv.h"
#include <stdio.h>

int between(double v, double low, double high) {
	if (high >= low) {
		return ((v<=high)&&(v>=low)) ? 1:0;
	} else {
		return ((v>=low)||(v<=high)) ? 1:0;
	}
}

void cvHorizFlip(const IplImage* img, IplImage* dst) {
	int i, j, k, tmp;
	int w = img->width;
	int h = img->height;
	int n = img->nChannels;
	int s = img->widthStep;

	for (i = 0; i < h; i++) {
		for (j = 0; j < floor(w/2); j ++) {
			for (k = 0; k < 3; k ++) {
				tmp = img->imageData[i*s + j*n + k];
				dst->imageData[i*s + j*n + k] = img->imageData[(i+1)*s - j*n + k];
				dst->imageData[(i+1)*s - j*n + k] = tmp;
			}
		}
	}
}

void cvPutTextInt(IplImage* img, int i, CvPoint p, CvFont* f, CvScalar color) {
	char str[8];
	sprintf(str, "%d", i);
	cvPutText(img, str, p, f, color);
}

int cvDistance(CvPoint* p1, CvPoint* p2) {
	int x = (int)(p1->x) - (int)(p2->x);
	int y = (int)(p1->y) - (int)(p2->y);
	return (int)sqrt(x*x + y*y);
}

int cvAngle(CvPoint* p1, CvPoint* p2) {
	int x = (int)(p2->x) - (int)(p1->x);
	int y = (int)(p2->y) - (int)(p1->y);
	return (int)(360*(atan2(y,x)+3.1415)/6.2830);
}

// Takes a sequence of points (in), and returns an ordered sequence of points (out). It is ordered so that
// the distance between the first two points is the minimum distance between any two points, and in
// general, the distance between the 2n'th and (2n+1)'th point is the n'th lowest distance between
// any two points.
void cvSeqSortByDist(const CvSeq* in, CvSeq* out) {
	
	typedef struct DistPoint {int dist; CvPoint* p1; CvPoint* p2;} DistPoint;
	DistPoint x[in->total*(in->total-1)/2];
	DistPoint tmp;
	int i, j, h, x1, x2, y1, y2;

	if (in->total <= 1) {
		return;
	}

	h = 0;
	for (i = 0; i < in->total-1; i ++) {
		for (j = i+1; j < in->total; j++) {
			x[h].dist = cvDistance((CvPoint*)cvGetSeqElem(in, i), (CvPoint*)cvGetSeqElem(in, j));
			x[h].p1 = (CvPoint*)cvGetSeqElem(in, i);
			x[h].p2 = (CvPoint*)cvGetSeqElem(in, j);
			h++;
		}
	}


	// this orders the x matrix by x.dist, and then pushes them onto out after they have been ordered
	for (i = 0; i < sizeof(x)/sizeof(DistPoint); i ++) {
		for (j = i+1; j < sizeof(x)/sizeof(DistPoint); j++) {
			if ((&x[j] != 0 && &x[i] != 0) && (x[j].dist < x[i].dist)) {
				tmp.dist = x[j].dist;
				tmp.p1 = x[j].p1;
				tmp.p2 = x[j].p2;
				x[j].dist = x[i].dist;
				x[j].p1 = x[i].p1;
				x[j].p2 = x[i].p2;
				x[i].dist = tmp.dist;
				x[i].p1 = tmp.p1;
				x[i].p2 = tmp.p2;
			}
		}
		cvSeqPush(out, x[i].p1);
		cvSeqPush(out, x[i].p2);
	}

}

// Set dst(x,y) = max_value when threshold_low < src(x,y) < threshold_max
//	   dst(x,y) = 0 else.  max_range is the max possible value of src(x,y), e.g. 180 for hue.
void cvDoubleThreshold(const IplImage* src, IplImage* dst, double threshold_low, double threshold_high, double max_value, double max_range) {

	IplImage* copy = cvCreateImage(cvGetSize(src), 8 , 1);	

	cvCopy(src, copy, 0);
	cvThreshold(copy, dst, threshold_low, max_value, CV_THRESH_BINARY);
	cvSubRS(copy, cvRealScalar(max_range), copy, 0);
	cvThreshold(copy, copy, max_range-threshold_high, max_value, CV_THRESH_BINARY);
	cvXor(copy, dst, dst, 0);
}

// Draw a 1D histogram ont img
void cvHistogramDraw(CvHistogram* hist, IplImage* img, int bins, CvScalar color) {
	int i, intensity;
	float max_value = 0;
	int hist_height = img->height;
	int hist_width = img->width;
	int scale = hist_width/bins;
	cvGetMinMaxHistValue(hist, 0, &max_value, 0, 0);

	for(i = 0; i < bins; i++) {
        intensity = cvRound(cvQueryHistValue_1D(hist, i)*hist_height/max_value);
        cvRectangle(img, cvPoint(i*scale, hist_height),
                     cvPoint((i+1)*scale, hist_height - intensity),
                     color, CV_FILLED, 8, 0 );
    }
}

// Draw a vertical line at x position given by marker, where the bars are of width scale
void cvHistogramMarker(IplImage* img, int marker, int scale, CvScalar color) {
	cvRectangle(img, cvPoint(marker*scale, img->height), cvPoint(marker*scale + 1, 0), color, CV_FILLED, 8, 0 );
}

void cvDrawRegions(IplImage* img, int rad) {
	CvScalar color = CV_RGB(0,0xff,0);
	int w = img->width;
	int h = img->height;
	int i;
	cvCircle(img, cvPoint(w/2, h/2), rad, color, 1, 8, 0);
	cvCircle(img, cvPoint(w/2, h/2), rad*2, color, 1, 8, 0);
	for (i = 0; i < 8; i ++) {
		cvLine(img, cvPoint(w/2 + rad*cos(6.2830*(0.0625+(double)i/8)), h/2 + rad*sin(6.2830*(0.0625+(double)i/8))),
					cvPoint(w/2 + w*cos(6.2830*(0.0625+(double)i/8)), h/2 + w*sin(6.2830*(0.0625+(double)i/8))), color, 1, 8, 0);
	}
}

void cvDrawLocation(IplImage* img, CvPoint* p1, CvPoint* p2) {
	
	CvScalar color = CV_RGB(0xff,0,0);
	cvCircle(img, *p1, 10, color, 1, 8, 0);
    cvCircle(img, *p2, 10, color, 1, 8, 0);
    cvLine(img, *p1, *p2, color, 1, 8, 0);
    cvCircle(img, cvPoint((p1->x+p2->x)/2, (p1->y+p2->y)/2), 4, CV_RGB(0,0xff,0), 1, 8, 0);
	


}