#include "cvFiles.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/highgui/highgui_c.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

typedef unsigned int uint_t;

const float epsilon = 0.001;

int *mask_idx;
int mask_size;

float calc_sum(float *pos, uint_t *count)
{
    int *p, *e;
    float s, v;
    uint_t c;

    c = 0;
    s = 0;
    e = &mask_idx[mask_size];
    for (p = mask_idx; p < e; p++) {
        v = *(pos + *p);
        if (v > epsilon) {
            s += v;
            c++;
        }
    }
    *count = c;
    return s;
}

float calc_sum2(float *pos, uint_t *count)
{
    int *p, *e;
    float s, v;
    uint_t c;

    c = 0;
    s = 0;
    e = &mask_idx[mask_size];
    for (p = mask_idx; p < e; p++) {
        v = *(pos + *p);
        if (v > epsilon) {
            s += v*v;
            c++;
        }
    }
    *count = c;
    return s;
}

float calc_sum9(float *pos, size_t stride)
{
    float *pos1, *pos2, *pos3, *pos4, *pos5;
    pos1 = pos - 1 * stride - 1;
    pos2 = pos - 1;
    pos3 = pos + stride - 1;
    return
        *pos1 + *(pos1+1) + *(pos1+2) +
        *pos2 + *(pos2+1) + *(pos2+2) +
        *pos3 + *(pos3+1) + *(pos3+2);
}

float calc_dx(float *pos)
{
    float d, p1, p2, n1, n2;

    p1 = *(pos + 1);
    p2 = *(pos + 2);
    n1 = *(pos - 1);
    n2 = *(pos - 2);

    d = 0;
    if (p1 > epsilon && n1 > epsilon) {
        d = p1 - n1;
    }
    if (p2 > epsilon && n2 > epsilon) {
        d += (2*p2 - 2*n2);
    }

    return d;
}

float calc_dy(float *pos, uint_t stride)
{
    float d, p1, p2, n1, n2;

    p1 = *(pos + stride);
    p2 = *(pos + 2 * stride);
    n1 = *(pos - stride);
    n2 = *(pos - 2 * stride);

    d = 0;
    if (p1 > epsilon && n1 > epsilon) {
        d = p1 - n1;
    }
    if (p2 > epsilon && n2 > epsilon) {
        d += (2*p2 - 2*n2);
    }

    return d;
}

#define C_PI 3.14159265358979323846
const float pi = C_PI;
const float dir_diff_small = C_PI / 8;
const float dir_diff_large = C_PI / 4;

float direction_difference(float a, float b)
{
    float diff = a - b;
    if (diff < 0) diff = -diff;
    if (diff > pi) diff = 2*pi - diff;
    return diff;
}
/*
   bool is_same_direction(float a, float b, float diff)
   {
   float lower_bound, upper_bound;

   lower_bound = a - diff;
   upper_bound = b + diff;

   if (lower_bound < -pi) {
   return ((((lower_bound + 2*pi) < b) || -pi < b) && b < upper_bound)
   }
   else
   if (upper_bound > pi) {
   return (lower_bound < b && (b < pi || b < (upper_bound - 2*pi))); 
   }
   else {
   return (lower_bound < b && b < upper_bound);
   }
   return false;
   }
   */

