image.c

#include "image.h"
#include "utils.h"
#include "blas.h"
#include "cuda.h"
#include <stdio.h>
#include <math.h>

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"

int windows = 0;

float colors[6][3] = { {1,0,1}, {0,0,1},{0,1,1},{0,1,0},{1,1,0},{1,0,0} };

//check if in bound
void DrawLine(image pic, int x1, int y1, int x2, int y2, float r, float g, float b) {
	int i, j;
	if (pic.c != 3)return;
	for (i = x1; i <= x2; i++) {
		j = (int)((float)(i - x1) / (float)(x2 - x1)*(float)(y2 - y1)) + y1;
		if (i >= 0 && i < pic.w && j >= 0 && j < pic.h) {
			pic.data[i + j*pic.w + 0 * pic.w*pic.h] = r;
			pic.data[i + j*pic.w + 1 * pic.w*pic.h] = g;
			pic.data[i + j*pic.w + 2 * pic.w*pic.h] = b;
		}
	}
}

image DrawLines_cv(image pic, int *x, int *y, int n, float width, float r, float g, float b) {
	if (pic.c != 3)return;
	int i, j, k;
	constrain_image(pic);
	rgbgr_image(pic);
	
	IplImage *disp = cvCreateImage(cvSize(pic.w, pic.h), IPL_DEPTH_8U, pic.c);
	int step = disp->widthStep;
	
	for (j = 0; j < pic.h; ++j) {
		for (i = 0; i < pic.w; ++i) {
			for (k = 0; k < pic.c; ++k) {
				disp->imageData[j*step + i*pic.c + k] = (unsigned char)(get_pixel(pic, i, j, k) * 255);
			}
		}
	}
	free_image(pic);
	
	for (i = 0; i < n; i++) {
		cvDrawLine(disp, cvPoint(x[i], y[i]), cvPoint(x[(i + 1) % n], y[(i + 1) % n]), CV_RGB(r * 255, b * 255, g * 255), width, 8, 0);
	}
	
	image pic_out = ipl_to_image(disp);
	cvReleaseImage(&disp);
	rgbgr_image(pic_out);
	return pic_out;
}

void SetColor(image pic, float value) {
	int i;
	for (i = 0; i < (pic.w*pic.h*pic.c); i++) {
		pic.data[i] = value;
	}
}

float get_color(int c, int x, int max)
{
    float ratio = ((float)x/max)*5;
    int i = floor(ratio);
    int j = ceil(ratio);
    ratio -= i;
    float r = (1-ratio) * colors[i][c] + ratio*colors[j][c];
    //printf("%f\n", r);
    return r;
}

void composite_image(image source, image dest, int dx, int dy)
{
    int x,y,k;
    for(k = 0; k < source.c; ++k){
        for(y = 0; y < source.h; ++y){
            for(x = 0; x < source.w; ++x){
                float val = get_pixel(source, x, y, k);
                float val2 = get_pixel_extend(dest, dx+x, dy+y, k);
                set_pixel(dest, dx+x, dy+y, k, val * val2);
            }
        }
    }
}

image border_image(image a, int border)
{
    image b = make_image(a.w + 2*border, a.h + 2*border, a.c);
    int x,y,k;
    for(k = 0; k < b.c; ++k){
        for(y = 0; y < b.h; ++y){
            for(x = 0; x < b.w; ++x){
                float val = get_pixel_extend(a, x - border, y - border, k);
                if(x - border < 0 || x - border >= a.w || y - border < 0 || y - border >= a.h) val = 1;
                set_pixel(b, x, y, k, val);
            }
        }
    }
    return b;
}

image tile_images(image a, image b, int dx)
{
    if(a.w == 0) return copy_image(b);
    image c = make_image(a.w + b.w + dx, (a.h > b.h) ? a.h : b.h, (a.c > b.c) ? a.c : b.c);
    fill_cpu(c.w*c.h*c.c, 1, c.data, 1);
    embed_image(a, c, 0, 0); 
    composite_image(b, c, a.w + dx, 0);
    return c;
}

image get_label(image **characters, char *string, int size)
{
    if(size > 7) size = 7;
    image label = make_empty_image(0,0,0);
    while(*string){
        image l = characters[size][(int)*string];
        image n = tile_images(label, l, -size - 1 + (size+1)/2);
        free_image(label);
        label = n;
        ++string;
    }
    image b = border_image(label, label.h*.25);
    free_image(label);
    return b;
}

void draw_label(image a, int r, int c, image label, const float *rgb)
{
    int w = label.w;
    int h = label.h;
    if (r - h >= 0) r = r - h;

    int i, j, k;
    for(j = 0; j < h && j + r < a.h; ++j){
        for(i = 0; i < w && i + c < a.w; ++i){
            for(k = 0; k < label.c; ++k){
                float val = get_pixel(label, i, j, k);
                set_pixel(a, i+c, j+r, k, rgb[k] * val);
            }
        }
    }
}