float calc_directionality_1(float *pos, uint_t stride)
{
    float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;

    result = 0;
    dir_c = *pos;
    if (direction_difference(dir_c, pi) < dir_diff_small || direction_difference(dir_c, 0) < dir_diff_small) {
        dir_a = *(pos - stride);
        dir_b = *(pos + stride);

        value = *(pos - stride - 1);
        dir_diff_s1 = direction_difference(dir_c, value);
        value = *(pos - 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        value = (*pos + stride - 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        dir_diff_s1 /= 3;

        value = *(pos - stride + 1);
        dir_diff_s2 = direction_difference(dir_c, value);
        value = *(pos + 1);
        dir_diff_s2 += direction_difference(dir_c, value);
        value = *(pos + stride + 1);
        dir_diff_s2 += direction_difference(dir_c, value);
        dir_diff_s2 /= 3;

        if (dir_diff_s1 > dir_diff_s2) {
            diff_s = dir_diff_s1;
        }
        else {
            diff_s = dir_diff_s2;
        }

        if (diff_s > dir_diff_small) {
            diff_c = direction_difference(dir_c, dir_a);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
            diff_c = direction_difference(dir_c, dir_b);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
        }
    }
    return result;
}

float calc_directionality_2(float *pos, uint_t stride)
{
    float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;

    result = 0;
    dir_c = *pos;
    if (direction_difference(dir_c, 3*pi/4) < dir_diff_small || direction_difference(dir_c, -pi/4) < dir_diff_small) {
        dir_a = *(pos - stride + 1);
        dir_b = *(pos + stride - 1);

        value = *(pos - stride - 1);
        dir_diff_s1 = direction_difference(dir_c, value);
        value = *(pos - stride);
        dir_diff_s1 += direction_difference(dir_c, value);
        value = (*pos - 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        dir_diff_s1 /= 3;

        value = *(pos + stride + 1);
        dir_diff_s2 = direction_difference(dir_c, value);
        value = *(pos + stride);
        dir_diff_s2 += direction_difference(dir_c, value);
        value = *(pos + 1);
        dir_diff_s2 += direction_difference(dir_c, value);
        dir_diff_s2 /= 3;

        if (dir_diff_s1 > dir_diff_s2) {
            diff_s = dir_diff_s1;
        }
        else {
            diff_s = dir_diff_s2;
        }

        if (diff_s > dir_diff_small) {
            diff_c = direction_difference(dir_c, dir_a);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
            diff_c = direction_difference(dir_c, dir_b);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
        }
    }
    return result;
}

float calc_directionality_3(float *pos, uint_t stride)
{
    float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;

    result = 0;
    dir_c = *pos;
    if (direction_difference(dir_c, pi/2) < dir_diff_small || direction_difference(dir_c, -pi/2) < dir_diff_small) {
        dir_a = *(pos + 1);
        dir_b = *(pos - 1);

        value = *(pos - stride - 1);
        dir_diff_s1 = direction_difference(dir_c, value);
        value = *(pos - stride);
        dir_diff_s1 += direction_difference(dir_c, value);
        value = (*pos - stride + 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        dir_diff_s1 /= 3;

        value = *(pos + stride - 1);
        dir_diff_s2 = direction_difference(dir_c, value);
        value = *(pos + stride);
        dir_diff_s2 += direction_difference(dir_c, value);
        value = *(pos + stride + 1);
        dir_diff_s2 += direction_difference(dir_c, value);
        dir_diff_s2 /= 3;

        if (dir_diff_s1 > dir_diff_s2) {
            diff_s = dir_diff_s1;
        }
        else {
            diff_s = dir_diff_s2;
        }

        if (diff_s > dir_diff_small) {
            diff_c = direction_difference(dir_c, dir_a);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
            diff_c = direction_difference(dir_c, dir_b);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
        }
    }
    return result;
}

float calc_directionality_4(float *pos, uint_t stride)
{
    float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;

    result = 0;
    dir_c = *pos;
    if (direction_difference(dir_c, pi/4) < dir_diff_small || direction_difference(dir_c, -3*pi/4) < dir_diff_small) {
        dir_a = *(pos + stride + 1);
        dir_b = *(pos - stride - 1);

        value = *(pos - stride);
        dir_diff_s1 = direction_difference(dir_c, value);
        value = *(pos - stride + 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        value = (*pos + 1);
        dir_diff_s1 += direction_difference(dir_c, value);
        dir_diff_s1 /= 3;

        value = *(pos - 1);
        dir_diff_s2 = direction_difference(dir_c, value);
        value = *(pos + stride - 1);
        dir_diff_s2 += direction_difference(dir_c, value);
        value = *(pos + stride);
        dir_diff_s2 += direction_difference(dir_c, value);
        dir_diff_s2 /= 3;

        if (dir_diff_s1 > dir_diff_s2) {
            diff_s = dir_diff_s1;
        }
        else {
            diff_s = dir_diff_s2;
        }

        if (diff_s > dir_diff_small) {
            diff_c = direction_difference(dir_c, dir_a);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
            diff_c = direction_difference(dir_c, dir_b);
            if (diff_c < dir_diff_small) {
                result += 1.0;
            }
            else
                if (diff_c < diff_s) {
                    result += 0.5;
                }
        }
    }
    return result;
}

IplImage *read_from_tcr(const char *path, uint_t **timestamp_array)
{
    IplImage *dst;
    float *dst_data, *dst_pos;
    uint_t dst_stride, *timestamps, timestamp;
    size_t width, height, file_size, read_size, line_size, timestamp_size;
    size_t line_count, timestamp_count;
    CvSize size;
    FILE *input_file;

    width = 2592;
    line_size = width * sizeof(float);
    timestamp_size = sizeof(uint_t);
    dst = NULL;

    input_file = fopen(path, "rb");
    if (input_file == NULL) {
        printf("Error: unable to open file %s\n", path);
        return NULL;
    }

    /* seek file end to determine image size */
    fseek(input_file , 0 , SEEK_END);
    file_size = ftell(input_file);
    rewind(input_file);
    height = (size_t)(file_size / (line_size + timestamp_size));

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    if (dst == NULL) {
        printf("Error: unable to create image\n");
        return NULL;
    }

    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    if (timestamp_array != NULL) {
        timestamps = (uint_t *)malloc(height * timestamp_size);
        if (timestamps == NULL) {
            printf("Error: unable to allocate timestamp array\n");
            return NULL;
        }
        *timestamp_array = timestamps;
    }
    else {
        timestamps = NULL;
    }

    line_count = 0;
    timestamp_count = 0;

    dst_pos = dst_data;
    while (1) {
        /* read line timestamp to use for rectification */
        read_size = fread((void*)&timestamp, timestamp_size, 1, input_file);
        if (read_size < 1) {
            if (line_count < height) {
                printf("Error: unexpected end of file after line %d\n", line_count);
                return dst;
            }
            break;
        }
        else {
            if (timestamps != NULL) {
                timestamps[timestamp_count++] = timestamp;
            }
        }

        /* read data line */
        read_size = fread((void*)dst_pos, sizeof(float), width, input_file);
        if (read_size < width) {
            printf("Error unexpected end of file after line %d\n", line_count);
            return dst;
        }
        else {
            line_count += 1;
            dst_pos += dst_stride;
        }
    }
    fclose(input_file);

    return dst;
}

IplImage *rectify_tcr(IplImage *src, uint_t *timestamps, uint_t startx, uint_t endx, uint_t starty, uint_t endy)
{
    IplImage *dst;
    float *src_data, *dst_data, *src_pos, *dst_pos, tratio;
    uint_t src_stride, dst_stride, tstart, tend, tdiff, *timestamps_accum;
    size_t height, new_width, new_height, row_size;
    int i, j, result;
    CvSize size;

    new_width = endx - startx;
    new_height = new_width;

    tstart = timestamps[starty];
    tend = timestamps[endy];
    tdiff = tend - tstart;
    tratio = (float)tdiff / (float)new_height;

    /* create a lookup table for rectified image time stamps */
    timestamps_accum = (uint_t *)malloc(new_height * sizeof(uint_t));
    if (timestamps_accum == NULL) {
        printf("Error: rectify_tcr: unable to allocate cumulative timestamp array\n");
        return NULL;
    }

    timestamps_accum[0] = tstart;
    for (i = 1; i < new_height; i++) {
        timestamps_accum[i] = tstart + (uint_t)(tratio * i);
    }

    size.width = new_width;
    size.height = new_height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    if (dst == NULL) {
        printf("Error: rectify_tcr: unable to create image\n");
        return NULL;
    }
    height = src->height;
    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    src_pos = src_data + starty * src_stride + startx;
    j = starty;
    row_size = new_width * sizeof(float);
    dst_pos = dst_data;
    for (i = 0; i < new_height; i++, dst_pos += dst_stride) {
        if (timestamps[j] < timestamps_accum[i]) {
            j++;
            if (j == height) {
                printf("Error: rectify_tcr: row overflow\n");
                break;
            }
            src_pos += src_stride;
        }
        memcpy(dst_pos, src_pos, row_size);
    }

    free(timestamps_accum);
    return dst;
}

IplImage *derivative_direction(IplImage *src, float ignore_val, float ignore_eps)
{
    IplImage *dst, *tr, *ta;
    float *src_data, *src_pos, *dst_data, *dst_pos, *tr_data, *tr_pos, *ta_data, *ta_pos;
    float dx, dy, r, a, asum, amean;
    uint_t src_stride, dst_stride, tr_stride, ta_stride, width, height;
    CvSize size;
    int x, y;

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    tr = cvCreateImage(size, IPL_DEPTH_32F, 1);
    ta = cvCreateImage(size, IPL_DEPTH_32F, 1);
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(tr, cvScalarAll(0), NULL);
    cvSet(ta, cvScalarAll(0), NULL);
    cvSet(dst, cvScalarAll(0), NULL);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));
    tr_data = (float*)tr->imageData;
    tr_stride = (uint_t)(tr->widthStep / sizeof(float));
    ta_data = (float*)ta->imageData;
    ta_stride = (uint_t)(ta->widthStep / sizeof(float));

    for (y = 2; y < height - 2; y++) {
        src_pos = src_data + y * src_stride + 2;
        tr_pos = tr_data + y * tr_stride + 2;
        ta_pos = ta_data + y * ta_stride + 2;
        for (x = 2; x < width - 2; x++, src_pos++, tr_pos++, ta_pos++) {
            dx = calc_dx(src_pos);
            dy = calc_dy(src_pos, src_stride);
            r = sqrt(dx*dx+dy*dy);
            if (dx == 0 && dy == 0) {
                a = 0;
            }
            else {
                a = atan2(dy,dx);
            }
            *tr_pos = r;
            *ta_pos = a;
        }
    }

    for (y = 2; y < height - 2; y++) {
        dst_pos = dst_data + y * dst_stride + 2;
        tr_pos = tr_data + y * tr_stride + 2;
        ta_pos = ta_data + y * ta_stride + 2;
        for (x = 2; x < width - 2; x++, dst_pos++, tr_pos++, ta_pos++) {
            a = calc_directionality_1(ta_pos, ta_stride) + 
                0.5 * calc_directionality_2(ta_pos, ta_stride) +
                calc_directionality_3(ta_pos, ta_stride) +
                0.5 * calc_directionality_4(ta_pos, ta_stride);
            /*
               asum = calc_sum9(ta_pos, ta_stride);
               amean = asum / 9;
               */
            /*r = sqrt(dx*dx+dy*dy);*/
            /*
               if (abs(a - amean) > 0.3) {
             *dst_pos = a;
             }
             else {
             *dst_pos = 0;
             }
             */
            *dst_pos = a;
        }
    }

    cvReleaseImage(&tr);
    cvReleaseImage(&ta);
    return dst;
}

IplImage *mean_fill_holes(IplImage *src, int r, float ignore_val, float ignore_eps)
{
    IplImage *dst;
    float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
    uint_t src_stride, dst_stride, width, height, sum_count, min_count;
    CvSize size;
    int x, y, p, d;/*, mask_size, *mask_idx;*/

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(dst, cvScalarAll(ignore_val), NULL);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    d = (2*r+1);
    /* create the mask offset table for the averaging filter mask */
    mask_size = d*d;
    mask_idx = malloc(mask_size * sizeof(int));
    for (y = -r, p = 0; y <= r; y++) {
        for (x = -r; x <= r; x++) {
            mask_idx[p++] = y * src_stride + x;
        }
    }

    min_count = (uint_t)(mask_size / 2);

    for (y = r; y < height - r; y++) {
        src_pos = src_data + y * src_stride + r;
        dst_pos = dst_data + y * dst_stride + r;
        for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
            value = *src_pos;
            if ((ignore_val - ignore_eps) < value && value < (ignore_val + ignore_eps)) {
                sum = calc_sum(src_pos, &sum_count);
                if (sum_count > min_count) {
                    value = (sum/sum_count);
                }
            }
            *dst_pos = value;
        }
    }

    free(mask_idx);
    mask_idx = NULL;
    return dst;
}

IplImage *filter_mean(IplImage *src, int r, float ignore_val, float ignore_eps)
{
    IplImage *dst;
    float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
    uint_t src_stride, dst_stride, width, height, sum_count, min_count;
    CvSize size;
    int x, y, p, d;/*, mask_size, *mask_idx;*/

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(dst, cvScalarAll(ignore_val), NULL);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    d = (2*r+1);
    /* create the mask offset table for the averaging filter mask */
    mask_size = d*d;
    mask_idx = malloc(mask_size * sizeof(int));
    for (y = -r, p = 0; y <= r; y++) {
        for (x = -r; x <= r; x++) {
            mask_idx[p++] = y * src_stride + x;
        }
    }

    min_count = (uint_t)(mask_size / 2);

    for (y = r; y < height - r; y++) {
        src_pos = src_data + y * src_stride + r;
        dst_pos = dst_data + y * dst_stride + r;
        for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
            value = *src_pos;
            sum = calc_sum(src_pos, &sum_count);
            if (sum_count > min_count) {
                *dst_pos = (sum / sum_count);
            }
            else {
                *dst_pos = ignore_val;
            }
        }
    }

    free(mask_idx);
    mask_idx = NULL;
    return dst;
}

IplImage *image_diff(IplImage *src1, IplImage *src2, float ignore_val, float ignore_eps)
{
    IplImage *dst;
    float *src1_data, *src2_data, *dst_data, *src1_pos, *src2_pos, *dst_pos, value, diff;
    uint_t src1_stride, src2_stride, dst_stride, width, height;
    CvSize size;
    int x, y;

    width = src1->width;
    height = src1->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(dst, cvScalarAll(ignore_val), NULL);

    src1_data = (float*)src1->imageData;
    src1_stride = (uint_t)(src1->widthStep / sizeof(float));
    src2_data = (float*)src2->imageData;
    src2_stride = (uint_t)(src2->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    for (y = 0; y < height; y++) {
        src1_pos = src1_data + y * src1_stride;
        src2_pos = src2_data + y * src2_stride;
        dst_pos = dst_data + y * dst_stride;
        for (x = 0; x < width; x++, src1_pos++, src2_pos++, dst_pos++) {
            value = *src1_pos;
            if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
                diff = abs(value - *src2_pos);
                *dst_pos = diff;
            }
            else {
                *dst_pos = 0;
            }
        }
    }

    return dst;
}

IplImage *abs_diff_mean(IplImage *src, float ignore_val, float ignore_eps)
{
    IplImage *dst;
    float *src_data, *dst_data, *src_pos, *dst_pos;
    float value, sum, count, avg;
    uint_t src_stride, dst_stride, width, height;
    CvSize size;
    int x, y;

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(dst, cvScalarAll(0), NULL);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    sum = 0;
    count = 0;
    for (y = 0; y < height; y++) {
        src_pos = src_data + y * src_stride;
        for (x = 0; x < width; x++, src_pos++) {
            value = *src_pos;
            if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
                sum += value;
                count += 1;
            }
        }
    }

    avg = sum / count;

    for (y = 0; y < height; y++) {
        src_pos = src_data + y * src_stride;
        dst_pos = dst_data + y * dst_stride;
        for (x = 0; x < width; x++, src_pos++, dst_pos++) {
            value = *src_pos;
            if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
                *dst_pos = abs(value - avg);
            }
        }
    }

    return dst;
}