void draw_box(image a, int x1, int y1, int x2, int y2, float r, float g, float b)
{
    //normalize_image(a);
    int i;
    if(x1 < 0) x1 = 0;
    if(x1 >= a.w) x1 = a.w-1;
    if(x2 < 0) x2 = 0;
    if(x2 >= a.w) x2 = a.w-1;

    if(y1 < 0) y1 = 0;
    if(y1 >= a.h) y1 = a.h-1;
    if(y2 < 0) y2 = 0;
    if(y2 >= a.h) y2 = a.h-1;

    for(i = x1; i <= x2; ++i){
        a.data[i + y1*a.w + 0*a.w*a.h] = r;
        a.data[i + y2*a.w + 0*a.w*a.h] = r;

        a.data[i + y1*a.w + 1*a.w*a.h] = g;
        a.data[i + y2*a.w + 1*a.w*a.h] = g;

        a.data[i + y1*a.w + 2*a.w*a.h] = b;
        a.data[i + y2*a.w + 2*a.w*a.h] = b;
    }
    for(i = y1; i <= y2; ++i){
        a.data[x1 + i*a.w + 0*a.w*a.h] = r;
        a.data[x2 + i*a.w + 0*a.w*a.h] = r;

        a.data[x1 + i*a.w + 1*a.w*a.h] = g;
        a.data[x2 + i*a.w + 1*a.w*a.h] = g;

        a.data[x1 + i*a.w + 2*a.w*a.h] = b;
        a.data[x2 + i*a.w + 2*a.w*a.h] = b;
    }
}
image draw_box5(image a, box2 box_in, float width, float r, float g, float b)
{
	//normalize_image(a);
	int x[4], y[4];

	float xshift = box_in.x;
	float yshift = box_in.y;
	float w = box_in.w;
	float h = box_in.h;
	float rz = box_in.rz;
	x[0] = (cosf(rz)*(-w / 2) + sinf(rz)*(-h / 2) + xshift)*a.w;
	y[0] = (-sinf(rz)*(-w / 2) + cosf(rz)*(-h / 2) + yshift)*a.h;
	x[1] = (cosf(rz)*(w / 2) + sinf(rz)*(-h / 2) + xshift)*a.w;
	y[1] = (-sinf(rz)*(w / 2) + cosf(rz)*(-h / 2) + yshift)*a.h;
	x[2] = (cosf(rz)*(w / 2) + sinf(rz)*(h / 2) + xshift)*a.w;
	y[2] = (-sinf(rz)*(w / 2) + cosf(rz)*(h / 2) + yshift)*a.h;
	x[3] = (cosf(rz)*(-w / 2) + sinf(rz)*(h / 2) + xshift)*a.w;
	y[3] = (-sinf(rz)*(-w / 2) + cosf(rz)*(h / 2) + yshift)*a.h;
	image pic_out = DrawLines_cv(a, x, y, 4, 1, r, g, b);
	return pic_out;
	/*for (int i = 0; i < 4; i++) {
		DrawLine(a, x[i], y[i], x[(i + 1) % 4], y[(i + 1) % 4], r, g, b);
	}*/
	/*int x1 = (box_in.x - box_in.w / 2.)*a.w;
	int x2 = (box_in.x + box_in.w / 2.)*a.w;
	int y1 = (box_in.y - box_in.h / 2.)*a.h;
	int y2 = (box_in.y + box_in.h / 2.)*a.h;
	float rz = box_in.rz;

	int i;
	if (x1 < 0) x1 = 0;
	if (x1 >= a.w) x1 = a.w - 1;
	if (x2 < 0) x2 = 0;
	if (x2 >= a.w) x2 = a.w - 1;

	if (y1 < 0) y1 = 0;
	if (y1 >= a.h) y1 = a.h - 1;
	if (y2 < 0) y2 = 0;
	if (y2 >= a.h) y2 = a.h - 1;

	for (i = 0; i <= (int)(box_in.w*a.w); ++i) {
		a.data[i + y1*a.w + 0 * a.w*a.h] = r;
		a.data[i + y2*a.w + 0 * a.w*a.h] = r;

		a.data[i + y1*a.w + 1 * a.w*a.h] = g;
		a.data[i + y2*a.w + 1 * a.w*a.h] = g;

		a.data[i + y1*a.w + 2 * a.w*a.h] = b;
		a.data[i + y2*a.w + 2 * a.w*a.h] = b;
	}
	for (i = y1; i <= y2; ++i) {
		a.data[x1 + i*a.w + 0 * a.w*a.h] = r;
		a.data[x2 + i*a.w + 0 * a.w*a.h] = r;

		a.data[x1 + i*a.w + 1 * a.w*a.h] = g;
		a.data[x2 + i*a.w + 1 * a.w*a.h] = g;

		a.data[x1 + i*a.w + 2 * a.w*a.h] = b;
		a.data[x2 + i*a.w + 2 * a.w*a.h] = b;
	}*/
}
image draw_box7(image a, box3 box_in, float width, float r, float g, float b)
{
	//normalize_image(a);
	int x[4], y[4];

	float xshift = box_in.x;
	float yshift = box_in.y;
	float w = box_in.w;
	float h = box_in.h;
	float rz = box_in.rz;
	x[0] = (cosf(rz)*(-w / 2) + sinf(rz)*(-h / 2) + xshift)*a.w;
	y[0] = (-sinf(rz)*(-w / 2) + cosf(rz)*(-h / 2) + yshift)*a.h;
	x[1] = (cosf(rz)*(w / 2) + sinf(rz)*(-h / 2) + xshift)*a.w;
	y[1] = (-sinf(rz)*(w / 2) + cosf(rz)*(-h / 2) + yshift)*a.h;
	x[2] = (cosf(rz)*(w / 2) + sinf(rz)*(h / 2) + xshift)*a.w;
	y[2] = (-sinf(rz)*(w / 2) + cosf(rz)*(h / 2) + yshift)*a.h;
	x[3] = (cosf(rz)*(-w / 2) + sinf(rz)*(h / 2) + xshift)*a.w;
	y[3] = (-sinf(rz)*(-w / 2) + cosf(rz)*(h / 2) + yshift)*a.h;
	image pic_out = DrawLines_cv(a, x, y, 4, 1, r, g, b);
	return pic_out;
}

void draw_box_width(image a, int x1, int y1, int x2, int y2, int w, float r, float g, float b)
{
    int i;
    for(i = 0; i < w; ++i){
        draw_box(a, x1+i, y1+i, x2-i, y2-i, r, g, b);
    }
}
void draw_box_width5(image a, int x1, int y1, int x2, int y2, int w, float r, float g, float b)
{
	int i;
	for (i = 0; i < w; ++i) {
		draw_box(a, x1 + i, y1 + i, x2 - i, y2 - i, r, g, b);
	}
}

void draw_bbox(image a, box bbox, int w, float r, float g, float b)
{
    int left  = (bbox.x-bbox.w/2)*a.w;
    int right = (bbox.x+bbox.w/2)*a.w;
    int top   = (bbox.y-bbox.h/2)*a.h;
    int bot   = (bbox.y+bbox.h/2)*a.h;

    int i;
    for(i = 0; i < w; ++i){
        draw_box(a, left+i, top+i, right-i, bot-i, r, g, b);
    }
}

image **load_alphabet()
{
    int i, j;
    const int nsize = 8;
    image **alphabets = calloc(nsize, sizeof(image));
    for(j = 0; j < nsize; ++j){
        alphabets[j] = calloc(128, sizeof(image));
        for(i = 32; i < 127; ++i){
            char buff[256];
            sprintf(buff, "data/labels/%d_%d.png", i, j);
            alphabets[j][i] = load_image_color(buff, 0, 0);
        }
    }
    return alphabets;
}