IplImage *box_mean_abs_diff(IplImage *src, int r, float ignore_val, float ignore_eps)
{
    IplImage *dst;
    float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
    uint_t src_stride, dst_stride, width, height, sum_count, min_count;
    CvSize size;
    int x, y, p, d;/*, mask_size, *mask_idx;*/

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
    cvSet(dst, cvScalarAll(ignore_val), NULL);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (float*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(float));

    d = (2*r+1);
    /* create the mask offset table for the averaging filter mask */
    mask_size = d*d;
    mask_idx = malloc(mask_size * sizeof(int));
    for (y = -r, p = 0; y <= r; y++) {
        for (x = -r; x <= r; x++) {
            mask_idx[p++] = y * src_stride + x;
        }
    }

    min_count = (uint_t)(mask_size / 2);

    for (y = r; y < height - r; y++) {
        src_pos = src_data + y * src_stride + r;
        dst_pos = dst_data + y * dst_stride + r;
        for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
            value = *src_pos;
            sum = calc_sum(src_pos, &sum_count);
            if (sum_count > min_count) {
                if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
                    value = abs((sum/sum_count) - value);
                }
                else {
                    value = ignore_val;
                }
                *dst_pos = value;
            }
            else {
                *dst_pos = ignore_val;
            }
        }
    }

    free(mask_idx);
    mask_idx = NULL;
    return dst;
}

void stretch_histogram_avg_sdv(IplImage *img, float ignore_val, float ignore_eps)
{
    float *img_data, *img_pos;
    uint_t img_stride, x, y, width, height;
    float value, min, max, sum1, sum2, count, avg, sdv;
    float bound_1, bound_2, stretch_1, stretch_2, stretch_3, stretch_4;

    width = img->width;
    height = img->height;
    img_data = (float*)img->imageData;
    img_stride = (uint_t)(img->widthStep / sizeof(float));

    min = 2000000000;
    max = 0;
    count = 0;
    sum1 = 0;
    sum2 = 0;
    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
                if (value < min) min = value; else if (value > max) max = value;
                sum1 += value;
                sum2 += value*value;
                count += 1;
            }
        }
    }

    avg = sum1 / count;
    sdv = sqrt(sum2 / count - avg*avg);
    printf("min=%f max=%f avg=%f sdv=%f\n", min, max, avg, sdv);

    if ((avg - 2 * sdv) > min) {
        min = avg - 2 * sdv;
    }
    if ((avg + 2 * sdv) < max) {
        max = avg + 2 * sdv;
    }
    if (avg - sdv < min) {
        bound_1 = min + 0.32 * (avg-min);
    }
    else {
        bound_1 = avg - sdv;
    }
    if (avg + sdv > max) {
        bound_2 = max - 0.32 * (max-avg);
    }
    else {
        bound_2 = avg + sdv;
    }
    stretch_1 = 0.25 / (bound_1 - min);
    stretch_2 = 0.25 / (avg - bound_1);
    stretch_3 = 0.25 / (bound_2 - avg);
    stretch_4 = 0.25 / (max - bound_2);

    /* normalize values and create the histogram */
    printf("b1=%f b2=%f b3=%f b4=%f b5=%f s1=%f s2=%f s3=%f s4=%f\n", min, bound_1, avg, bound_2, max, stretch_1, stretch_2, stretch_3, stretch_4);

    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if ((ignore_val - ignore_eps) < value && value < (ignore_val + ignore_eps)) {
                value = 0;
            }
            else
                if (value < min) {
                    value = 0;
                }
                else
                    if (value < bound_1) {
                        value = stretch_1 * (value - min);
                    }
                    else
                        if (value < avg) {
                            value = 0.25 + stretch_2 * (value - bound_1);
                        }
                        else
                            if (value < bound_2) {
                                value = 0.5 + stretch_3 * (value - avg);
                            }
                            else
                                if (value < max) {
                                    value = 0.75 + stretch_4 * (value - bound_2);
                                }
                                else {
                                    value = 1;
                                }

            *img_pos = value;
        }
    }
}

void equalize_histogram(IplImage *img, float ignore_val, float ignore_eps)
{
    float *img_data, *img_pos;
    uint_t img_stride, i, x, y, width, height, hidx;
    float value, count, min, max, stretch;
    float hist[256];

    width = img->width;
    height = img->height;
    img_data = (float*)img->imageData;
    img_stride = (uint_t)(img->widthStep / sizeof(float));

    min = 2000000000;
    max = 0;
    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
                if (value < min) min = value; else if (value > max) max = value;
            }
        }
    }

    stretch = 1.0 / (max - min);
    printf("min=%f max=%f stretch=%f\n", min, max, stretch);

    /* normalize values and create the histogram */
    for (i = 0; i < 256; i++) {
        hist[i] = 0;
    }

    count = 0;
    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
                value = stretch * (value - min);
                if (value < 0) value = 0; else if (value > 1) value = 1;
                hidx = (int)(255 * value);
                if (hidx < 0) hidx = 0; else if (hidx > 255) hidx = 255;
                hist[hidx] += 1;
                count += 1;
            }
            else {
                value = 0;
            }
            *img_pos = value;
        }
    }

    /* transform the histogram into a cumulative distribution */
    printf("%f ", hist[0]);
    hist[0] /= count;
    for (i = 1; i < 256; i++) {
        /*printf("(%.2f,", hist[i]);*/
        hist[i] = (hist[i] / count) + hist[i-1];
        /*printf("%.2f) ", hist[i]);*/
    }
    /*printf("\n");*/

    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            hidx = (int)(255 * value);
            if (hidx < 0) hidx = 0; else if (hidx > 255) hidx = 255;
            value = (hist[hidx] * value);
            if (value < 0) value = 0; else if (value > 1) value = 1;
            *img_pos = value;
        }
    }
}

void stretch_histogram(IplImage *img, float ignore_val, float ignore_eps)
{
    float *img_data, *img_pos;
    uint_t img_stride, x, y, width, height, count;
    float value, min, max, stretch;

    width = img->width;
    height = img->height;
    img_data = (float*)img->imageData;
    img_stride = (uint_t)(img->widthStep / sizeof(float));

    min = 2000000000;
    max = 0;
    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
                if (value < min) min = value; else if (value > max) max = value;
            }
        }
    }

    stretch = 1.0 / (max - min);
    printf("min=%f max=%f stretch=%f\n", min, max, stretch);

    for (y = 0; y < height; y++) {
        img_pos = img_data + y * img_stride;
        for (x = 0; x < width; x++, img_pos++) {
            value = *img_pos;
            if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
                value = stretch * (value - min);
                if (value < 0) value = 0; else if (value > 1) value = 1;
            }
            else {
                value = 0;
            }
            *img_pos = value;
        }
    }
}

IplImage *to_8bit(IplImage *src)
{
    IplImage *dst;
    float *src_data, *src_pos, value;
    unsigned char *dst_data, *dst_pos;
    uint_t x, y, width, height, src_stride, dst_stride;
    int temp;
    CvSize size;

    width = src->width;
    height = src->height;

    size.width = width;
    size.height = height;
    dst = cvCreateImage(size, IPL_DEPTH_8U, 1);

    src_data = (float*)src->imageData;
    src_stride = (uint_t)(src->widthStep / sizeof(float));
    dst_data = (unsigned char*)dst->imageData;
    dst_stride = (uint_t)(dst->widthStep / sizeof(unsigned char));

    for (y = 0; y < height; y++) {
        src_pos = src_data + y * src_stride;
        dst_pos = dst_data + y * dst_stride;
        for (x = 0; x < width; x++, src_pos++, dst_pos++) {
            value = *src_pos;
            temp = (int)(255 * value);
            if (temp < 0) temp = 0;
            else if (temp > 255) temp = 255;
            *dst_pos = (unsigned char)temp;
        }
    }

    return dst;
}

IplImage *read_from_tcr_mag(const char *path)
{
    IplImage *src, *tmp1, *tmp2, *dst;
    uint_t *timestamps;

    printf("read\n");
    src = read_from_tcr(path, &timestamps);
    printf("filter\n");
    tmp1 = filter_mean(src, 3, 0, 0.001);
    cvReleaseImage(&src);
    printf("derivate\n");
    tmp2 = derivative_direction(tmp1, 0, 0.001);
    cvReleaseImage(&tmp1);
    printf("equalize\n");
    equalize_histogram(tmp2, 0, 0.001);
    printf("convert\n");
    dst = to_8bit(tmp2);
    cvReleaseImage(&tmp2);
    cvSaveImage("tcr_mag.png", dst, 0);
    return dst;
}

IplImage *calc_derivative_direction(IplImage *src)
{
    IplImage *dst;
    dst = derivative_direction(src, 0, 0.001);
    equalize_histogram(dst, 0, 0.001);
    return dst;
}