void draw_detections(image im, int num, float thresh, box *boxes, float **probs, char **names, image **alphabet, int classes)
{
    int i;

    for(i = 0; i < num; ++i){
        int class = max_index(probs[i], classes);
        float prob = probs[i][class];
        if(prob > thresh){

            int width = im.h * .012;

            if(0){
                width = pow(prob, 1./2.)*10+1;
                alphabet = 0;
            }

            printf("%s: %.0f%%\n", names[class], prob*100);
            int offset = class*123457 % classes;
            float red = get_color(2,offset,classes);
            float green = get_color(1,offset,classes);
            float blue = get_color(0,offset,classes);
            float rgb[3];

            //width = prob*20+2;

            rgb[0] = red;
            rgb[1] = green;
            rgb[2] = blue;
            box b = boxes[i];

            int left  = (b.x-b.w/2.)*im.w;
            int right = (b.x+b.w/2.)*im.w;
            int top   = (b.y-b.h/2.)*im.h;
            int bot   = (b.y+b.h/2.)*im.h;

            if(left < 0) left = 0;
            if(right > im.w-1) right = im.w-1;
            if(top < 0) top = 0;
            if(bot > im.h-1) bot = im.h-1;

            draw_box_width(im, left, top, right, bot, width, red, green, blue);
            if (alphabet) {
				image label = get_label(alphabet, names[class], (im.h*.03) / 16);
                draw_label(im, top + width, left, label, rgb);
				//draws probability after class name
				char prob_string[10];
				sprintf_s(prob_string, 10, "%.1f", prob*100);
				image label_prob = get_label(alphabet, prob_string, (im.h*.03) / 16);
				draw_label(im, top + width, left + label.w, label_prob, rgb);
            }
        }
    }
}
void draw_detections5(image *im, int num, float thresh, box2 *boxes, float **probs, char **names, image **alphabet, int classes)
{
	int i;

	for (i = 0; i < num; ++i) {
		int class = max_index(probs[i], classes);
		float prob = probs[i][class];
		if (prob > thresh) {

			int width = im->h * .012;

			if (0) {
				width = pow(prob, 1. / 2.) * 10 + 1;
				alphabet = 0;
			}

			
			int offset = class * 123457 % classes;
			float red = get_color(2, offset, classes);
			float green = get_color(1, offset, classes);
			float blue = get_color(0, offset, classes);
			float rgb[3];

			//width = prob*20+2;

			rgb[0] = red;
			rgb[1] = green;
			rgb[2] = blue;
			box2 b = boxes[i];
			
			printf("%s: %.0f%%,@(%.2f,%.2f),size(%.2f,%.2f),rz(%.2f)\n", names[class], prob * 100, b.x, b.y, b.w, b.h, b.rz);

			int left = (b.x - b.w / 2.)*im->w;
			int right = (b.x + b.w / 2.)*im->w;
			int top = (b.y - b.h / 2.)*im->h;
			int bot = (b.y + b.h / 2.)*im->h;

			if (left < 0) left = 0;
			if (right > im->w - 1) right = im->w - 1;
			if (top < 0) top = 0;
			if (bot > im->h - 1) bot = im->h - 1;

			image pic = draw_box5(*im, b, width, red, green, blue);
			*im = copy_image(pic);
			/*if (alphabet) {
				image label = get_label(alphabet, names[class], (im.h*.03) / 16);
				draw_label(im, top + width, left, label, rgb);
				//draws probability after class name
				char prob_string[10];
				sprintf_s(prob_string, 10, "%.1f", prob * 100);
				image label_prob = get_label(alphabet, prob_string, (im.h*.03) / 16);
				draw_label(im, top + width, left + label.w, label_prob, rgb);
			}*/
			free_image(pic);
		}
	}
}
void draw_detections7(image *im, int num, float thresh, box3 *boxes, float **probs, char **names, image **alphabet, int classes)
{
	int i;

	for (i = 0; i < num; ++i) {
		int class = max_index(probs[i], classes);
		float prob = probs[i][class];
		if (prob > thresh) {

			int width = im->h * .012;

			if (0) {
				width = pow(prob, 1. / 2.) * 10 + 1;
				alphabet = 0;
			}

			
			int offset = class * 123457 % classes;
			float red = get_color(2, offset, classes);
			float green = get_color(1, offset, classes);
			float blue = get_color(0, offset, classes);
			float rgb[3];

			//width = prob*20+2;

			rgb[0] = red;
			rgb[1] = green;
			rgb[2] = blue;
			box3 b = boxes[i];
			
			printf("%s: %.0f%%,@(%.2f,%.2f),size(%.2f,%.2f),rz(%.2f)\n", names[class], prob * 100, b.x, b.y, b.w, b.h, b.rz);

			int left = (b.x - b.w / 2.)*im->w;
			int right = (b.x + b.w / 2.)*im->w;
			int top = (b.y - b.h / 2.)*im->h;
			int bot = (b.y + b.h / 2.)*im->h;

			if (left < 0) left = 0;
			if (right > im->w - 1) right = im->w - 1;
			if (top < 0) top = 0;
			if (bot > im->h - 1) bot = im->h - 1;

			image pic = draw_box7(*im, b, width, red, green, blue);
			*im = copy_image(pic);
			/*if (alphabet) {
				image label = get_label(alphabet, names[class], (im.h*.03) / 16);
				draw_label(im, top + width, left, label, rgb);
				//draws probability after class name
				char prob_string[10];
				sprintf_s(prob_string, 10, "%.1f", prob * 100);
				image label_prob = get_label(alphabet, prob_string, (im.h*.03) / 16);
				draw_label(im, top + width, left + label.w, label_prob, rgb);
			}*/
			free_image(pic);
		}
	}
}
void draw_detections_3d(image *im, int n, float thresh, box3 *boxes, float **probs, char **names, image **alphabet, int classes) {
	int i, j;
	float p0[12] = { 721.5377,0.0,609.5593,44.85728,0.0,721.5377,172.854,0.2163791,0.0,0.0,1.0,0.002745884 };
	float r0[9] = { 0.9999239,0.009833776,-0.007445048,-0.009869795,0.9999421,-0.004278459,0.007402527,0.004351614,0.9999631 };
	float tr[16] = { 0.007533745,-0.9999714,-0.000616602,-0.004069766,0.01480249,0.0007280733,-0.9998902,-0.0763161,0.9998621,0.00752379,0.01480755,-0.2717806};
	for (i = 0; i < n; i++) {
		int class = max_index(probs[i], classes);
		float prob = probs[i][class];
		if (prob > thresh) {
			float rgb[3];
			rgb[0] = 1.0;
			rgb[1] = 0.0;
			rgb[2] = 0.0;
			box3 b = boxes[i];
			float x[8], y[8], z[8];
			float u[8], v[8], bias[8];
			y[0] = 15.2 - (cosf(b.rz)*(-b.w / 2) + sinf(b.rz)*(-b.h / 2) + b.x)*30.4;
			x[0] = 30.4 - (-sinf(b.rz)*(-b.w / 2) + cosf(b.rz)*(-b.h / 2) + b.y)*30.4;
			y[1] = 15.2 - (cosf(b.rz)*(b.w / 2) + sinf(b.rz)*(-b.h / 2) + b.x)*30.4;
			x[1] = 30.4 - (-sinf(b.rz)*(b.w / 2) + cosf(b.rz)*(-b.h / 2) + b.y)*30.4;
			y[5] = 15.2 - (cosf(b.rz)*(b.w / 2) + sinf(b.rz)*(b.h / 2) + b.x)*30.4;
			x[5] = 30.4 - (-sinf(b.rz)*(b.w / 2) + cosf(b.rz)*(b.h / 2) + b.y)*30.4;
			y[4] = 15.2 - (cosf(b.rz)*(-b.w / 2) + sinf(b.rz)*(b.h / 2) + b.x)*30.4;
			x[4] = 30.4 - (-sinf(b.rz)*(-b.w / 2) + cosf(b.rz)*(b.h / 2) + b.y)*30.4;
			x[2] = x[0];
			y[2] = y[0];
			x[3] = x[1];
			y[3] = y[1];
			x[6] = x[4];
			y[6] = y[4];
			x[7] = x[5];
			y[7] = y[5];
			z[0] = b.z*5.0 + b.l*5.0 / 2.0 - 4.0;
			z[2] = b.z*5.0 - b.l*5.0 / 2.0 - 4.0;
			z[1] = z[0];
			z[4] = z[0];
			z[5] = z[0];
			z[3] = z[2];
			z[6] = z[2];
			z[7] = z[2];
			
			float mid[4], mid2[4];
			for (j = 0; j < 8; j++) {
				mid[0] = tr[0] * x[j] + tr[1] * y[j] + tr[2] * z[j] + tr[3];
				mid[1] = tr[4] * x[j] + tr[5] * y[j] + tr[6] * z[j] + tr[7];
				mid[2] = tr[8] * x[j] + tr[9] * y[j] + tr[10] * z[j] + tr[11];
				mid[3] = 1;
				//printf("point %d, %f %f %f %f\n", j + 1, mid[0], mid[1], mid[2], mid[3]);

				mid2[0] = r0[0] * mid[0] + r0[1] * mid[1] + r0[2] * mid[2];
				mid2[1] = r0[3] * mid[0] + r0[4] * mid[1] + r0[5] * mid[2];
				mid2[2] = r0[6] * mid[0] + r0[7] * mid[1] + r0[8] * mid[2];
				mid2[3] = 1;
				//printf("\t%f %f %f %f\n", mid2[0], mid2[1], mid2[2], mid2[3]);

				u[j] = p0[0] * mid2[0] + p0[1] * mid2[1] + p0[2] * mid2[2] + p0[3] * mid2[3];
				v[j] = p0[4] * mid2[0] + p0[5] * mid2[1] + p0[6] * mid2[2] + p0[7] * mid2[3];
				bias[j] = p0[8] * mid2[0] + p0[9] * mid2[1] + p0[10] * mid2[2] + p0[11] * mid2[3];
				//printf("\t%f %f %f\n",  u[j], v[j], bias[j]);

				u[j] = u[j] / bias[j];
				v[j] = v[j] / bias[j];
				//printf("\t\t%f %f\n", u[j], v[j]);
				
				/*u[j] = p0[0] * x[j] + p0[1] * y[j] + p0[2] * z[j] + p0[3];
				v[j] = p0[4] * x[j] + p0[5] * y[j] + p0[6] * z[j] + p0[7];
				bias[j] = p0[8] * x[j] + p0[9] * y[j] + p0[10] * z[j] + p0[11];
				printf("\t%f %f %f\n\n",  u[j], v[j], bias[j]);

				u[j] = u[j] / bias[j];
				v[j] = v[j] / bias[j];
				printf("\t\t%f %f\n", u[j], v[j]);*/
			}

			int xdraw[4], ydraw[4];
			xdraw[0] = u[0];
			xdraw[1] = u[1];
			xdraw[2] = u[3];
			xdraw[3] = u[2];
			ydraw[0] = v[0];
			ydraw[1] = v[1];
			ydraw[2] = v[3];
			ydraw[3] = v[2];
			image pic = DrawLines_cv(*im, xdraw, ydraw, 4, 1, rgb[0], rgb[1], rgb[2]);
			*im = copy_image(pic);
			free_image(pic);
			xdraw[0] = u[4];
			xdraw[1] = u[5];
			xdraw[2] = u[7];
			xdraw[3] = u[6];
			ydraw[0] = v[4];
			ydraw[1] = v[5];
			ydraw[2] = v[7];
			ydraw[3] = v[6];
			pic = DrawLines_cv(*im, xdraw, ydraw, 4, 1, rgb[0], rgb[1], rgb[2]);
			*im = copy_image(pic);
			free_image(pic);
			xdraw[0] = u[1];
			xdraw[1] = u[3];
			xdraw[2] = u[7];
			xdraw[3] = u[5];
			ydraw[0] = v[1];
			ydraw[1] = v[3];
			ydraw[2] = v[7];
			ydraw[3] = v[5];
			pic = DrawLines_cv(*im, xdraw, ydraw, 4, 1, rgb[0], rgb[1], rgb[2]);
			*im = copy_image(pic);
			free_image(pic);
			xdraw[0] = u[0];
			xdraw[1] = u[2];
			xdraw[2] = u[6];
			xdraw[3] = u[4];
			ydraw[0] = v[0];
			ydraw[1] = v[2];
			ydraw[2] = v[6];
			ydraw[3] = v[4];
			pic = DrawLines_cv(*im, xdraw, ydraw, 4, 1, rgb[0], rgb[1], rgb[2]);
			*im = copy_image(pic);
			free_image(pic);
		}
	}
}