IplImage *read_from_tcr_rectified(const char *path)
{
    IplImage *src, *tmp1, *tmp2, *dst;
    float *tmp_data, *tmp_pos, value;
    uint_t *timestamps, tmp_stride, width, height, x, y, r;
    uint_t minx, maxx, startx, endx, miny, maxy, starty, endy;

    src = read_from_tcr(path, &timestamps);
    if (src == NULL || timestamps == NULL) {
        return NULL;
    }

    /*src = mean_fill_holes(tmp, 3, 0, 0.001);*/
    /*cvReleaseImage(&tmp);*/
    /*tmp1 = filter_mean(src, 5, 0, 0.001);*/
    /*cvSaveImage("tcr_mean.png", tmp, 0);*/
    /*tmp2 = image_diff(src, tmp1, 0, 0.001);*/
    /*tmp2 = abs_diff_mean(tmp1, 0, 0.001);*/
    /*cvSaveImage("tcr_diff.png", dst, 0);*/
    tmp1 = box_mean_abs_diff(src, 5, 0, 0.001);
    cvReleaseImage(&src);
    /*cvReleaseImage(&tmp1);*/
    stretch_histogram_avg_sdv(tmp1, 0, 0.001);
    /*equalize_histogram(tmp2, 0, 0.001);*/
    /*stretch_histogram(tmp2, 0, 0.001);*/

    width = tmp1->width;
    height = tmp1->height;
    tmp_data = (float*)tmp1->imageData;
    tmp_stride = (uint_t)(tmp1->widthStep / sizeof(float));

    minx = 2000000000;
    maxx = 0;
    miny = 2000000000;
    maxy = 0;
    for (y = 0; y < height; y++) {
        tmp_pos = tmp_data + y * tmp_stride;
        for (x = 0; x < width; x++, tmp_pos++) {
            value = *tmp_pos;
            if (value > 0.001) {
                if (x < minx) minx = x; else if (x > maxx) maxx = x;
                if (y < miny) miny = y; else if (y > maxy) maxy = y;
            }
        }
    }
    printf("minx=%d maxx=%d miny=%d maxy=%d\n", minx, maxx, miny, maxy);

    startx = 0;
    if (minx > 2) startx = minx - 2;
    endx = width - 1;
    if (maxx < endx - 2) endx = maxx + 2;
    starty = 0;
    if (miny >  2) starty = miny - 2;
    endy = height - 1;
    if (maxy < endy - 2) endy = maxy + 2;

    tmp2 = rectify_tcr(tmp1, timestamps, startx, endx, starty, endy);
    cvReleaseImage(&tmp1);
    dst = to_8bit(tmp2);
    cvReleaseImage(&tmp2);
    cvSaveImage("tcr_eq.png", dst, 0);
    free(timestamps);

    return dst;
}