void transpose_image(image im)
{
    assert(im.w == im.h);
    int n, m;
    int c;
    for(c = 0; c < im.c; ++c){
        for(n = 0; n < im.w-1; ++n){
            for(m = n + 1; m < im.w; ++m){
                float swap = im.data[m + im.w*(n + im.h*c)];
                im.data[m + im.w*(n + im.h*c)] = im.data[n + im.w*(m + im.h*c)];
                im.data[n + im.w*(m + im.h*c)] = swap;
            }
        }
    }
}

void rotate_image_cw(image im, int times)
{
    assert(im.w == im.h);
    times = (times + 400) % 4;
    int i, x, y, c;
    int n = im.w;
    for(i = 0; i < times; ++i){
        for(c = 0; c < im.c; ++c){
            for(x = 0; x < n/2; ++x){
                for(y = 0; y < (n-1)/2 + 1; ++y){
                    float temp = im.data[y + im.w*(x + im.h*c)];
                    im.data[y + im.w*(x + im.h*c)] = im.data[n-1-x + im.w*(y + im.h*c)];
                    im.data[n-1-x + im.w*(y + im.h*c)] = im.data[n-1-y + im.w*(n-1-x + im.h*c)];
                    im.data[n-1-y + im.w*(n-1-x + im.h*c)] = im.data[x + im.w*(n-1-y + im.h*c)];
                    im.data[x + im.w*(n-1-y + im.h*c)] = temp;
                }
            }
        }
    }
}

void flip_image(image a)
{
    int i,j,k;
    for(k = 0; k < a.c; ++k){
        for(i = 0; i < a.h; ++i){
            for(j = 0; j < a.w/2; ++j){
                int index = j + a.w*(i + a.h*(k));
                int flip = (a.w - j - 1) + a.w*(i + a.h*(k));
                float swap = a.data[flip];
                a.data[flip] = a.data[index];
                a.data[index] = swap;
            }
        }
    }
}

image image_distance(image a, image b)
{
    int i,j;
    image dist = make_image(a.w, a.h, 1);
    for(i = 0; i < a.c; ++i){
        for(j = 0; j < a.h*a.w; ++j){
            dist.data[j] += pow(a.data[i*a.h*a.w+j]-b.data[i*a.h*a.w+j],2);
        }
    }
    for(j = 0; j < a.h*a.w; ++j){
        dist.data[j] = sqrt(dist.data[j]);
    }
    return dist;
}

void embed_image(image source, image dest, int dx, int dy)
{
    int x,y,k;
    for(k = 0; k < source.c; ++k){
        for(y = 0; y < source.h; ++y){
            for(x = 0; x < source.w; ++x){
                float val = get_pixel(source, x,y,k);
                set_pixel(dest, dx+x, dy+y, k, val);
            }
        }
    }
}

image collapse_image_layers(image source, int border)
{
    int h = source.h;
    h = (h+border)*source.c - border;
    image dest = make_image(source.w, h, 1);
    int i;
    for(i = 0; i < source.c; ++i){
        image layer = get_image_layer(source, i);
        int h_offset = i*(source.h+border);
        embed_image(layer, dest, 0, h_offset);
        free_image(layer);
    }
    return dest;
}

void constrain_image(image im)
{
    int i;
    for(i = 0; i < im.w*im.h*im.c; ++i){
        if(im.data[i] < 0) im.data[i] = 0;
        if(im.data[i] > 1) im.data[i] = 1;
    }
}

void normalize_image(image p)
{
    int i;
    float min = 9999999;
    float max = -999999;

    for(i = 0; i < p.h*p.w*p.c; ++i){
        float v = p.data[i];
        if(v < min) min = v;
        if(v > max) max = v;
    }
    if(max - min < .000000001){
        min = 0;
        max = 1;
    }
    for(i = 0; i < p.c*p.w*p.h; ++i){
        p.data[i] = (p.data[i] - min)/(max-min);
    }
}

void normalize_image2(image p)
{
    float *min = calloc(p.c, sizeof(float));
    float *max = calloc(p.c, sizeof(float));
    int i,j;
    for(i = 0; i < p.c; ++i) min[i] = max[i] = p.data[i*p.h*p.w];

    for(j = 0; j < p.c; ++j){
        for(i = 0; i < p.h*p.w; ++i){
            float v = p.data[i+j*p.h*p.w];
            if(v < min[j]) min[j] = v;
            if(v > max[j]) max[j] = v;
        }
    }
    for(i = 0; i < p.c; ++i){
        if(max[i] - min[i] < .000000001){
            min[i] = 0;
            max[i] = 1;
        }
    }
    for(j = 0; j < p.c; ++j){
        for(i = 0; i < p.w*p.h; ++i){
            p.data[i+j*p.h*p.w] = (p.data[i+j*p.h*p.w] - min[j])/(max[j]-min[j]);
        }
    }
    free(min);
    free(max);
}

image copy_image(image p)
{
    image copy = p;
    copy.data = calloc(p.h*p.w*p.c, sizeof(float));
    memcpy(copy.data, p.data, p.h*p.w*p.c*sizeof(float));
    return copy;
}

void rgbgr_image(image im)
{
    int i;
    for(i = 0; i < im.w*im.h; ++i){
        float swap = im.data[i];
        im.data[i] = im.data[i+im.w*im.h*2];
        im.data[i+im.w*im.h*2] = swap;
    }
}

#ifdef OPENCV
void show_image_cv(image p, const char *name)
{
    int x,y,k;
    image copy = copy_image(p);
    constrain_image(copy);
    if(p.c == 3) rgbgr_image(copy);
    //normalize_image(copy);

    char buff[256];
    //sprintf(buff, "%s (%d)", name, windows);
    sprintf(buff, "%s", name);

    IplImage *disp = cvCreateImage(cvSize(p.w,p.h), IPL_DEPTH_8U, p.c);
    int step = disp->widthStep;
    cvNamedWindow(buff, CV_WINDOW_AUTOSIZE); 
    //cvMoveWindow(buff, 100*(windows%10) + 200*(windows/10), 100*(windows%10));
    ++windows;
    for(y = 0; y < p.h; ++y){
        for(x = 0; x < p.w; ++x){
            for(k= 0; k < p.c; ++k){
                disp->imageData[y*step + x*p.c + k] = (unsigned char)(get_pixel(copy,x,y,k)*255);
            }
        }
    }
    free_image(copy);
    if(0){
        int w = 448;
        int h = w*p.h/p.w;
        if(h > 1000){
            h = 1000;
            w = h*p.w/p.h;
        }
        IplImage *buffer = disp;
        disp = cvCreateImage(cvSize(w, h), buffer->depth, buffer->nChannels);
        cvResize(buffer, disp, CV_INTER_LINEAR);
        cvReleaseImage(&buffer);
    }
    cvShowImage(buff, disp);

	{
		CvSize size;
		{
			size.width = disp->width, size.height = disp->height;
		}

		static CvVideoWriter* output_video = NULL;    // cv::VideoWriter output_video;
		if (output_video == NULL)
		{
			printf("\n SRC output_video = %p \n", output_video);
			const char* output_name = "test_dnn_out.avi";
			//output_video = cvCreateVideoWriter(output_name, CV_FOURCC('H', '2', '6', '4'), 25, size, 1);
			output_video = cvCreateVideoWriter(output_name, CV_FOURCC('D', 'I', 'V', 'X'), 25, size, 1);
			//output_video = cvCreateVideoWriter(output_name, CV_FOURCC('M', 'J', 'P', 'G'), 25, size, 1);
			printf("\n cvCreateVideoWriter, DST output_video = %p  \n", output_video);
		}

		cvWriteFrame(output_video, disp);
		printf("\n cvWriteFrame \n");
	}

    cvReleaseImage(&disp);
}
#endif

void show_image(image p, const char *name)
{
#ifdef OPENCV
    show_image_cv(p, name);
#else
    fprintf(stderr, "Not compiled with OpenCV, saving to %s.png instead\n", name);
    save_image(p, name);
#endif
}

#ifdef OPENCV

image ipl_to_image(IplImage* src)
{
    unsigned char *data = (unsigned char *)src->imageData;
    int h = src->height;
    int w = src->width;
    int c = src->nChannels;
    int step = src->widthStep;
    image out = make_image(w, h, c);
    int i, j, k, count=0;;

    for(k= 0; k < c; ++k){
        for(i = 0; i < h; ++i){
            for(j = 0; j < w; ++j){
                out.data[count++] = data[i*step + j*c + k]/255.;
            }
        }
    }
    return out;
}

image load_image_cv(char *filename, int channels)
{
    IplImage* src = 0;
    int flag = -1;
    if (channels == 0) flag = -1;
    else if (channels == 1) flag = 0;
    else if (channels == 3) flag = 1;
    else {
        fprintf(stderr, "OpenCV can't force load with %d channels\n", channels);
    }

    if( (src = cvLoadImage(filename, flag)) == 0 )
    {
        fprintf(stderr, "Cannot load image \"%s\"\n", filename);
        char buff[256];
        sprintf(buff, "echo %s >> bad.list", filename);
        system(buff);
        return make_image(10,10,3);
        //exit(0);
    }
    image out = ipl_to_image(src);
    cvReleaseImage(&src);
    rgbgr_image(out);
    return out;
}

image get_image_from_stream(CvCapture *cap)
{
    IplImage* src = cvQueryFrame(cap);
    if (!src) return make_empty_image(0,0,0);
    image im = ipl_to_image(src);
    rgbgr_image(im);
    return im;
}

void save_image_jpg(image p, const char *name)
{
    image copy = copy_image(p);
    if(p.c == 3) rgbgr_image(copy);
    int x,y,k;

    char buff[256];
    sprintf(buff, "%s.jpg", name);

    IplImage *disp = cvCreateImage(cvSize(p.w,p.h), IPL_DEPTH_8U, p.c);
    int step = disp->widthStep;
    for(y = 0; y < p.h; ++y){
        for(x = 0; x < p.w; ++x){
            for(k= 0; k < p.c; ++k){
                disp->imageData[y*step + x*p.c + k] = (unsigned char)(get_pixel(copy,x,y,k)*255);
            }
        }
    }
    cvSaveImage(buff, disp,0);
    cvReleaseImage(&disp);
    free_image(copy);
}
#endif

void save_image_png(image im, const char *name)
{
    char buff[256];
    //sprintf(buff, "%s (%d)", name, windows);
    sprintf(buff, "%s.png", name);
    unsigned char *data = calloc(im.w*im.h*im.c, sizeof(char));
    int i,k;
    for(k = 0; k < im.c; ++k){
        for(i = 0; i < im.w*im.h; ++i){
            data[i*im.c+k] = (unsigned char) (255*im.data[i + k*im.w*im.h]);
        }
    }
    int success = stbi_write_png(buff, im.w, im.h, im.c, data, im.w*im.c);
    free(data);
    if(!success) fprintf(stderr, "Failed to write image %s\n", buff);
}

void save_image(image im, const char *name)
{
#ifdef OPENCV
    save_image_jpg(im, name);
#else
    save_image_png(im, name);
#endif
}


void show_image_layers(image p, char *name)
{
    int i;
    char buff[256];
    for(i = 0; i < p.c; ++i){
        sprintf(buff, "%s - Layer %d", name, i);
        image layer = get_image_layer(p, i);
        show_image(layer, buff);
        free_image(layer);
    }
}

void show_image_collapsed(image p, char *name)
{
    image c = collapse_image_layers(p, 1);
    show_image(c, name);
    free_image(c);
}

image make_empty_image(int w, int h, int c)
{
    image out;
    out.data = 0;
    out.h = h;
    out.w = w;
    out.c = c;
    return out;
}