IplImage *read_from_tcr_rectified_old(const char *path)
{
    IplImage *src, *tmp, *tmp2, *tmp3, *tmp4, *dst;
    float *src_data, *tmp_data, *dst_data, *src_pos, *tmp_pos, *dst_pos;
    int src_stride, tmp_stride, dst_stride;
    unsigned int *timestamps, *timestamps_accum;
    unsigned int timestamp, timestamp_count, tstart, tend, tdiff;
    float tratio;
    CvSize size;
    size_t width, height, read_size;
    int line_width, timestamp_width, line_count, file_size;
    FILE *input_file;
    /* histogram for grayscale equalization */
    float hist[256];

    width = 2592;
    line_width = width * sizeof(float);
    timestamp_width = sizeof(unsigned int);
    src = NULL;
    tmp = NULL;
    dst = NULL;

    input_file = fopen(path, "rb");
    if (input_file != NULL) {
        /* seek file end to determine image size */
        fseek (input_file , 0 , SEEK_END);
        file_size = ftell(input_file);
        rewind (input_file);
        height = (size_t)(file_size / (line_width + timestamp_width));
        /*printf("File size: %d, rows: %d\n", file_size, height);*/

        size.width = width;
        size.height = height;
        src = cvCreateImage(size, IPL_DEPTH_32F, 1);
        tmp = cvCreateImage(size, IPL_DEPTH_32F, 1);
        tmp2 = cvCreateImage(size, IPL_DEPTH_32F, 1);
        tmp3 = cvCreateImage(size, IPL_DEPTH_32F, 1);
        tmp4 = cvCreateImage(size, IPL_DEPTH_32F, 1);
        src_data = (float*)src->imageData;
        tmp_data = (float*)tmp->imageData;
        src_stride = (int)(src->widthStep / sizeof(float));
        tmp_stride = (int)(tmp->widthStep / sizeof(float));

        cvSet(tmp, cvScalarAll(0), NULL);

        uint_t r = 2;
        uint_t d = (2*r+1);
        /* create the mask offset table for the averaging filter mask */
        mask_size = d*d;
        mask_idx = malloc(mask_size * sizeof(int));
        for (int i = -r, pos = 0; i <= r; i++) {
            for (int j = -r; j <= r; j++) {
                mask_idx[pos++] = i * src_stride + j;
                /*printf("%d ", mask_idx[pos-1]);*/
            }
        }
        /*printf("\n");*/

        timestamps = (unsigned int *)malloc(height * timestamp_width);
        if (src_data != NULL) {
            line_count = 0;
            timestamp_count = 0;

            src_pos = src_data;
            while (1) {
                /* read line timestamp to use for rectification */
                read_size = fread((void*)&timestamp, timestamp_width, 1, input_file);
                if (read_size < 1) {
                    break;
                }
                else {
                    timestamps[timestamp_count++] = timestamp;
                }

                /* read data line */
                read_size = fread((void*)src_pos, sizeof(float), width, input_file);
                if (read_size < width) {
                    break;
                }
                else {
                    line_count += 1;
                    src_pos += src_stride;
                }
            }
            fclose(input_file);

            /*cvSaveImage("tcr_raw.png", src, 0);*/
            /*
               cvSmooth(src,tmp,CV_GAUSSIAN,5,5,0,0);
               cvLaplace(tmp,tmp2,5);
               */
            /*
               cvSmooth(src,tmp,CV_BLUR,5,5,0,0);
               cvSmooth(src,tmp2,CV_GAUSSIAN,3,3,0,0);
               cvAbsDiff(tmp,src,tmp3);
               cvSaveImage("tcr_diff.png", tmp3, 0);
               cvSaveImage("tcr_smooth2.png", tmp2, 0);
               */
            /*
               cvSaveImage("tcr_laplacian5.png", tmp2, 0);
               */
            {
                unsigned int x, y, minx, maxx, miny, maxy, count;
                unsigned int new_width, new_height, startx, endx, starty, endy;
                float value, sum, sum1, sum2, min, max, ratio, stretch, avg, sdv;
                int sum_count, min_count;
                float stretch_1, stretch_2;

                min = 2000000000;
                max = 0;
                minx = 2000000000;
                maxx = 0;
                miny = 2000000000;
                maxy = 0;
                count = 0;
                sum1 = 0;
                sum2 = 0;
                min_count = mask_size / 2.0;
                for (y = r; y < height - r; y++) {
                    src_pos = src_data + y * src_stride + r;
                    tmp_pos = tmp_data + y * tmp_stride + r;
                    for (x = r; x < width - r; x++, src_pos++, tmp_pos++) {
                        value = *src_pos;
                        sum = calc_sum(src_pos, &sum_count);
                        if (sum_count > min_count) {
                            if (value > epsilon) {
                                value = abs((sum/sum_count) - value);
                                if (value < min) min = value; else if (value > max) max = value;
                                if (x < minx) minx = x; else if (x > maxx) maxx = x;
                                if (y < miny) miny = y; else if (y > maxy) maxy = y;
                                sum1 += value;
                                sum2 += value*value;
                                count += 1;
                            }
                            else {
                                value = 0;//sum/count;
                            }
                            *tmp_pos = value;
                        }
                        else {
                            *tmp_pos = 0;
                        }
                    }
                }
                printf("\n");
                avg = sum1 / count;
                sdv = sqrt(sum2 / count - avg*avg);
                printf("min=%f max=%f avg=%f sdv=%f minx=%d maxx=%d miny=%d maxy=%d\n", min, max, avg, sdv, minx, maxx, miny, maxy);
                /*cvSaveImage("tcr_filtered.png", tmp, 0);*/

                startx = r;
                if (minx > startx + 2) startx = minx - 2;
                endx = width - r - 1;
                if (maxx < endx - 2) endx = maxx + 2;
                starty = r;
                if (miny > starty + 2) starty = miny - 2;
                endy = height - r - 1;
                if (maxy < endy - 2) endy = maxy + 2;

                new_width = endx - startx;
                /*new_height = endy - starty;*/
                new_height = new_width;

                tstart = timestamps[starty];
                tend = timestamps[endy];
                tdiff = tend - tstart;
                ratio = (float)tdiff / (float)new_height;
                printf("start=%d end=%d diff=%d ratio=%f\n", tstart, tend, tdiff, ratio);

                /* create a lookup table for rectified image time stamps */
                timestamps_accum = (unsigned int *)malloc(new_height * timestamp_width);
                timestamps_accum[0] = tstart;
                for (int i = 1; i < new_height; i++) {
                    timestamps_accum[i] = tstart + (unsigned int)(ratio * i);
                }

                size.width = new_width;
                size.height = new_height;
                dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
                dst_data = (float*)dst->imageData;
                dst_stride = (int)(dst->widthStep / sizeof(float));

                dst_pos = dst_data;
                int j = starty;
                int row_size = new_width * sizeof(float);
                tmp_pos = tmp_data + starty * width + startx;
                for (int i = 0; i < new_height; i++, dst_pos += dst_stride) {
                    if (timestamps[j] < timestamps_accum[i]) {
                        j += 1;
                        if (j == height) {
                            printf("row overflow\n");
                            break;
                        }
                        tmp_pos += tmp_stride;
                    }
                    memcpy(dst_pos, tmp_pos, row_size);
                }

                free(timestamps_accum);
                /*cvSaveImage("tcr_rectified.png", dst, 0);*/

                int hidx;
                /* create a cumulative gaussian distribution table for normalizing the histogram */
                float norm[256];
                float nsum = 0;
                for (int i = 0; i < 256; i++) {
                    norm[i] = exp(-(pow(((float)i/255.0) - 0.5, 2.0) / (2*0.25*0.25)));
                    nsum += norm[i];
                }
                norm[0] /= nsum;
                for (int i = 1; i < 256; i++) {
                    norm[i] = (norm[i] / nsum) + norm[i-1];
                    /*printf("%f ", norm[i]);*/
                }
                /*printf("\n");*/
                for (int i = 0; i < 256; i++) {
                    hist[i] = 0;
                }
                stretch = 1.0 / ((avg+2*sdv) - 0);
                stretch_1 = 0.5 / (avg - 0);
                stretch_2 = 0.5 / (max - avg);

                /* normalize values and create the histogram */
                printf("stretch_1=%f stretch_2=%f\n", stretch_1, stretch_2);
                count = 0;
                for (y = 0; y < new_height; y++) {
                    dst_pos = dst_data + y * dst_stride;
                    for (x = 0; x < new_width; x++, dst_pos++) {
                        value = *dst_pos;
                        if (value > epsilon) {
                            if (value < (avg+epsilon)) {
                                value = stretch_1 * (value - 0);
                            }
                            else {
                                value = 0.5 + stretch_2 * (value - avg);
                            }
                            /*value = stretch * (value - 0);*/
                            hidx = (int)(255.0 * value);
                            if (hidx < 1) hidx = 1;
                            else if (hidx > 255) hidx = 255;
                            hist[hidx] += 1;
                            count += 1;
                            *dst_pos = value;
                        }
                        else {
                            *dst_pos = 0;
                        }
                    }
                }
                /*cvSaveImage("tcr_normalized.png", dst, 0);*/

                for (int i = 1; i < 256; i++) {
                    printf("%.3f ", hist[i]);
                }
                printf("\n");
                /* transform the histogram into a cumulative distribution */
                hist[0] = 0;
                for (int i = 1; i < 256; i++) {
                    hist[i] = (hist[i] / count) + hist[i-1];
                    printf("%.3f ", hist[i]);
                }
                printf("\n");

                for (y = 0; y < new_height; y++) {
                    dst_pos = dst_data + y * dst_stride;
                    for (x = 0; x < new_width; x++, dst_pos++) {
                        value = *dst_pos;
                        if (value < epsilon) {
                            value = 0;
                        }
                        else {
                            hidx = (int)(255.0 * value);
                            if (hidx < 1) hidx = 1;
                            else if (hidx > 255) hidx = 255;
                            value = (hist[hidx] * value); /*norm[hidx] * */
                        }
                        *dst_pos = value;
                    }
                }
                /*cvSaveImage("tcr_stretched.png", dst, 0);*/
            }
        }
        else {
            printf("Failed to allocate memory for image");
        }
        free(mask_idx);
        free(timestamps);
        cvReleaseImage(&tmp);
        cvReleaseImage(&tmp2);
        cvReleaseImage(&tmp3);
        cvReleaseImage(&tmp4);
        cvReleaseImage(&src);
    }
    else {
        printf("Failed to open file %s\n", path);
    }

    return dst;
}

IplImage *read_from_tcr_projected(const char *path)
{

}