image make_image(int w, int h, int c)
{
    image out = make_empty_image(w,h,c);
    out.data = calloc(h*w*c, sizeof(float));
    return out;
}

image make_random_image(int w, int h, int c)
{
    image out = make_empty_image(w,h,c);
    out.data = calloc(h*w*c, sizeof(float));
    int i;
    for(i = 0; i < w*h*c; ++i){
        out.data[i] = (rand_normal() * .25) + .5;
    }
    return out;
}

image float_to_image(int w, int h, int c, float *data)
{
    image out = make_empty_image(w,h,c);
    out.data = data;
    return out;
}


image rotate_crop_image(image im, float rad, float s, int w, int h, float dx, float dy, float aspect)
{
    int x, y, c;
    float cx = im.w/2.;
    float cy = im.h/2.;
    image rot = make_image(w, h, im.c);
    for(c = 0; c < im.c; ++c){
        for(y = 0; y < h; ++y){
            for(x = 0; x < w; ++x){
                float rx = cos(rad)*((x - w/2.)/s*aspect + dx/s*aspect) - sin(rad)*((y - h/2.)/s + dy/s) + cx;
                float ry = sin(rad)*((x - w/2.)/s*aspect + dx/s*aspect) + cos(rad)*((y - h/2.)/s + dy/s) + cy;
                float val = bilinear_interpolate(im, rx, ry, c);
                set_pixel(rot, x, y, c, val);
            }
        }
    }
    return rot;
}

image rotate_image(image im, float rad)
{
    int x, y, c;
    float cx = im.w/2.;
    float cy = im.h/2.;
    image rot = make_image(im.w, im.h, im.c);
    for(c = 0; c < im.c; ++c){
        for(y = 0; y < im.h; ++y){
            for(x = 0; x < im.w; ++x){
                float rx = cos(rad)*(x-cx) - sin(rad)*(y-cy) + cx;
                float ry = sin(rad)*(x-cx) + cos(rad)*(y-cy) + cy;
                float val = bilinear_interpolate(im, rx, ry, c);
                set_pixel(rot, x, y, c, val);
            }
        }
    }
    return rot;
}

void translate_image(image m, float s)
{
    int i;
    for(i = 0; i < m.h*m.w*m.c; ++i) m.data[i] += s;
}

void scale_image(image m, float s)
{
    int i;
    for(i = 0; i < m.h*m.w*m.c; ++i) m.data[i] *= s;
}

image crop_image(image im, int dx, int dy, int w, int h)
{
    image cropped = make_image(w, h, im.c);
    int i, j, k;
    for(k = 0; k < im.c; ++k){
        for(j = 0; j < h; ++j){
            for(i = 0; i < w; ++i){
                int r = j + dy;
                int c = i + dx;
                float val = 0;
                r = constrain_int(r, 0, im.h-1);
                c = constrain_int(c, 0, im.w-1);
                if (r >= 0 && r < im.h && c >= 0 && c < im.w) {
                    val = get_pixel(im, c, r, k);
                }
                set_pixel(cropped, i, j, k, val);
            }
        }
    }
    return cropped;
}

int best_3d_shift_r(image a, image b, int min, int max)
{
    if(min == max) return min;
    int mid = floor((min + max) / 2.);
    image c1 = crop_image(b, 0, mid, b.w, b.h);
    image c2 = crop_image(b, 0, mid+1, b.w, b.h);
    float d1 = dist_array(c1.data, a.data, a.w*a.h*a.c, 10);
    float d2 = dist_array(c2.data, a.data, a.w*a.h*a.c, 10);
    free_image(c1);
    free_image(c2);
    if(d1 < d2) return best_3d_shift_r(a, b, min, mid);
    else return best_3d_shift_r(a, b, mid+1, max);
}

int best_3d_shift(image a, image b, int min, int max)
{
    int i;
    int best = 0;
    float best_distance = FLT_MAX;
    for(i = min; i <= max; i += 2){
        image c = crop_image(b, 0, i, b.w, b.h);
        float d = dist_array(c.data, a.data, a.w*a.h*a.c, 100);
        if(d < best_distance){
            best_distance = d;
            best = i;
        }
        printf("%d %f\n", i, d);
        free_image(c);
    }
    return best;
}

void composite_3d(char *f1, char *f2, char *out, int delta)
{
    if(!out) out = "out";
    image a = load_image(f1, 0,0,0);
    image b = load_image(f2, 0,0,0);
    int shift = best_3d_shift_r(a, b, -a.h/100, a.h/100);

    image c1 = crop_image(b, 10, shift, b.w, b.h);
    float d1 = dist_array(c1.data, a.data, a.w*a.h*a.c, 100);
    image c2 = crop_image(b, -10, shift, b.w, b.h);
    float d2 = dist_array(c2.data, a.data, a.w*a.h*a.c, 100);

    if(d2 < d1 && 0){
        image swap = a;
        a = b;
        b = swap;
        shift = -shift;
        printf("swapped, %d\n", shift);
    }
    else{
        printf("%d\n", shift);
    }

    image c = crop_image(b, delta, shift, a.w, a.h);
    int i;
    for(i = 0; i < c.w*c.h; ++i){
        c.data[i] = a.data[i];
    }
#ifdef OPENCV
    save_image_jpg(c, out);
#else
    save_image(c, out);
#endif
}

image resize_max(image im, int max)
{
    int w = im.w;
    int h = im.h;
    if(w > h){
        h = (h * max) / w;
        w = max;
    } else {
        w = (w * max) / h;
        h = max;
    }
    if(w == im.w && h == im.h) return im;
    image resized = resize_image(im, w, h);
    return resized;
}

image resize_min(image im, int min)
{
    int w = im.w;
    int h = im.h;
    if(w < h){
        h = (h * min) / w;
        w = min;
    } else {
        w = (w * min) / h;
        h = min;
    }
    if(w == im.w && h == im.h) return im;
    image resized = resize_image(im, w, h);
    return resized;
}

image random_crop_image(image im, int w, int h)
{
    int dx = rand_int(0, im.w - w);
    int dy = rand_int(0, im.h - h);
    image crop = crop_image(im, dx, dy, w, h);
    return crop;
}

image random_augment_image(image im, float angle, float aspect, int low, int high, int size)
{
    aspect = rand_scale(aspect);
    int r = rand_int(low, high);
    int min = (im.h < im.w*aspect) ? im.h : im.w*aspect;
    float scale = (float)r / min;

    float rad = rand_uniform(-angle, angle) * TWO_PI / 360.;

    float dx = (im.w*scale/aspect - size) / 2.;
    float dy = (im.h*scale - size) / 2.;
    if(dx < 0) dx = 0;
    if(dy < 0) dy = 0;
    dx = rand_uniform(-dx, dx);
    dy = rand_uniform(-dy, dy);

    image crop = rotate_crop_image(im, rad, scale, size, size, dx, dy, aspect);

    return crop;
}

float three_way_max(float a, float b, float c)
{
    return (a > b) ? ( (a > c) ? a : c) : ( (b > c) ? b : c) ;
}

float three_way_min(float a, float b, float c)
{
    return (a < b) ? ( (a < c) ? a : c) : ( (b < c) ? b : c) ;
}