IplImage *read_from_tcr_old(const char *path)
{
    IplImage *tmp, *dst;
    CvSize size;
    float *tmp_data, *dst_data, *tmp_pos, *dst_pos;
    /*unsigned char *b_data, *b_pos;*/
    float hist[256];
    size_t width = 2592;
    size_t height;
    int tmp_stride, dst_stride;/*, b_stride;*/
    int line_width = width * sizeof(float);
    int timestamp_width = sizeof(unsigned int);

    tmp = NULL;
    dst = NULL;
    FILE *input_file = fopen(path, "rb");
    size_t readsize;
    int line_count;
    int file_size;
    if (input_file != NULL) {
        unsigned int *timestamps, *timestamps_accum;
        unsigned int timestamp, prev_timestamp, timestamp_diff;
        unsigned int total_timestamp_diff;
        unsigned int total_timestamp_diff_2;
        unsigned int timestamp_count;
        float timestamp_avg, timestamp_sdv;

        fseek (input_file , 0 , SEEK_END);
        file_size = ftell (input_file);
        rewind (input_file);
        height = (size_t)(file_size / (line_width + timestamp_width));
        printf("File size: %d, rows: %d\n", file_size, height);
        size.width = width;
        size.height = height;
        tmp = cvCreateImage(size, IPL_DEPTH_32F, 1);
        tmp_data = (float*)tmp->imageData;
        tmp_stride = (int)(tmp->widthStep / sizeof(float));
        timestamps = (unsigned int *)malloc(height * timestamp_width);
        timestamps_accum = (unsigned int *)malloc(height * timestamp_width);
        if (tmp_data != NULL) {
            line_count = 0;
            prev_timestamp = 0;
            total_timestamp_diff = 0;
            total_timestamp_diff_2 = 0;
            timestamp_count = 1;
            timestamps[0] = 0;
            tmp_pos = tmp_data;

            while (1) {
                readsize = fread((void*)&timestamp, timestamp_width, 1, input_file);
                /*printf("Read %d elements for timestamp\n", readsize);*/
                if (readsize < 1) {
                    break;
                }
                else {
                    if (prev_timestamp > 0) {
                        timestamp_diff = (timestamp - prev_timestamp);
                        /*printf("%d ", timestamp_diff);*/
                        total_timestamp_diff += timestamp_diff;
                        total_timestamp_diff_2 += (timestamp_diff * timestamp_diff);
                        timestamps[timestamp_count] = timestamp_diff;
                        timestamp_count += 1;
                    }
                    prev_timestamp = timestamp;
                }
                readsize = fread((void*)tmp_pos, sizeof(float), width, input_file);
                /*printf("Read %d elements for line\n", readsize);*/
                if (readsize < width) {
                    break;
                }
                else {
                    line_count += 1;
                    tmp_pos += tmp_stride;
                }
            }

            fclose(input_file);
            timestamp_avg = total_timestamp_diff / timestamp_count;
            timestamp_sdv = sqrt(total_timestamp_diff_2 / timestamp_count - timestamp_avg*timestamp_avg);
            printf("Read %d lines, avg diff %f, diff sdv %f\n", line_count, timestamp_avg, timestamp_sdv);

            timestamps_accum[0] = 0;
            for (int i = i; i < height; i++) {
                timestamps_accum[i] = timestamps[i] / total_timestamp_diff + timestamps_accum[i-1];
            }

            {
                unsigned int pos, x, y, px, py, minx, maxx, miny, maxy, sumx, sumy, count;
                unsigned int new_width, new_height;
                float value, min, max, sum, sum2, avg, sdv, ratio, stretch, stretch_1, stretch_2;
                float a, r, rmin, rmax, cx, cy;
                unsigned char bval;

                min = 2000000000;
                max = 0;
                sum = 0;
                sum2 = 0;
                minx = 2000000000;
                maxx = 0;
                miny = 2000000000;
                maxy = 0;
                sumx = 0;
                sumy = 0;
                count = 0;

                for (y = 0; y < height; y++) {
                    tmp_pos = tmp_data + y * tmp_stride;
                    for (x = 0; x < width; x++, tmp_pos++) {
                        value = *tmp_pos;
                        if (value > 0.0001) {
                            if (value < min) min = value;
                            if (value > max) max = value;
                            if (x < minx) minx = x;
                            if (x > maxx) maxx = x;
                            if (y < miny) miny = y;
                            if (y > maxy) maxy = y;
                            sum += value;
                            sum2 += (value * value);
                            sumx += x;
                            sumy += y;
                            count += 1;
                        }
                    }
                }

                for (int i = 0; i < 256; i++) {
                    hist[i] = 0;
                }
                int hidx;

                avg = sum / count;
                sdv = sqrt((sum2 / count) - (avg * avg));
                cx = (float)sumx / count;
                cy = (float)sumy / count;
                ratio = (float)(maxy - miny) / (float)(maxx - minx);

                printf("amin=%f amax=%f aavg=%f asdv=%f\n", min, max, avg, sdv);
                printf("cx=%f cy=%f ratio=%f\n", cx, cy, ratio);

                stretch = 1.0 / (max - min);
                stretch_1 = 0.2 / (avg - min);
                stretch_2 = 0.8 / (max - avg);

                rmin = 2000000000;
                rmax = 0;
                count = 0;
                for (y = 0; y < height; y++) {
                    tmp_pos = tmp_data + y * tmp_stride;
                    for (x = 0; x < width; x++, tmp_pos++) {
                        value = *tmp_pos;
                        if (value > 0.0001) {
                            px = floor(x - cx);
                            py = floor((y / ratio) - (cy / ratio));
                            r = sqrt(py*py + px*px);
                            if (r < rmin) rmin = r;
                            if (r > rmax) rmax = r;
                            hidx = (int)(255 * stretch * (value - min));
                            if (hidx < 1) hidx = 1;
                            else if (hidx > 255) hidx = 255;
                            hist[hidx] += 1;
                            count += 1;
                        }
                    }
                }

                hist[0] = 0;
                for (int i = 1; i < 256; i++) {
                    hist[i] = (hist[i] / count) + hist[i-1];
                    printf("%f ", hist[i]);
                }

                new_height = ceil(rmax - rmin);
                new_width = ceil(2 * M_PI * rmax);
                printf("w=%d h=%d rmin=%f rmax=%f\n", new_width, new_height, rmin, rmax);
                size.width = new_width;
                size.height = new_height;

                dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
                dst_data = (float*)dst->imageData;
                dst_stride = (int)(dst->widthStep / sizeof(float));

                for (y = 0; y < new_height; y++) {
                    dst_pos = dst_data + y * dst_stride;
                    r = (float)(rmax - y);
                    for (x = 0; x < new_width; x++, dst_pos++) {
                        a = ((float)x / (float)new_width) * 2 * M_PI;
                        px = floor(cx + (r * cos(a)));
                        py = floor(((cy / ratio) + (r * sin(a))) * ratio);
                        if (px < 0 || py < 0 || px >= width || py >= height) {
                            /*printf("x=%d y=%d\n", px, py);*/
                        }
                        else {
                            value = *(tmp_data + py * tmp_stride + px);
                            if (value < 0.0001) {
                                value = 0;
                            }
                            if (value < avg) {
                                value = stretch_1 * (value - min);
                                hidx = (int)(255 * value);
                                if (hidx < 1) hidx = 1;
                                else if (hidx > 255) hidx = 255;
                                value = (hist[hidx] * value);
                            }
                            else {
                                value = 0.2 + stretch_2 * (value - avg);
                                hidx = (int)(255 * value);
                                if (hidx < 1) hidx = 1;
                                else if (hidx > 255) hidx = 255;
                                value = (hist[hidx] * value);
                            }
                            *dst_pos = value;
                        }
                    }
                }
            }
        }
        else {
            printf("Failed to allocate memory for image");
        }
        cvReleaseImage(&tmp);
    }
    else {
        printf("Failed to open file %s\n", path);
    }
    return dst;
}


IplImage *read_from_tcr_sdv(const char *path)
{
    IplImage *dst;
    CvSize size;
    float *data, *data2, *data_pos, *data2_pos;
    size_t width = 2592;
    size_t height;
    int line_width = width * sizeof(float);
    int timestamp_width = sizeof(unsigned int);

    dst = NULL;
    FILE *input_file = fopen(path, "rb");
    size_t readsize;
    int line_count;
    int file_size;
    if (input_file != NULL) {
        unsigned int timestamp, prev_timestamp, timestamp_diff;
        unsigned int total_timestamp_diff;
        unsigned int total_timestamp_diff_2;
        unsigned int timestamp_count;

        fseek (input_file , 0 , SEEK_END);
        file_size = ftell (input_file);
        rewind (input_file);
        height = (size_t)(file_size / (line_width + timestamp_width));
        printf("File size: %d, rows: %d\n", file_size, height);

        data = (float *)malloc(height * width * sizeof(float));
        data2 = (float *)malloc(height * width * sizeof(float));
        memset(data2, 0, height * width * sizeof(float));
        if (data != NULL) {
            line_count = 0;
            prev_timestamp = 0;
            total_timestamp_diff = 0;
            total_timestamp_diff_2 = 0;
            timestamp_count = 0;
            data_pos = data;

            while (1) {
                readsize = fread((void*)&timestamp, timestamp_width, 1, input_file);
                /*printf("Read %d elements for timestamp\n", readsize);*/
                if (readsize < 1) {
                    break;
                }
                else {
                    if (prev_timestamp > 0) {
                        timestamp_diff = (timestamp - prev_timestamp);
                        total_timestamp_diff += timestamp_diff;
                        total_timestamp_diff_2 += (timestamp_diff * timestamp_diff);
                        timestamp_count += 1;
                    }
                    prev_timestamp = timestamp;
                }
                readsize = fread((void*)data_pos, sizeof(float), width, input_file);
                /*printf("Read %d elements for line\n", readsize);*/
                if (readsize < width) {
                    break;
                }
                else {
                    line_count += 1;
                    data_pos += width;
                }
            }
            size.width = width;
            size.height = line_count;
            dst = cvCreateImageHeader(size, IPL_DEPTH_32F, 1);
            cvSetData(dst, (void*)data2, line_width);
            fclose(input_file);
            printf("Read %d lines, average diff %d\n", line_count, (int)(total_timestamp_diff / timestamp_count));

            {
                int size, pos, x, y, count, c, c2;
                float amin, amax, smin, smax, asum, asum2, ssum, ssum2, aavg, asdv, savg, ssdv;
                float value,v1,v2,v3,v4,v5,v6,v7,v8,v9,vsum,vsum2,vavg,vavg2,vsdv,vsdv2;
                float stretch_1, stretch_2;
                amin = 2000000000;
                smin = 2000000000;
                amax = 0;
                smax = 0;
                asum = 0;
                asum2 = 0;
                ssum = 0;
                ssum2 = 0;
                count = 0;
                vsum = 0;
                vsum2 = 0;
                vavg = 0;
                vavg2 = 0;
                vsdv = 0;
                vsdv2 = 0;
                for (y = 2; y < height - 2; y++) {
                    for (x = 2; x < width - 2; x++) {
                        data_pos = &data[y * width + x];
                        if (*data_pos > 0.0001) {
                            /*
                               v1 = *(data_pos - width - 1);
                               v2 = *(data_pos - width);
                               v3 = *(data_pos - width + 1);
                               v4 = *(data_pos - 1);
                               v5 = *data_pos;
                               v6 = *(data_pos + 1);
                               v7 = *(data_pos + width - 1);
                               v8 = *(data_pos + width);
                               v9 = *(data_pos + width + 1);
                               */
                            vsum = calc_sum(data_pos, &c);
                            /*v1+v2+v3+v4+v5+v6+v7+v8+v9;*/
                            vsum2 = calc_sum2(data_pos, &c2);
                            /*(v1*v1)+(v2*v2)+(v3*v3)+(v4*v4)+(v5*v5)+(v6*v6)+(v7*v7)+(v8*v8)+(v9*v9);*/
                            vavg = vsum / c;
                            vavg2 = vavg * vavg;
                            if (vavg < amin) amin = vavg;
                            else if (vavg > amax) amax = vavg;
                            asum += vavg;
                            asum2 += vavg2;

                            vsum2 /= c2;
                            if (vsum2 < vavg2) {
                                vsdv = 0;
                            }
                            else {
                                vsdv = sqrt(vsum2 - vavg2);
                            }
                            if (vsdv < smin) smin = vsdv;
                            else if (vsdv > smax) smax = vsdv;
                            ssum += vsdv;
                            ssum2 += (vsdv*vsdv);
                            count += 1;
                        }
                    }
                }
                /*
                   for (pos = 0; pos < size; pos++) {
                   value = data[pos];
                   v1 = data[pos-
                   if (value < 0.0001) {
                   count_0 += 1;
                   }
                   else {
                   if (value < min) min = value;
                   if (value > max) max = value;
                   sum_b += value;
                   sum2_b += (value * value);
                   count_b += 1;
                   }
                   }
                   */
                aavg = asum / count;
                asdv = sqrt((asum2 / count) - (aavg * aavg));
                savg = ssum / count;
                ssdv = sqrt((ssum2 / count) - (savg * savg));
                /*
                   if (min < (avg - 2 * sdv)) min = (avg - 2 * sdv);
                   if (max > (avg + 2 * sdv)) max = (avg + 2 * sdv);
                   */
                /*
                   stretch_1 = 0.1 / (avg - min);
                   stretch_2 = 0.9 / (max - avg);
                   */
                printf("amin=%f amax=%f aavg=%f asdv=%f\n", amin, amax, aavg, asdv);
                printf("smin=%f smax=%f savg=%f ssdv=%f\n", smin, smax, savg, ssdv);

                smin = 2000000000;
                smax = 0;
                ssum = 0;
                ssum2 = 0;
                count = 0;
                vsum = 0;
                vsum2 = 0;
                for (y = 2; y < height-2; y++) {
                    for (x = 2; x < width-2; x++) {
                        data_pos = &data[y * width + x];
                        data2_pos = &data2[y * width + x];
                        if (*data_pos > 0.0001) {
                            /*
                               v1 = *(data_pos - width - 1);
                               v2 = *(data_pos - width);
                               v3 = *(data_pos - width + 1);
                               v4 = *(data_pos - 1);
                               v5 = *data_pos;
                               v6 = *(data_pos + 1);
                               v7 = *(data_pos + width - 1);
                               v8 = *(data_pos + width);
                               v9 = *(data_pos + width + 1);
                               */
                            vsum = calc_sum(data_pos, &c);
                            /*v1+v2+v3+v4+v5+v6+v7+v8+v9;*/
                            vsum2 = calc_sum2(data_pos, &c2);
                            /*(v1*v1)+(v2*v2)+(v3*v3)+(v4*v4)+(v5*v5)+(v6*v6)+(v7*v7)+(v8*v8)+(v9*v9);*/
                            vavg = vsum/c;
                            vavg2 = vavg*vavg;
                            vsum2 /= c2;
                            if (vsum2 < vavg2) {
                                vsdv = 0;
                            }
                            else {
                                vsdv = sqrt(vsum2 - vavg2);
                            }
                            /*if (vsdv > savg) {*/
                            value = vsdv / ssdv; /* - savg */
                            *data2_pos = value;
                            if (value > 1) value = 1;
                            if (value < smin) smin = value;
                            if (value > smax) smax = value;
                            ssum += value;
                            ssum2 += value*value;
                            count += 1;
                            /*}*/
                        }
                    }
                }
                savg = ssum / count;
                ssdv = sqrt((ssum2 / count) - (savg*savg));
                printf("vmin=%f vmax=%f vavg=%f vsdv=%f\n", smin, smax, savg, ssdv);

                size = width * height;
                stretch_1 = 1.0 / (smax - smin);
                for (pos = 0; pos < size; pos++) {
                    value = data2[pos];
                    if (value < 0.0001) {
                        value = 0.5;
                    }
                    else {
                        value = stretch_1 * (value - smin);
                    }
                    data2[pos] = value;
                }
            }
        }
        else {
            printf("Failed to allocate memory for image");
        }
        free(data);
    }
    else {
        printf("Failed to open file %s\n", path);
    }
    return dst;
}