// http://www.cs.rit.edu/~ncs/color/t_convert.html
void rgb_to_hsv(image im)
{
    assert(im.c == 3);
    int i, j;
    float r, g, b;
    float h, s, v;
    for(j = 0; j < im.h; ++j){
        for(i = 0; i < im.w; ++i){
            r = get_pixel(im, i , j, 0);
            g = get_pixel(im, i , j, 1);
            b = get_pixel(im, i , j, 2);
            float max = three_way_max(r,g,b);
            float min = three_way_min(r,g,b);
            float delta = max - min;
            v = max;
            if(max == 0){
                s = 0;
                h = 0;
            }else{
                s = delta/max;
                if(r == max){
                    h = (g - b) / delta;
                } else if (g == max) {
                    h = 2 + (b - r) / delta;
                } else {
                    h = 4 + (r - g) / delta;
                }
                if (h < 0) h += 6;
                h = h/6.;
            }
            set_pixel(im, i, j, 0, h);
            set_pixel(im, i, j, 1, s);
            set_pixel(im, i, j, 2, v);
        }
    }
}

void hsv_to_rgb(image im)
{
    assert(im.c == 3);
    int i, j;
    float r, g, b;
    float h, s, v;
    float f, p, q, t;
    for(j = 0; j < im.h; ++j){
        for(i = 0; i < im.w; ++i){
            h = 6 * get_pixel(im, i , j, 0);
            s = get_pixel(im, i , j, 1);
            v = get_pixel(im, i , j, 2);
            if (s == 0) {
                r = g = b = v;
            } else {
                int index = floor(h);
                f = h - index;
                p = v*(1-s);
                q = v*(1-s*f);
                t = v*(1-s*(1-f));
                if(index == 0){
                    r = v; g = t; b = p;
                } else if(index == 1){
                    r = q; g = v; b = p;
                } else if(index == 2){
                    r = p; g = v; b = t;
                } else if(index == 3){
                    r = p; g = q; b = v;
                } else if(index == 4){
                    r = t; g = p; b = v;
                } else {
                    r = v; g = p; b = q;
                }
            }
            set_pixel(im, i, j, 0, r);
            set_pixel(im, i, j, 1, g);
            set_pixel(im, i, j, 2, b);
        }
    }
}

image grayscale_image(image im)
{
    assert(im.c == 3);
    int i, j, k;
    image gray = make_image(im.w, im.h, 1);
    float scale[] = {0.587, 0.299, 0.114};
    for(k = 0; k < im.c; ++k){
        for(j = 0; j < im.h; ++j){
            for(i = 0; i < im.w; ++i){
                gray.data[i+im.w*j] += scale[k]*get_pixel(im, i, j, k);
            }
        }
    }
    return gray;
}

image threshold_image(image im, float thresh)
{
    int i;
    image t = make_image(im.w, im.h, im.c);
    for(i = 0; i < im.w*im.h*im.c; ++i){
        t.data[i] = im.data[i]>thresh ? 1 : 0;
    }
    return t;
}

image blend_image(image fore, image back, float alpha)
{
    assert(fore.w == back.w && fore.h == back.h && fore.c == back.c);
    image blend = make_image(fore.w, fore.h, fore.c);
    int i, j, k;
    for(k = 0; k < fore.c; ++k){
        for(j = 0; j < fore.h; ++j){
            for(i = 0; i < fore.w; ++i){
                float val = alpha * get_pixel(fore, i, j, k) + 
                    (1 - alpha)* get_pixel(back, i, j, k);
                set_pixel(blend, i, j, k, val);
            }
        }
    }
    return blend;
}

void scale_image_channel(image im, int c, float v)
{
    int i, j;
    for(j = 0; j < im.h; ++j){
        for(i = 0; i < im.w; ++i){
            float pix = get_pixel(im, i, j, c);
            pix = pix*v;
            set_pixel(im, i, j, c, pix);
        }
    }
}

void translate_image_channel(image im, int c, float v)
{
    int i, j;
    for(j = 0; j < im.h; ++j){
        for(i = 0; i < im.w; ++i){
            float pix = get_pixel(im, i, j, c);
            pix = pix+v;
            set_pixel(im, i, j, c, pix);
        }
    }
}

image binarize_image(image im)
{
    image c = copy_image(im);
    int i;
    for(i = 0; i < im.w * im.h * im.c; ++i){
        if(c.data[i] > .5) c.data[i] = 1;
        else c.data[i] = 0;
    }
    return c;
}

void saturate_image(image im, float sat)
{
    rgb_to_hsv(im);
    scale_image_channel(im, 1, sat);
    hsv_to_rgb(im);
    constrain_image(im);
}

void hue_image(image im, float hue)
{
    rgb_to_hsv(im);
    int i;
    for(i = 0; i < im.w*im.h; ++i){
        im.data[i] = im.data[i] + hue;
        if (im.data[i] > 1) im.data[i] -= 1;
        if (im.data[i] < 0) im.data[i] += 1;
    }
    hsv_to_rgb(im);
    constrain_image(im);
}

void exposure_image(image im, float sat)
{
    rgb_to_hsv(im);
    scale_image_channel(im, 2, sat);
    hsv_to_rgb(im);
    constrain_image(im);
}

void distort_image(image im, float hue, float sat, float val)
{
    rgb_to_hsv(im);
    scale_image_channel(im, 1, sat);
    scale_image_channel(im, 2, val);
    int i;
    for(i = 0; i < im.w*im.h; ++i){
        im.data[i] = im.data[i] + hue;
        if (im.data[i] > 1) im.data[i] -= 1;
        if (im.data[i] < 0) im.data[i] += 1;
    }
    hsv_to_rgb(im);
    constrain_image(im);
}

void random_distort_image(image im, float hue, float saturation, float exposure)
{
    float dhue = rand_uniform(-hue, hue);
    float dsat = rand_scale(saturation);
    float dexp = rand_scale(exposure);
    distort_image(im, dhue, dsat, dexp);
}

void saturate_exposure_image(image im, float sat, float exposure)
{
    rgb_to_hsv(im);
    scale_image_channel(im, 1, sat);
    scale_image_channel(im, 2, exposure);
    hsv_to_rgb(im);
    constrain_image(im);
}

float bilinear_interpolate(image im, float x, float y, int c)
{
    int ix = (int) floorf(x);
    int iy = (int) floorf(y);

    float dx = x - ix;
    float dy = y - iy;

    float val = (1-dy) * (1-dx) * get_pixel_extend(im, ix, iy, c) + 
        dy     * (1-dx) * get_pixel_extend(im, ix, iy+1, c) + 
        (1-dy) *   dx   * get_pixel_extend(im, ix+1, iy, c) +
        dy     *   dx   * get_pixel_extend(im, ix+1, iy+1, c);
    return val;
}

image resize_image(image im, int w, int h)
{
    image resized = make_image(w, h, im.c);   
    image part = make_image(w, im.h, im.c);
    int r, c, k;
    float w_scale = (float)(im.w - 1) / (w - 1);
    float h_scale = (float)(im.h - 1) / (h - 1);
    for(k = 0; k < im.c; ++k){
        for(r = 0; r < im.h; ++r){
            for(c = 0; c < w; ++c){
                float val = 0;
                if(c == w-1 || im.w == 1){
                    val = get_pixel(im, im.w-1, r, k);
                } else {
                    float sx = c*w_scale;
                    int ix = (int) sx;
                    float dx = sx - ix;
                    val = (1 - dx) * get_pixel(im, ix, r, k) + dx * get_pixel(im, ix+1, r, k);
                }
                set_pixel(part, c, r, k, val);
            }
        }
    }
    for(k = 0; k < im.c; ++k){
        for(r = 0; r < h; ++r){
            float sy = r*h_scale;
            int iy = (int) sy;
            float dy = sy - iy;
            for(c = 0; c < w; ++c){
                float val = (1-dy) * get_pixel(part, c, iy, k);
                set_pixel(resized, c, r, k, val);
            }
            if(r == h-1 || im.h == 1) continue;
            for(c = 0; c < w; ++c){
                float val = dy * get_pixel(part, c, iy+1, k);
                add_pixel(resized, c, r, k, val);
            }
        }
    }

    free_image(part);
    return resized;
}


void test_resize(char *filename)
{
    image im = load_image(filename, 0,0, 3);
    float mag = mag_array(im.data, im.w*im.h*im.c);
    printf("L2 Norm: %f\n", mag);
    image gray = grayscale_image(im);

    image c1 = copy_image(im);
    image c2 = copy_image(im);
    image c3 = copy_image(im);
    image c4 = copy_image(im);
    distort_image(c1, .1, 1.5, 1.5);
    distort_image(c2, -.1, .66666, .66666);
    distort_image(c3, .1, 1.5, .66666);
    distort_image(c4, .1, .66666, 1.5);


    show_image(im,   "Original");
    show_image(gray, "Gray");
    show_image(c1, "C1");
    show_image(c2, "C2");
    show_image(c3, "C3");
    show_image(c4, "C4");
#ifdef OPENCV
    while(1){
        image aug = random_augment_image(im, 0, .75, 320, 448, 320);
        show_image(aug, "aug");
        free_image(aug);


        float exposure = 1.15;
        float saturation = 1.15;
        float hue = .05;

        image c = copy_image(im);

        float dexp = rand_scale(exposure);
        float dsat = rand_scale(saturation);
        float dhue = rand_uniform(-hue, hue);

        distort_image(c, dhue, dsat, dexp);
        show_image(c, "rand");
        printf("%f %f %f\n", dhue, dsat, dexp);
        free_image(c);
        cvWaitKey(0);
    }
#endif
}


image load_image_stb(char *filename, int channels)
{
    int w, h, c;
    unsigned char *data = stbi_load(filename, &w, &h, &c, channels);
    if (!data) {
        fprintf(stderr, "Cannot load image \"%s\"\nSTB Reason: %s\n", filename, stbi_failure_reason());
        exit(0);
    }
    if(channels) c = channels;
    int i,j,k;
    image im = make_image(w, h, c);
    for(k = 0; k < c; ++k){
        for(j = 0; j < h; ++j){
            for(i = 0; i < w; ++i){
                int dst_index = i + w*j + w*h*k;
                int src_index = k + c*i + c*w*j;
                im.data[dst_index] = (float)data[src_index]/255.;
            }
        }
    }
    free(data);
    return im;
}

image load_image(char *filename, int w, int h, int c)
{
#ifdef OPENCV
    image out = load_image_cv(filename, c);
#else
    image out = load_image_stb(filename, c);
#endif

    if((h && w) && (h != out.h || w != out.w)){
        image resized = resize_image(out, w, h);
        free_image(out);
        out = resized;
    }
    return out;
}

image load_image_color(char *filename, int w, int h)
{
    return load_image(filename, w, h, 3);
}

image get_image_layer(image m, int l)
{
    image out = make_image(m.w, m.h, 1);
    int i;
    for(i = 0; i < m.h*m.w; ++i){
        out.data[i] = m.data[i+l*m.h*m.w];
    }
    return out;
}

float get_pixel(image m, int x, int y, int c)
{
    assert(x < m.w && y < m.h && c < m.c);
    return m.data[c*m.h*m.w + y*m.w + x];
}
float get_pixel_extend(image m, int x, int y, int c)
{
    if(x < 0) x = 0;
    if(x >= m.w) x = m.w-1;
    if(y < 0) y = 0;
    if(y >= m.h) y = m.h-1;
    if(c < 0 || c >= m.c) return 0;
    return get_pixel(m, x, y, c);
}
void set_pixel(image m, int x, int y, int c, float val)
{
    if (x < 0 || y < 0 || c < 0 || x >= m.w || y >= m.h || c >= m.c) return;
    assert(x < m.w && y < m.h && c < m.c);
    m.data[c*m.h*m.w + y*m.w + x] = val;
}
void add_pixel(image m, int x, int y, int c, float val)
{
    assert(x < m.w && y < m.h && c < m.c);
    m.data[c*m.h*m.w + y*m.w + x] += val;
}

void print_image(image m)
{
    int i, j, k;
    for(i =0 ; i < m.c; ++i){
        for(j =0 ; j < m.h; ++j){
            for(k = 0; k < m.w; ++k){
                printf("%.2lf, ", m.data[i*m.h*m.w + j*m.w + k]);
                if(k > 30) break;
            }
            printf("\n");
            if(j > 30) break;
        }
        printf("\n");
    }
    printf("\n");
}

image collapse_images_vert(image *ims, int n)
{
    int color = 1;
    int border = 1;
    int h,w,c;
    w = ims[0].w;
    h = (ims[0].h + border) * n - border;
    c = ims[0].c;
    if(c != 3 || !color){
        w = (w+border)*c - border;
        c = 1;
    }

    image filters = make_image(w, h, c);
    int i,j;
    for(i = 0; i < n; ++i){
        int h_offset = i*(ims[0].h+border);
        image copy = copy_image(ims[i]);
        //normalize_image(copy);
        if(c == 3 && color){
            embed_image(copy, filters, 0, h_offset);
        }
        else{
            for(j = 0; j < copy.c; ++j){
                int w_offset = j*(ims[0].w+border);
                image layer = get_image_layer(copy, j);
                embed_image(layer, filters, w_offset, h_offset);
                free_image(layer);
            }
        }
        free_image(copy);
    }
    return filters;
} 

image collapse_images_horz(image *ims, int n)
{
    int color = 1;
    int border = 1;
    int h,w,c;
    int size = ims[0].h;
    h = size;
    w = (ims[0].w + border) * n - border;
    c = ims[0].c;
    if(c != 3 || !color){
        h = (h+border)*c - border;
        c = 1;
    }

    image filters = make_image(w, h, c);
    int i,j;
    for(i = 0; i < n; ++i){
        int w_offset = i*(size+border);
        image copy = copy_image(ims[i]);
        //normalize_image(copy);
        if(c == 3 && color){
            embed_image(copy, filters, w_offset, 0);
        }
        else{
            for(j = 0; j < copy.c; ++j){
                int h_offset = j*(size+border);
                image layer = get_image_layer(copy, j);
                embed_image(layer, filters, w_offset, h_offset);
                free_image(layer);
            }
        }
        free_image(copy);
    }
    return filters;
} 

void show_image_normalized(image im, const char *name)
{
    image c = copy_image(im);
    normalize_image(c);
    show_image(c, name);
    free_image(c);
}

void show_images(image *ims, int n, char *window)
{
    image m = collapse_images_vert(ims, n);
    /*
       int w = 448;
       int h = ((float)m.h/m.w) * 448;
       if(h > 896){
       h = 896;
       w = ((float)m.w/m.h) * 896;
       }
       image sized = resize_image(m, w, h);
     */
    normalize_image(m);
    save_image(m, window);
    show_image(m, window);
    free_image(m);
}

void free_image(image m)
{
    if(m.data){
        free(m.data);
    }
}