/*
 * dct.cpp
 *
 * Author: M. Karnutsch (mkarnut@cosy.sbg.ac.at), C. Rathgeb (crathgeb@cosy.sbg.ac.at), P. Wild (pwild@cosy.sbg.ac.at)
 *
 * Generates an iris code from iris texture using the Monro et al. algorithm
 *
 */
#include <cstdio>
#include <map>
#include <vector>
#include <string>
#include <cstring>
#include <ctime>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>

using namespace std;
using namespace cv;

#define pi 3.1415926535897931
#define M_PI 3.14159265358979323846
#define pihalf 1.57079632679489661923

/*
 * default options
 */
#define DCT_CUTOFF 3
#define DCT_DEGREE 65
#define DCT_PATCHHEIGHT 8
#define DCT_PATCHWIDTH 12
#define DCT_PATCHES 85
#define DCT_ROWS 12
#define DCT_WNDHEIGHT 2
#define DCT_WNDWIDTH 1
#define DCT_COEFFSIZE 3

struct b_d{
    unsigned char **data;
    unsigned char **bits;
    int size;
    struct b_d *next;
};

struct allshifts_and_bits{
    int size;
    struct b_d *bitsdata;
};

struct window_functions
{
    double *window_w;
    double *window_h;
    int sizew;
    int sizeh;
};

/** no globbing in win32 mode **/
int _CRT_glob = 0;

/** Program modes **/
static const int MODE_MAIN = 1, MODE_HELP = 2;

/*
 * Print command line usage for this program
 */
void printUsage() {
	printf("+-----------------------------------------------------------------------------+\n");
	printf("| dct - Iris-code generation (feature extraction) using the Monro algorithm   |\n");
	printf("|                                                                             |\n");
	printf("| Monro D. M., Rakshit S., Zhang D.: DCT-based Iris Recognition,              |\n");
	printf("| IEEE Trans Pattern Anal Mach Intell. 2007 Apr;29(4):586-95.                 |\n");
	printf("|                                                                             |\n");
	printf("| MODES                                                                       |\n");
	printf("|                                                                             |\n");
    printf("| (# 1) DCT iris code extraction from iris textures                           |\n");
    printf("| (# 2) usage                                                                 |\n");
    printf("|                                                                             |\n");
    printf("| ARGUMENTS                                                                   |\n");
    printf("|                                                                             |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| Name | Parameters | # | ? | Description                                     |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| -i   | infile     | 1 | N | input iris texture (use * as wildcard, all other|\n");
    printf("|      |            |   |   | file parameters may refer to n-th * with ?n)    |\n");
    printf("| -o   | outfile    | 1 | N | output iris code image                          |\n");
    printf("| -q   |            | 1 | Y | quiet mode on (off)                             |\n");
    printf("| -t   |            | 1 | Y | time progress on (off)                          |\n");
    printf("| -h   |            | 2 | N | prints usage                                    |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("|                                                                             |\n");
    printf("| EXAMPLE USAGE                                                               |\n");
    printf("|                                                                             |\n");
    printf("| -i s1.tiff -o s1.png                                                        |\n");
    printf("| -i *.tiff -o ?1.png -q -t                                                   |\n");
    printf("|                                                                             |\n");
    printf("| AUTHORS                                                                     |\n");
    printf("|                                                                             |\n");
    printf("| Michael Karnutsch (mkarnut@cosy.sbg.ac.at)                                  |\n");
    printf("| Christian Rathgeb (crathgeb@cosy.sbg.ac.at)                                 |\n");
    printf("| Peter Wild (pwild@cosy.sbg.ac.at)                                           |\n");
    printf("|                                                                             |\n");
    printf("| COPYRIGHT                                                                   |\n");
    printf("|                                                                             |\n");
    printf("| (C) 2012 All rights reserved. Do not distribute without written permission. |\n");
    printf("+-----------------------------------------------------------------------------+\n");
}

/** ------------------------------- image processing functions ------------------------------- **/

unsigned char **data, **left4, **left8, **left12, **right4, **right8, **right12;

struct allshifts_and_bits *ab = NULL;
struct window_functions *window = NULL;

int h;
int w;
int rotateDCT[3] = {4,8,12};
int degree0[64] = {0};
int degree10[64] = {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7, 8, 8, 8, 8, 9, 9, 9, 9, 10, 10, 10, 10, 11, 11, 11, 11, 12, 12, 12, 12, 13, 13, 13, 13, 14, 14, 14, 14, 15, 15, 15, 15};
int degree17[64] = {0, 0, 0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 11, 11, 11, 12, 12, 12, 13, 13, 14, 14, 14, 15, 15, 15, 16, 16, 17, 17, 17, 18, 18, 18, 19, 19, 20, 20, 20, 21, 21, 21, 22, 22, 23, 23, 23};
int degree25[64] = {0, 0,  1, 1, 2, 2,  3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 13, 13, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 23, 24, 24, 25, 25, 26, 26, 27, 27, 28, 28, 29, 29, 30, 30, 31, 31};
int degree32[64] = {0, 1, 1, 2, 2, 3, 3, 4, 5, 6, 6, 7, 7, 8, 8, 9, 10, 11, 11, 12, 12, 13, 13, 14, 15, 16, 16, 17, 17, 18, 18, 19, 20, 21, 21, 22, 22, 23, 23, 24, 25, 26, 26, 27, 27, 28, 28, 29, 30, 31, 31, 32, 32, 33, 33, 34, 35, 36, 36, 37, 37, 38, 38, 39};
int degree38[64] = {0, 1, 1, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11, 12, 13, 13, 14, 15, 16, 16, 17, 18, 19, 19, 20, 21, 22, 22, 23, 24, 25, 25, 26, 27, 28, 28, 29, 30, 31, 31, 32, 33, 34, 34, 35, 36, 37, 37, 38, 39, 40, 40, 41, 42, 43, 43, 44, 45, 46, 46, 47};
int degree41[64] = {0, 1, 2, 3, 3, 4, 5, 6, 7, 8, 9, 10, 10, 11, 12, 13, 14, 15, 16, 17, 17, 18, 19, 20, 21, 22, 23, 24, 24, 25, 26, 27, 28, 29, 30, 31, 31, 32, 33, 34, 35, 36, 37, 38, 38, 39, 40, 41, 42, 43, 44, 45, 45, 46, 47, 48, 49, 50, 51, 52, 52, 53, 54, 55};
int degree45[64] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63};
int degree50[64] = {0, 1, 2, 3, 5, 6, 7, 8, 9, 10, 11, 12, 14, 15, 16, 17, 18, 19, 20, 21, 23, 24, 25, 26, 27, 28, 29, 30, 32, 33, 34, 35, 36, 37, 38, 39,  41, 42, 43, 44, 45,  46, 47, 48,  50, 51,  52, 53, 54, 55, 56, 57, 59, 60, 61, 62, 63, 64, 65, 66, 68, 69, 70, 71};
int degree55[64] = {0, 1, 3, 4, 5, 6, 8, 9, 10, 11, 13, 14, 15, 16, 18, 19, 20, 21, 23, 24, 25, 26, 28, 29, 30, 31, 33, 34, 35, 36, 38, 39, 40, 41, 43, 44, 45, 46, 48, 49, 50, 51, 53, 54, 55, 56, 58, 59, 60, 61, 63, 64, 65, 66, 68, 69, 70, 71, 73, 74, 75, 76, 78, 79};
int degree60[64] = {0, 1, 3, 5, 7, 9, 11, 12, 13, 14, 16, 18, 20, 22, 24, 25, 26, 27, 29, 31, 33, 35, 37, 38, 39, 40, 42, 44, 46, 48, 50, 51, 52, 53, 55, 57, 59, 61, 63, 64, 65, 66, 67, 69, 71, 73, 75, 77, 78, 79, 81, 83, 85, 87, 89, 90, 91, 92, 93, 95, 97, 99, 101, 102};
int degree65[64] = {0, 2, 4, 6, 8,  10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88,  90,  92,  94,  96,  98, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126};
int degree70[64] = {0, 2, 5, 7, 10, 12, 15, 17, 18, 20, 23, 25, 28, 30, 33, 35, 36, 38, 41, 43, 45, 47, 50, 52, 53, 55, 58, 60, 63, 65, 68, 70, 71, 73,  76,  78,  81,  83,  85,  87,  88,  90,  93,  95,  98, 100, 103, 105, 106, 108, 111, 113, 116, 118, 121, 123, 124, 126, 129, 131, 134, 136, 139, 141};
int degree75[64] = {0, 3, 6, 9, 12, 15, 18, 21, 24, 27, 30, 33, 36, 39, 42, 45, 48, 51, 54, 57, 60, 63, 66, 69, 72, 75, 78, 81, 84, 87, 90, 93, 96, 99, 102, 105, 108, 111, 114, 117, 120, 123, 126, 129, 132, 135, 138, 141, 145, 148, 151, 154, 157, 160, 163, 166, 169, 172, 175, 178, 181, 184, 187, 190};

void setBitTo1(uchar* code, int bitpos){
	code[bitpos / 8] |= (1 << (bitpos % 8));
}

void setBitTo0(uchar* code, int bitpos){
	code[bitpos / 8] &= (0xff ^ 1 << (bitpos % 8));
}

int signum(double i)
{
    if(i > 0.0) return 1;
    else return -1;
}

double *mydct(double *i, int n)
{
    long bin,k;
    double *d = (double*)malloc(sizeof(double) * n);
    double arg;
    double one = sqrt((float)n);
    one = 1/one;
    double two = 2/(float)n;
    two = sqrt(two);
    for (bin = 1; bin <= n; bin++)
    {
        d[bin - 1] = 0.;
        for (k = 1; k <= n; k++)
        {
            arg = (2*k - 1) * (bin - 1)  / (float)n;
            arg = arg * pihalf;
            d[bin - 1] += i[k - 1] * cos(arg);
        }
        if(bin == 1)
        {
            d[bin - 1] = d[bin - 1] * one;
        }
        else
        {
            d[bin - 1] = d[bin - 1] * two;
        }
    }
    return d;
}

unsigned char **truncate_image(const Mat& texture)
{
    h = (DCT_ROWS + 1)*(DCT_PATCHHEIGHT/2);
    w = (DCT_PATCHES + 9)*(DCT_PATCHWIDTH/2);
    unsigned char **image;
    image = (unsigned char**)malloc( h * sizeof(unsigned char*));
    int front = DCT_PATCHWIDTH;
    int i = (DCT_CUTOFF)*(texture.cols);
    int diff = w - texture.cols - front;
    uchar* textureData = texture.data;
    for(int q = 0; q < h; q++)
    {
        image[q] = (unsigned char*)malloc( w * sizeof(unsigned char));
    }

    if(diff > 0)
    {
      for(int r = 0; r < h*texture.cols; r++)
      {
          image[(r/512)][(r%512) + front] = textureData[r + i];
      }
        for(int q = 0; q < h; q++)
        {
            for(int e = 0; e < diff; e++)
            {
                image[q][texture.cols + e + front] = image[q][e + front];
                //printf("(%i,%i) = %d \n", q, img->w + e, image[q][e]);
            }
            for(int t = 0; t < front; t++)
            {
                image[q][t] = image[q][texture.cols + t];
            }
        }
    }
    else
    {
        for(int q = 0; q < h; q++)
        {
            for(int e = 0; e < w; e++)
            {
                image[q][e] = textureData[q*texture.cols + i + e];
            }
        }
    }
    return image;
}

unsigned char **shift_image(unsigned char **img)
{
    int *shiftpointer;
    switch(DCT_DEGREE)
    {
        case 0:
          shiftpointer = degree0;
          break;
        case 10:
          shiftpointer = degree10;
          break;
        case 17:
          shiftpointer = degree17;
          break;
        case 25:
          shiftpointer = degree25;
          break;
        case 32:
          shiftpointer = degree32;
          break;
        case 38:
          shiftpointer = degree38;
          break;
        case 41:
          shiftpointer = degree41;
          break;
        case 45:
          shiftpointer = degree45;
          break;
        case 50:
          shiftpointer = degree50;
          break;
        case 55:
          shiftpointer = degree55;
          break;
        case 60:
          shiftpointer = degree60;
          break;
        case 65:
          shiftpointer = degree65;
          break;
        case 70:
          shiftpointer = degree70;
          break;
        case 75:
          shiftpointer = degree75;
          break;
        default:
          shiftpointer = degree45;
          printf("degree: %i not possible, 45 used instead\n" , DCT_DEGREE);
          break;
    }
    unsigned char **shifted = (unsigned char**)malloc( h * sizeof(unsigned char*));
    int y = 0;
    while(y < h)
    {
        shifted[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        int i;
        for(i = 0; i < w; i++)
        {
            shifted[y][(i + shiftpointer[y])%w] = img[y][i];
        }
        y++;
    }
    return shifted;
}

void rotate_image()
{
    left4 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    left8 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    left12 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    right4 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    right8 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    right12 = (unsigned char**)malloc( h * sizeof(unsigned char*));
    int y = 0;
    while(y < h)
    {
        left4[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        left8[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        left12[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        right4[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        right8[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        right12[y] = (unsigned char*)malloc( w * sizeof(unsigned char));
        int i;
        for(i = 0; i < w; i++)
        {
          left4[y][i] = data[y][(i + rotateDCT[0])%w];
          left8[y][i] = data[y][(i + rotateDCT[1])%w];
          left12[y][i] = data[y][(i + rotateDCT[2])%w];
          right4[y][i] = data[y][(i + w - rotateDCT[0])%w];
          right8[y][i] = data[y][(i + w - rotateDCT[1])%w];
          right12[y][i] = data[y][(i + w -rotateDCT[2])%w];
        }
        y++;
    }
    ab = (allshifts_and_bits*)malloc(sizeof(struct allshifts_and_bits ));
    ab->size = 7;
    struct b_d *left12bd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *left8bd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *left4bd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *origbd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *right4bd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *right8bd = (b_d*)malloc(sizeof(struct b_d));
    struct b_d *right12bd = (b_d*)malloc(sizeof(struct b_d));
    origbd->data = data;
    origbd->next = right4bd;
    right4bd->data = right4;
    right4bd->next = right8bd;
    right8bd->data = right8;
    right8bd->next = right12bd;
    right12bd->data = right12;
    right12bd->next = left4bd;
    left4bd->data = left4;
    left4bd->next = left8bd;
    left8bd->data = left8;
    left8bd->next = left12bd;
    left12bd->data = left12;
    left12bd->next = NULL;
    ab->bitsdata = origbd;
}

double *compute_dct(double *patch)
{
    double *d, *all;
    d= (double*)malloc(DCT_COEFFSIZE * sizeof(double));
    all = mydct(patch, DCT_PATCHHEIGHT);
    for(int i = 0; i < DCT_COEFFSIZE; i++)
    {
        d[i] = all[i];
    }
    return d;
}

unsigned char *generate_codes()
{
    struct b_d *actualstruct = ab->bitsdata;
    unsigned char **actualdata;
    double actualpatch[DCT_PATCHHEIGHT];
    double *actualcoeff;
    double dctvalues[DCT_COEFFSIZE][DCT_PATCHES + 2 + 3];
    double diffs[DCT_COEFFSIZE][DCT_PATCHES + 1 + 3];
    unsigned char **bits;
    int howmanybits = DCT_PATCHES * DCT_ROWS;
    int extrabits = 0;
    int size = howmanybits;
    if(howmanybits%8 > 0)
    {
        howmanybits = howmanybits + 8 - howmanybits%8;
        size = howmanybits;
        extrabits = 1;
    }
    int  r, p, ah, aw, c, d, halfpatchsizeheight = DCT_PATCHHEIGHT/2, halfpatchsizewidth = DCT_PATCHWIDTH/2;
    while(actualstruct)
    {
        actualstruct->size = size;

        bits = (uchar**)malloc(sizeof(unsigned char*) * DCT_COEFFSIZE);
        int q;
        for(q = 0; q < DCT_COEFFSIZE; q++)
        {
            bits[q] = (unsigned char*)malloc(sizeof(unsigned char) * size);
        }
        actualdata = actualstruct->data;
        for(r = 0; r < DCT_ROWS; r++)
        {
            for(p = 0; p < DCT_PATCHES + 2 + 3; p++)
            {
                for(ah = 0; ah < DCT_PATCHHEIGHT; ah++)
                {
                    actualpatch[ah] = 0.0;
                    for(aw = 0; aw < DCT_PATCHWIDTH; aw++)
                    {
                        actualpatch[ah] += (window->window_w[aw] * actualdata[halfpatchsizeheight*r + ah][halfpatchsizewidth*p + aw]);
                    }
                    actualpatch[ah] = actualpatch[ah] * window->window_h[ah];
                }
                actualcoeff = compute_dct(actualpatch);
                for(c = 0; c < DCT_COEFFSIZE; c++)
                {
                    dctvalues[c][p] = actualcoeff[c];
                }
            }
            for(c = 0; c < DCT_COEFFSIZE; c++)
            {
                for(d = 0; d < DCT_PATCHES + 1 + 3; d++)
                {
                    diffs[c][d] = dctvalues[c][d + 1] - dctvalues[c][d];
                }
                for(d = 0; d < DCT_PATCHES; d++)
                {
                    if(signum(diffs[c][d + 3])==signum(diffs[c][d + 4]))
                    {
                        bits[c][r*DCT_PATCHES + d] = 0;
                    }
                    else
                    {
                        bits[c][r*DCT_PATCHES + d] = 1;
                    }
                }
            }
        }
        if(extrabits == 1)
        {
            for(c = 0; c < DCT_COEFFSIZE; c++)
            {
                for(int e = 0; e < size - DCT_PATCHES * DCT_ROWS; e++)
                {
                    int randbit = rand()%10 + 1;
                    if(randbit > 5)
                    {
                        bits[c][DCT_PATCHES * DCT_ROWS + e] = 1;
                    }
                    else
                    {
                        bits[c][DCT_PATCHES * DCT_ROWS + e] = 0;
                    }
                }
            }
        }
        actualstruct->bits = bits;
        actualstruct = actualstruct->next;
    }

    int idx_1,i;
    unsigned char *code = (unsigned char*)malloc(512*21*2 * sizeof(unsigned char));
    int cnt = 0;
    actualstruct = ab->bitsdata;
    while(actualstruct)
    {
        for(i = 0; i < DCT_COEFFSIZE; i++)
        {
            for (idx_1 = 0; idx_1 < actualstruct->size; idx_1++)
            {
                code[cnt*DCT_COEFFSIZE*actualstruct->size+i*actualstruct->size + idx_1] = actualstruct->bits[i][idx_1];
            }
        }
        cnt++;
        actualstruct = actualstruct->next;
    }
    return code;
}

double *hann(int size, int add)
{
    double *hh = (double*)malloc(sizeof(double) * size + 2*add);
    double *r = (double*)malloc(sizeof(double) * size);
    for(int j = 0; j < size; j++) r[j] = 0.0;

    int L = size + 2*add + 2;
    int N = L - 1;
    for(int i = 1; i <= size + 2*add ; i++)
    {
        hh[i-1] = (double)0.5 * (1.0 - cos(2.0*pi*(double)(i) / (double)N));
    }
    if(add >= 0)
    {
        for(int j = 0; j < size; j++)
        {
            r[j] = 0.0;
            r[j] = hh[j + add];
        }
    }
    else
    {
        for(int i = 0; i < size; i++)
        {
            r[i] = (double)0.0;
        }

        for(int k = -add; k < size + add; k++)
        {
            r[k] = hh[k + add];
        }
    }
    return r;
}

struct window_functions  *compute_window()
{
    struct window_functions *we  = (window_functions*)malloc(sizeof(struct window_functions));
    we->sizeh = DCT_PATCHHEIGHT + DCT_WNDHEIGHT;
    we->sizew = DCT_PATCHWIDTH + DCT_WNDWIDTH;
    we->window_h = hann(DCT_PATCHHEIGHT , DCT_WNDHEIGHT);
    we->window_w = hann(DCT_PATCHWIDTH , DCT_WNDWIDTH);
    return we;
}

void freeall()
{
    struct b_d *actualstruct = ab->bitsdata, *next;
    unsigned char **actualdata;
    unsigned char **actualbits;
    while(actualstruct)
    {
        actualdata = actualstruct->data;
        actualbits = actualstruct->bits;
        next = actualstruct->next;
        free(actualdata);
        free(actualbits);
        free(actualstruct);
        actualstruct = next;
    }
    free(ab);
    free(window);
}

/*
 * The Monro feature extraction algorithm
 *
 * code: Code matrix
 * texture: texture matrix
 */
void featureExtract(Mat& code, const Mat& texture)
{
    /*truncate to size*/
    unsigned char **temp;
    uchar* features;
    uchar* iris_code = code.data;

    //imagei = img;
    temp = truncate_image(texture);
    /*shift image by degree x*/
    data = shift_image(temp);
    /*rotates the image to the left and to the right and store in struct*/
    rotate_image();
    /*compute windowfunction*/
    window = compute_window();

    /*generates the codes for the 7 shifted images*/
    features = generate_codes();

    for (int i=0; i < texture.cols*(texture.rows/3)*2; i++)
    {
		if (features[i] == 0) setBitTo0(iris_code,i);
		else setBitTo1(iris_code,i);
    }
    //freeall();
}

/** ------------------------------- commandline functions ------------------------------- **/

/**
 * Parses a command line
 * This routine should be called for parsing command lines for executables.
 * Note, that all options require '-' as prefix and may contain an arbitrary
 * number of optional arguments.
 *
 * cmd: commandline representation
 * argc: number of parameters
 * argv: string array of argument values
 */
void cmdRead(map<string ,vector<string> >& cmd, int argc, char *argv[]){
	for (int i=1; i< argc; i++){
		char * argument = argv[i];
		if (strlen(argument) > 1 && argument[0] == '-' && (argument[1] < '0' || argument[1] > '9')){
			cmd[argument]; // insert
			char * argument2;
			while (i + 1 < argc && (strlen(argument2 = argv[i+1]) <= 1 || argument2[0] != '-'  || (argument2[1] >= '0' && argument2[1] <= '9'))){
				cmd[argument].push_back(argument2);
				i++;
			}
		}
		else {
			CV_Error(CV_StsBadArg,"Invalid command line format");
		}
	}
}

/**
 * Checks, if each command line option is valid, i.e. exists in the options array
 *
 * cmd: commandline representation
 * validOptions: list of valid options separated by pipe (i.e. |) character
 */
void cmdCheckOpts(map<string ,vector<string> >& cmd, const string validOptions){
	vector<string> tokens;
	const string delimiters = "|";
	string::size_type lastPos = validOptions.find_first_not_of(delimiters,0); // skip delimiters at beginning
	string::size_type pos = validOptions.find_first_of(delimiters, lastPos); // find first non-delimiter
	while (string::npos != pos || string::npos != lastPos){
		tokens.push_back(validOptions.substr(lastPos,pos - lastPos)); // add found token to vector
		lastPos = validOptions.find_first_not_of(delimiters,pos); // skip delimiters
		pos = validOptions.find_first_of(delimiters,lastPos); // find next non-delimiter
	}
	sort(tokens.begin(), tokens.end());
	for (map<string, vector<string> >::iterator it = cmd.begin(); it != cmd.end(); it++){
		if (!binary_search(tokens.begin(),tokens.end(),it->first)){
			CV_Error(CV_StsBadArg,"Command line parameter '" + it->first + "' not allowed.");
			tokens.clear();
			return;
		}
	}
	tokens.clear();
}

/*
 * Checks, if a specific required option exists in the command line
 *
 * cmd: commandline representation
 * option: option name
 */
void cmdCheckOptExists(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it == cmd.end()) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is required, but does not exist.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * size: appropriate number of parameters for the option
 */
void cmdCheckOptSize(map<string ,vector<string> >& cmd, const string option, const unsigned int size = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it->second.size() != size) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' has unexpected size.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * min: minimum appropriate number of parameters for the option
 * max: maximum appropriate number of parameters for the option
 */
void cmdCheckOptRange(map<string ,vector<string> >& cmd, string option, unsigned int min = 0, unsigned int max = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	unsigned int size = it->second.size();
	if (size < min || size > max) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is out of range.");
}

/*
 * Returns the list of parameters for a given option
 *
 * cmd: commandline representation
 * option: name of the option
 */
vector<string> * cmdGetOpt(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? &(it->second) : 0;
}

/*
 * Returns number of parameters in an option
 *
 * cmd: commandline representation
 * option: name of the option
 */
unsigned int cmdSizePars(map<string ,vector<string> >& cmd, const string option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? it->second.size() : 0;
}

/*
 * Returns a specific parameter type (int) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
int cmdGetParInt(map<string ,vector<string> >& cmd, string option, unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atoi(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (float) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
float cmdGetParFloat(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atof(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (string) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
string cmdGetPar(map<string ,vector<string> >& cmd, const string option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return it->second[param];
		}
	}
	return 0;
}

/** ------------------------------- timing functions ------------------------------- **/

/**
 * Class for handling timing progress information
 */
class Timing{
public:
	/** integer indicating progress with respect tot total **/
	int progress;
	/** total count for progress **/
	int total;

	/*
	 * Default constructor for timing initializing time.
	 * Automatically calls init()
	 *
	 * seconds: update interval in seconds
	 * eraseMode: if true, outputs sends erase characters at each print command
	 */
	Timing(long seconds, bool eraseMode){
		updateInterval = seconds;
		progress = 1;
		total = 100;
		eraseCount=0;
		erase = eraseMode;
		init();
	}

	/*
	 * Destructor
	 */
	~Timing(){}

	/*
	 * Initializes timing variables
	 */
	void init(void){
		start = boost::posix_time::microsec_clock::universal_time();
		lastPrint = start - boost::posix_time::seconds(updateInterval);
	}

	/*
	 * Clears printing (for erase option only)
	 */
	void clear(void){
		string erase(eraseCount,'\r');
		erase.append(eraseCount,' ');
		erase.append(eraseCount,'\r');
		printf("%s",erase.c_str());
		eraseCount = 0;
	}

	/*
	 * Updates current time and returns true, if output should be printed
	 */
	bool update(void){
		current = boost::posix_time::microsec_clock::universal_time();
		return ((current - lastPrint > boost::posix_time::seconds(updateInterval)) || (progress == total));
	}

	/*
	 * Prints timing object to STDOUT
	 */
	void print(void){
		lastPrint = current;
		float percent = 100.f * progress / total;
		boost::posix_time::time_duration passed = (current - start);
		boost::posix_time::time_duration togo = passed * (total - progress) / max(1,progress);
		if (erase) {
			string erase(eraseCount,'\r');
			printf("%s",erase.c_str());
			int newEraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
			if (newEraseCount < eraseCount) {
				string erase(newEraseCount-eraseCount,' ');
				erase.append(newEraseCount-eraseCount,'\r');
				printf("%s",erase.c_str());
			}
			eraseCount = newEraseCount;
		}
		else {
			eraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
		}
	}
private:
	long updateInterval;
	boost::posix_time::ptime start;
	boost::posix_time::ptime current;
	boost::posix_time::ptime lastPrint;
	int eraseCount;
	bool erase;
};

/** ------------------------------- file pattern matching functions ------------------------------- **/

/*
 * Formats a given string, such that it can be used as a regular expression
 * I.e. escapes special characters and uses * and ? as wildcards
 *
 * pattern: regular expression path pattern
 * pos: substring starting index
 * n: substring size
 *
 * returning: escaped substring
 */
string patternSubstrRegex(string& pattern, size_t pos, size_t n){
	string result;
	for (size_t i=pos, e=pos+n; i < e; i++ ) {
		char c = pattern[i];
		if ( c == '\\' || c == '.' || c == '+' || c == '[' || c == '{' || c == '|' || c == '(' || c == ')' || c == '^' || c == '$' || c == '}' || c == ']') {
			result.append(1,'\\');
			result.append(1,c);
		}
		else if (c == '*'){
			result.append("([^/\\\\]*)");
		}
		else if (c == '?'){
			result.append("([^/\\\\])");
		}
		else {
			result.append(1,c);
		}
	}
	return result;
}

/*
 * Converts a regular expression path pattern into a list of files matching with this pattern by replacing wildcards
 * starting in position pos assuming that all prior wildcards have been resolved yielding intermediate directory path.
 * I.e. this function appends the files in the specified path according to yet unresolved pattern by recursive calling.
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 * pos: an index such that positions 0...pos-1 of pattern are already considered/matched yielding path
 * path: the current directory (or empty)
 */
void patternToFiles(string& pattern, vector<string>& files, const size_t& pos, const string& path){
	size_t first_unknown = pattern.find_first_of("*?",pos); // find unknown * in pattern
	if (first_unknown != string::npos){
		size_t last_dirpath = pattern.find_last_of("/\\",first_unknown);
		size_t next_dirpath = pattern.find_first_of("/\\",first_unknown);
		if (next_dirpath != string::npos){
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,next_dirpath-last_dirpath-1) : patternSubstrRegex(pattern,pos,next_dirpath-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr( ((path.length() > 0) ? path + pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					if (boost::filesystem::is_directory(itr->path())){
						boost::filesystem::path p = itr->path().filename();
						string s =  p.string();
						if (boost::regex_match(s.c_str(), expr)){
							patternToFiles(pattern,files,(int)(next_dirpath+1),((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
						}
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
		else {
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,pattern.length()-last_dirpath-1) : patternSubstrRegex(pattern,pos,pattern.length()-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr(((path.length() > 0) ? path +  pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					boost::filesystem::path p = itr->path().filename();
					string s =  p.string();
					if (boost::regex_match(s.c_str(), expr)){
						files.push_back(((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
	}
	else { // no unknown symbols
		boost::filesystem::path file(((path.length() > 0) ? path + "/" : "") + pattern.substr(pos,pattern.length()-pos));
		if (boost::filesystem::exists(file)){
			files.push_back(file.string());
		}
	}
}

/**
 * Converts a regular expression path pattern into a list of files matching with this pattern
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 */
void patternToFiles(string& pattern, vector<string>& files){
	patternToFiles(pattern,files,0,"");
}

/*
 * Renames a given filename corresponding to the actual file pattern using a renaming pattern.
 * Wildcards can be referred to as ?1, ?2, ... in the order they appeared in the file pattern.
 *
 * pattern: regular expression path pattern
 * renamePattern: renaming pattern using ?1, ?2, ... as placeholders for wildcards
 * infile: path of the file (matching with pattern) to be renamed
 * outfile: path of the renamed file
 * par: used parameter (default: '?')
 */
void patternFileRename(string& pattern, const string& renamePattern, const string& infile, string& outfile, const char par = '?'){
	size_t first_unknown = renamePattern.find_first_of(par,0); // find unknown ? in renamePattern
	if (first_unknown != string::npos){
		string formatOut = "";
		for (size_t i=0, e=renamePattern.length(); i < e; i++ ) {
			char c = renamePattern[i];
			if ( c == par && i+1 < e) {
				c = renamePattern[i+1];
				if (c > '0' && c <= '9'){
					formatOut.append(1,'$');
					formatOut.append(1,c);
				}
				else {
					formatOut.append(1,par);
					formatOut.append(1,c);
				}
				i++;
			}
			else {
				formatOut.append(1,c);
			}
		}
		boost::regex patternOut(patternSubstrRegex(pattern,0,pattern.length()));
		outfile = boost::regex_replace(infile,patternOut,formatOut,boost::match_default | boost::format_perl);
	} else {
		outfile = renamePattern;
	}
}

/** ------------------------------- Program ------------------------------- **/

/*
 * Main program
 */
int main(int argc, char *argv[])
{
	int mode = MODE_HELP;
	map<string,vector<string> > cmd;
	try {
		cmdRead(cmd,argc,argv);
    	if (cmd.size() == 0 || cmdGetOpt(cmd,"-h") != 0) mode = MODE_HELP;
    	else mode = MODE_MAIN;
    	if (mode == MODE_MAIN){
			// validate command line
			cmdCheckOpts(cmd,"-i|-o|-q|-t");
			cmdCheckOptExists(cmd,"-i");
			cmdCheckOptSize(cmd,"-i",1);
			string inFiles = cmdGetPar(cmd,"-i");
			cmdCheckOptExists(cmd,"-o");
			cmdCheckOptSize(cmd,"-o",1);
			string outFiles = cmdGetPar(cmd,"-o");
			string imaskFiles, omaskFiles;
			bool quiet = false;
			if (cmdGetOpt(cmd,"-q") != 0){
				cmdCheckOptSize(cmd,"-q",0);
				quiet = true;
			}
			bool time = false;
			if (cmdGetOpt(cmd,"-t") != 0){
				cmdCheckOptSize(cmd,"-t",0);
				time = true;
			}
			// starting routine
			Timing timing(1,quiet);
			vector<string> files;
			patternToFiles(inFiles,files);
			CV_Assert(files.size() > 0);
			timing.total = files.size();
			for (vector<string>::iterator inFile = files.begin(); inFile != files.end(); ++inFile, timing.progress++){
				if (!quiet) printf("Loading texture '%s' ...\n", (*inFile).c_str());;
				Mat img = imread(*inFile, CV_LOAD_IMAGE_GRAYSCALE);
				CV_Assert(img.data != 0);
				Mat out;
				int w = img.cols;
				int h = (img.rows/DCT_COEFFSIZE)*2;
				if (!quiet) printf("Creating %d x %d iris-code ...\n", w, h);
				Mat code (1,(w*h)/8,CV_8UC1);
				code.setTo(0);
				featureExtract(code, img);
				out = code;
				string outfile;
				patternFileRename(inFiles,outFiles,*inFile,outfile);
				if (!quiet) printf("Storing code '%s' ...\n", outfile.c_str());
				if (!imwrite(outfile,out)) CV_Error(CV_StsError,"Could not save image '" + outfile + "'");
				if (time && timing.update()) timing.print();
			}
			if (time && quiet) timing.clear();
    	}
    	else if (mode == MODE_HELP){
			// validate command line
			cmdCheckOpts(cmd,"-h");
			if (cmdGetOpt(cmd,"-h") != 0) cmdCheckOptSize(cmd,"-h",0);
			// starting routine
			printUsage();
    	}
    }
	catch (...){
	   	printf("Exit with errors.\n");
	   	exit(EXIT_FAILURE);
	}
    return EXIT_SUCCESS;
}


#ifndef KOOPMAN_OPERATOR_HPP
#define KOOPMAN_OPERATOR_HPP

#include <armadillo>
#include "../system.hpp"
#include "basis_functions/basis_template.hpp"
#include <math.h>

class KoopmanOperator : public System
{

private:
    arma::mat _A;
    arma::mat _G;
    arma::mat _Ktrans;
public:
    arma::mat _K;
    arma::mat _Kx;
    arma::mat _Ku;

    int _nK;
    int _nM;
    int _nX;
    int _nU;
    int _nKU;

    Basis* basis;

    KoopmanOperator(Basis* _basis) : System(_basis->_nX, _basis->_nU) {
        basis = _basis;
        _nK = basis->_nK;
        _nX = basis->_nX;
        _nU = basis->_nU;
        _nM = basis->_nM;
        _nKU = basis->_nKU;
        _A = arma::zeros<arma::mat>(_nK, _nK);
        _G = arma::zeros<arma::mat>(_nK, _nK);
        _K = arma::zeros<arma::mat>(_nK, _nK);
        _Kx = 0.1*arma::ones<arma::mat>(_nX, _nX);
        _Ku = 0.1*arma::ones<arma::mat>(_nX, _nKU);

        _Ktrans = _K.t();
    }

    inline arma::vec f(const arma::vec& x, const arma::vec& u) {
        if (x.n_rows < _nX) {
            return _Kx * basis->fkx(x) + _Ku * basis->fku(x, u);
        } else {
            arma::vec fkx = basis->fkx(x.head_rows(_nM));
            arma::vec fku = basis->fku(x.head_rows(_nM), u);
            return _Kx * fkx + _Ku * fku;
        }
    }

    inline arma::mat fdx(const arma::vec& x, const arma::vec& u) {
        return _Kx;
    }
    inline arma::mat fdu(const arma::vec& x, const arma::vec& u) {
        return _Ku * basis->fkudu(x.head_rows(_nM), u);
    }

    void gradStep(const arma::vec& dataIn, const arma::vec& hdataIn, const arma::vec& dataOut, const arma::vec& hdataOut) {

        arma::vec phix = basis->fk(dataIn, hdataIn);
        arma::vec phixpo = basis->fk(dataOut, hdataOut);

        _G += phix * phix.t();
        _A += phix * phixpo.t();

        try {
            _K = arma::pinv(_G) * _A;
            _Ktrans = _K.t();
            _Kx = _Ktrans.submat(0, 0, _nX-1, _nX-1);
            _Ku = _Ktrans.submat(0, _nX, _nX-1, _nX + _nKU - 1);
        } catch (std::runtime_error& e) {
            std::cout << "CAUGHT ERROR" << std::endl;
        }

    }

    void computeOperator() {
      try {
            _K = arma::pinv(_G) * _A;
            _Ktrans = _K.t();
            _Kx = _Ktrans.submat(0, 0, _nX-1, _nX-1);
            _Ku = _Ktrans.submat(0, _nX, _nX-1, _nX + _nKU - 1);
        } catch (std::runtime_error& e) {
            std::cout << "CAUGHT ERROR" << std::endl;
        }
    }

    void saveOperator(std::string filePath) {
      _Ktrans.save(filePath + ".csv", arma::raw_ascii);
      _Ktrans.save(filePath + ".bin");
    }

    void loadOperator(std::string filePath) {
      _Ktrans.load(filePath);
      _K = _Ktrans.t();
      _Kx = _Ktrans.submat(0, 0 ,_nX-1, _nX-1);
      _Ku = _Ktrans.submat(0, _nX, _nX-1, _nX + _nKU-1);
      std::cout << "Loaded Koopman Operator." << std::endl;
    }

    void showOperator() {
        std::cout << _K.t() << std::endl;
    }
};

#endif


#include <iostream>
#include <math.h>
#include <Eigen/Dense>
using Eigen::Matrix;

float dt;
float vicon_R;
float accelerometer_R;
float sigma_J;
float sigma_bAcc;

Matrix<float, 12, 1> x_est;
Matrix<float, 12, 12> p_est;
Matrix<float, 12, 1> x_prd;
Matrix<float, 12, 12> p_prd;
Matrix<float, 12, 12> Q;
Matrix<float, 3, 3> R_vicon;
Matrix<float, 3, 3> R_acc;
Matrix<float, 12, 12> A;
Matrix<float, 3, 12> H_pos;
Matrix<float, 3, 12> H_v;
Matrix<float, 3, 12> H_acc;  //Acceleration without bias
Matrix<float, 3, 12> H_accm; //Measured acceleration (bias added)
Matrix<float, 3, 3> S;
Matrix<float, 3, 12> B;
Matrix<float, 12, 3> K;
Matrix<float, 3, 1> y;
Matrix<float, 3, 1> v;
Matrix<float, 3, 1> a;
Matrix<float, 3, 1> bAcc;
Matrix<float, 12, 1> result;
Matrix<float, 12, 6> Gammak;
Matrix<float, 6, 6> Qk;
Matrix<float, 12, 12> I_12x12;
Matrix<float, 3, 3> I_3x3;
Matrix<float, 3, 3> Zero_3x3;

void kalman_init()
{

	dt = 0.010;             // 100 Hz
  sigma_J = 20;           // Jerk process noise
  sigma_bAcc = 0.01;      // Accelerometer bias process noise
	vicon_R = 0.0001;       // estimated vicon standard deviation
  accelerometer_R = 0.5; // estimated accelerometer standard deviation
  I_12x12 = Matrix<float, 12, 12>::Identity();
  I_3x3 = Matrix<float, 3, 3>::Identity();
  Zero_3x3 = Matrix<float, 3, 3>::Zero(3, 3);

  Gammak << I_3x3 * (pow(dt, 3)/6), Zero_3x3,
            I_3x3 * (pow(dt, 2)/2), Zero_3x3,
                        I_3x3 * dt, Zero_3x3,
                          Zero_3x3, I_3x3*dt;
  Qk << pow(sigma_J, 2)*I_3x3,                 Zero_3x3,
                     Zero_3x3, pow(sigma_bAcc, 2)*I_3x3; //Process noise covariance matrix
  Q = Gammak*Qk*Gammak.transpose();

	R_vicon = pow(vicon_R, 2) * I_3x3;       // Vicon measurement covariance matrix
  R_acc = pow(accelerometer_R, 2) * I_3x3; // Accelerometer measurement covariance matrix

  A << I_3x3,    dt*I_3x3, (pow(dt,2)/2)*I_3x3, Zero_3x3,
       Zero_3x3,    I_3x3,            dt*I_3x3, Zero_3x3,
       Zero_3x3, Zero_3x3,               I_3x3, Zero_3x3,
       Zero_3x3, Zero_3x3,            Zero_3x3, I_3x3;

  //Initial estimate: zeros
  x_est = Matrix<float, 12, 1>::Zero();

  //Initial covariance: we assume we don't know initial position, but velocity and
  // acceleration are close to zero. Also, we don't know the accelerometer bias
  p_est << 10*I_3x3,     Zero_3x3,     Zero_3x3,  Zero_3x3,
            Zero_3x3, 0.0001*I_3x3,     Zero_3x3,  Zero_3x3,
            Zero_3x3,     Zero_3x3, 0.0001*I_3x3,  Zero_3x3,
            Zero_3x3,     Zero_3x3,     Zero_3x3, 1000*I_3x3;

  H_pos  << I_3x3,    Zero_3x3, Zero_3x3, Zero_3x3;
  H_v    << Zero_3x3, I_3x3,    Zero_3x3, Zero_3x3;
  H_acc  << Zero_3x3, Zero_3x3,    I_3x3, Zero_3x3; //Acceleration we want to determine
  H_accm << Zero_3x3, Zero_3x3,    I_3x3,    I_3x3; //Measured acceleration (includes bias)

}

Matrix<float, 12, 1> kalman_propagate()
{
  // Predicting state and covariance

  x_prd = A * x_est;
  p_prd = A * p_est * A.transpose() + Q;

  y = H_pos * x_prd; // Position estimate
  v = H_v * x_prd;   // Velocity estimate
  a = H_acc * x_prd; // Velocity estimate
  bAcc = (H_accm - H_acc)*x_prd; //Bias estimate

  result << y,
            v,
            a,
            bAcc;

  x_est = x_prd;
  p_est = p_prd;

  return result;

}

Matrix<float, 12, 1> kalman_estimate_pos(Matrix<float, 3, 1> z)
{

// Estimating state and covariance

  S = H_pos * p_prd.transpose() * H_pos.transpose() + R_vicon;
  B = H_pos * p_prd.transpose();
  K = (S.inverse() * B).transpose();

  x_est = x_prd + K * (z - H_pos * x_prd);
  p_est = (I_12x12 - K * H_pos) * p_prd * (I_12x12 - K * H_pos).transpose() + K * R_vicon * K.transpose();   // Joseph form

// Computing measurements
  y = H_pos * x_est;   // Position estimate
  v = H_v * x_est; // Velocity estimate
  a = H_acc * x_est; // Acc estimate
  bAcc = (H_accm - H_acc)*x_prd; //Bias estimate

  result << y,
            v,
            a,
            bAcc;

  //These are necessary if the acceleration is updated
  x_prd = x_est;
  p_prd = p_est;

  return result;
}

Matrix<float, 12, 1> kalman_estimate_acc(Matrix<float, 3, 1> z)
{

// Estimating state and covariance

  S = H_accm * p_prd.transpose() * H_accm.transpose() + R_acc;
  B = H_accm * p_prd.transpose();
  K = (S.inverse() * B).transpose();

  x_est = x_prd + K * (z - H_accm * x_prd);
  p_est = (I_12x12 - K * H_accm) * p_prd * (I_12x12 - K * H_accm).transpose() + K * R_acc * K.transpose();   // Joseph form

// Computing measurements
  y = H_pos * x_est;   // Position estimate
  v = H_v * x_est; // Velocity estimate
  a = H_acc * x_est;
  bAcc = (H_accm - H_acc)*x_prd; //Bias estimate

  result << y,
            v,
            a,
            bAcc;

  // std::cout << x_est << std::endl << std::endl;
  // std::cout << p_est << std::endl << std::endl;

  return result;
}

// int main()
// {

// 	kalman_init();

// 	MatrixXd z(3,1);   // measurement vector

// 	z << 1,
// 	     1,
// 	     1;

// 	std::cout << kalman(z) << std::endl;

// }


#include <iostream>
#include <cassert>
#include <vector>
#include <deque>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_data_structure_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <boost/graph/adjacency_list.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> TVB;
typedef CGAL::Triangulation_face_base_2<K> TFB;
typedef CGAL::Triangulation_data_structure_2<TVB, TFB> TriangulationDataStructure;
typedef CGAL::Delaunay_triangulation_2<K, TriangulationDataStructure> Triangulation;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

K::Point_2 read_point()
{
  int x, y;
  std::cin >> x >> y;
  assert(abs(x) < (1 << 24) && abs(y) < (1 << 24));
  return K::Point_2(x, y);
}

inline Triangulation::Vertex_handle vertex_from_edge(Triangulation::Edge &edge, int i)
{
  assert(i == 0 || i == 1);
  return edge.first->vertex((edge.second + 1 + i) % 3);
}

inline Triangulation::Vertex_handle other_vertex_from_edge(Triangulation::Edge &edge, Triangulation::Vertex_handle &v)
{
  auto vertex_a = vertex_from_edge(edge, 0);
  auto vertex_b = vertex_from_edge(edge, 1);
  if (vertex_a == v)
  {
    return vertex_b;
  }
  else if (vertex_b == v)
  {
    return vertex_a;
  }
  assert(false);
  __builtin_unreachable();
}

struct NetworkAnalysis
{
  NetworkAnalysis(int n) : valid(false), component_map(n, -1){};

  bool valid;
  std::vector<int> component_map;
};

NetworkAnalysis analyze_network(std::vector<std::pair<K::Point_2, int>> radio_stations, Triangulation &all_stations_triangulation, std::function<bool(double)> is_close_enough)
{
  const int n = radio_stations.size();
  NetworkAnalysis network_analysis(n);

  std::vector<Triangulation::Vertex_handle> all_stations_triangulation_handles(n);
  for (auto it = all_stations_triangulation.finite_vertices_begin(); it != all_stations_triangulation.finite_vertices_end(); it++)
  {
    all_stations_triangulation_handles.at(it->info()) = it;
  }

  std::deque<std::pair<int, int>> queue;
  std::vector<int> color_by_station(n, -1);
  int next_connected_component = 0;
  for (int queue_starter = 0; queue_starter < n; queue_starter++)
  {
    if (network_analysis.component_map.at(queue_starter) >= 0)
    {
      continue;
    }
    queue.push_back(std::make_pair(queue_starter, 0));
    const int this_connected_component = next_connected_component++;
    network_analysis.component_map.at(queue_starter) = this_connected_component;

    while (!queue.empty())
    {
      int prev_index = queue.front().first;
      int prev_color = queue.front().second;
      queue.pop_front();
      K::Point_2 &prev_point = radio_stations.at(prev_index).first;

      assert(color_by_station.at(prev_index) == -1);
      color_by_station.at(prev_index) = prev_color;

      auto &prev_vertex = all_stations_triangulation_handles.at(prev_index);
      Triangulation::Edge_circulator c = all_stations_triangulation.incident_edges(prev_vertex);
      do
      {
        if (all_stations_triangulation.is_infinite(c))
        {
          continue;
        }

        auto next_vertex = other_vertex_from_edge(*c, prev_vertex);
        int next_index = next_vertex->info();
        K::Point_2 &next_point = next_vertex->point();
        int next_color = 1 - prev_color;

        if (network_analysis.component_map.at(next_index) == -1 && is_close_enough(CGAL::squared_distance(prev_point, next_point)))
        {
          network_analysis.component_map.at(next_index) = this_connected_component;
          queue.push_front(std::make_pair(next_index, next_color));
        }
      } while (++c != all_stations_triangulation.incident_edges(prev_vertex));
    }
  }

  assert(*std::min_element(network_analysis.component_map.begin(), network_analysis.component_map.end()) >= 0);

  std::vector<std::vector<K::Point_2>> point_vectors_by_color(2);
  for (int i = 0; i < n; i++)
  {
    int color = color_by_station.at(i);
    point_vectors_by_color.at(color).push_back(radio_stations.at(i).first);
  }

  for (auto &points_this_color : point_vectors_by_color)
  {
    Triangulation color_triangulation;
    color_triangulation.insert(points_this_color.begin(), points_this_color.end());
    for (auto it = color_triangulation.finite_edges_begin(); it != color_triangulation.finite_edges_end(); it++)
    {
      if (is_close_enough(color_triangulation.segment(it).squared_length()))
      {
        return network_analysis;
      }
    }
  }

  network_analysis.valid = true;
  return network_analysis;
}

void testcase()
{
  int n, m, r;
  std::cin >> n >> m >> r;
  assert(n >= 1 && n <= 9e4 && m >= 1 && m <= 9e4 && r > 0 && r < (1 << 24));

  const double r_squared = pow(double(r), 2);
  const auto is_close_enough = [r_squared](double sqlen) { return sqlen <= r_squared; };

  std::vector<std::pair<K::Point_2, int>> radio_stations;
  for (int i = 0; i < n; i++)
  {
    radio_stations.push_back(std::make_pair(read_point(), i));
  }

  std::vector<std::pair<K::Point_2, K::Point_2>> clues;
  for (int i = 0; i < m; i++)
  {
    auto p1 = read_point();
    auto p2 = read_point();
    clues.push_back(std::make_pair(p1, p2));
  }

  Triangulation all_stations_triangulation;
  all_stations_triangulation.insert(radio_stations.begin(), radio_stations.end());

  NetworkAnalysis network_analysis = analyze_network(radio_stations, all_stations_triangulation, is_close_enough);
  DEBUG(2, "network_analysis.valid " << network_analysis.valid);

  const auto network_component_from_radio_set = [&is_close_enough, &all_stations_triangulation, &network_analysis](K::Point_2 &radio_set) -> int {
    auto nearest_station = all_stations_triangulation.nearest_vertex(radio_set);
    double set_station_sq_dist = CGAL::squared_distance(radio_set, nearest_station->point());
    if (!is_close_enough(set_station_sq_dist))
    {
      return -1;
    }
    return network_analysis.component_map.at(nearest_station->info());
  };

  std::vector<bool> can_transmit_by_clue(m, false);
  if (network_analysis.valid)
  {
    for (int i = 0; i < m; i++)
    {
      DEBUG(3, "clue " << i);
      auto &clue = clues.at(i);
      if (is_close_enough(CGAL::squared_distance(clue.first, clue.second)))
      {
        can_transmit_by_clue.at(i) = true;
      }
      else
      {
        int first_component = network_component_from_radio_set(clue.first);
        if (first_component >= 0 && first_component == network_component_from_radio_set(clue.second))
        {
          can_transmit_by_clue.at(i) = true;
        }
      }
    }
  }

  for (bool can_transmit : can_transmit_by_clue)
  {
    std::cout << (can_transmit ? "y" : "n");
  }
  std::cout << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include "acceleration/two_grid/spectral_shape/spectral_shape.hpp"

#include <deal.II/lac/petsc_full_matrix.h>
#include <numeric>

#include "solver/eigenvalue/tests/spectral_radius_mock.hpp"
#include "test_helpers/gmock_wrapper.h"
#include "test_helpers/test_helper_functions.h"

namespace  {

using namespace bart;
using ::testing::Return, ::testing::ResultOf, ::testing::ContainerEq, ::testing::Pointwise, ::testing::DoubleNear;

class CalculatorTwoGridSpectralShapeTest : public ::testing::Test {
 public:
  using Matrix = dealii::FullMatrix<double>;
  using PETScFullMatrix = dealii::PETScWrappers::FullMatrix;
  using EigenvalueSolver = solver::eigenvalue::SpectralRadiusMock;
  using MatrixValues = std::vector<std::vector<double>>;
  using TestClass = acceleration::two_grid::spectral_shape::SpectralShape;

  // Test object
  std::unique_ptr<TestClass> test_class_{ nullptr };

  // Supporting objects
  Matrix sigma_t, sigma_s;
  PETScFullMatrix expected_a;

  // Supporting mocks and observation pointers
  EigenvalueSolver* eigenvalue_solver_obs_ptr_{ nullptr };

  // test parameters
  const MatrixValues expected_a_values{{0, 1.0/4.0, 1.0/2.0}, {0, 1.0/20.0, 7.0/10.0}, {0, 13.0/120.0, 31.0/60.0}};
  static constexpr int n_groups{ 3 };
  const double eigenvalue_{ test_helpers::RandomDouble(-100, 100) };
  std::vector<double> eigenvector_;
  std::vector<double> spectral_shape_function_;

  auto SetUp() -> void override;
};

auto CalculatorTwoGridSpectralShapeTest::SetUp() -> void {

  const MatrixValues sigma_t_values{{7, 0, 0}, {0, 10, 0}, {0, 0, 13}};
  const MatrixValues sigma_s_values{{3, 1, 2}, {1, 5, 3}, {2, 3, 7}};

  sigma_t.reinit(n_groups, n_groups);
  sigma_s.reinit(n_groups, n_groups);
  expected_a.reinit(n_groups, n_groups);

  for (int i = 0; i < n_groups; ++i) {
    for (int j = 0; j < n_groups; ++j) {
      sigma_t.set(i, j, sigma_t_values.at(i).at(j));
      sigma_s.set(i, j, sigma_s_values.at(i).at(j));
      expected_a.set(i, j, expected_a_values.at(i).at(j));
    }
  }
  // The eigenvector will be returned normalized in the L2 norm
  eigenvector_ = test_helpers::RandomVector(n_groups, 0, 100);

  double magnitude{ 0.0 };
  for (int i = 0; i < n_groups; ++i)
    magnitude += eigenvector_.at(i) * eigenvector_.at(i);

  std::for_each(eigenvector_.begin(), eigenvector_.end(), [magnitude](double& val) { val /= std::sqrt(magnitude); });
  double sum{ 0.0 };
  for (int i = 0; i < n_groups; ++i) {
    sum += eigenvector_.at(i);
  }
  // The spectral shape function should be normalized in the L1 norm
  spectral_shape_function_ = eigenvector_;
  for (int i = 0; i < n_groups; ++i) {
    spectral_shape_function_.at(i) /= sum;
  }

  auto eigenvalue_solver_ptr = std::make_unique<EigenvalueSolver>();
  eigenvalue_solver_obs_ptr_ = eigenvalue_solver_ptr.get();

  test_class_ = std::make_unique<TestClass>(std::move(eigenvalue_solver_ptr));
}

TEST_F(CalculatorTwoGridSpectralShapeTest, Getters) {
  ASSERT_NE(test_class_->eigenvalue_solver_ptr(), nullptr);
  EXPECT_EQ(test_class_->eigenvalue_solver_ptr(), eigenvalue_solver_obs_ptr_);
}

TEST_F(CalculatorTwoGridSpectralShapeTest, ConstructorThrowsOnNullDependency) {
  EXPECT_ANY_THROW(TestClass(nullptr););
}

auto vectorize_matrix(const dealii::PETScWrappers::MatrixBase& to_vectorize) {
  std::vector<std::vector<double>> return_vector;
  for (unsigned int i = 0; i < to_vectorize.m(); ++i) {
    std::vector<double> row_values;
    for (unsigned int j = 0; j < to_vectorize.n(); ++j) {
      row_values.push_back(to_vectorize(i, j));
    }
    return_vector.push_back(row_values);
  }
  return return_vector;
}

MATCHER(VectorsNear, "") {
  std::vector<double> result;
  auto v1 = std::get<0>(arg);
  auto v2 = std::get<1>(arg);
  std::transform(v1.begin(), v1.end(), v2.begin(), std::back_inserter(result), std::minus<double>());
  for (const auto val : result) {
    if (std::abs(val) > 1e-6)
      return false;
  }
  return true;
}

// SpectralShape should return the correct value
TEST_F(CalculatorTwoGridSpectralShapeTest, SpectralShape) {
  std::pair<double, std::vector<double>> eigenpair{eigenvalue_, eigenvector_};

  EXPECT_CALL(*this->eigenvalue_solver_obs_ptr_, SpectralRadius(ResultOf(vectorize_matrix,
                                                                         Pointwise(VectorsNear(), expected_a_values))))
      .WillOnce(Return(eigenpair));
  auto spectral_shape_vector = test_class_->CalculateSpectralShape(sigma_t, sigma_s);
  EXPECT_THAT(spectral_shape_vector, ContainerEq(spectral_shape_function_));
}

// Spectral shape should fix the returned vector if it is negative (it should be normalized already)
TEST_F(CalculatorTwoGridSpectralShapeTest, SpectralShapeNegative) {
  std::vector<double> negative_eigenvector(eigenvector_);
  std::for_each(negative_eigenvector.begin(), negative_eigenvector.end(), [](double& val) { val *= -1; });

  std::pair<double, std::vector<double>> eigenpair{eigenvalue_, negative_eigenvector};

  EXPECT_CALL(*this->eigenvalue_solver_obs_ptr_, SpectralRadius(ResultOf(vectorize_matrix,
                                                                         Pointwise(VectorsNear(), expected_a_values))))
      .WillOnce(Return(eigenpair));
  auto spectral_shape_vector = test_class_->CalculateSpectralShape(sigma_t, sigma_s);
  EXPECT_THAT(spectral_shape_vector, ContainerEq(spectral_shape_function_));
}

TEST_F(CalculatorTwoGridSpectralShapeTest, BadMatrixSize) {
  for (const int bad_size : {0, n_groups + 1, n_groups - 1} ) {
    Matrix bad_rows(bad_size, n_groups);
    Matrix bad_cols(n_groups, bad_size);
    for (auto matrix : {sigma_t, sigma_s}) {
      EXPECT_ANY_THROW(test_class_->CalculateSpectralShape(matrix, bad_rows));
      EXPECT_ANY_THROW(test_class_->CalculateSpectralShape(bad_rows, matrix));
      EXPECT_ANY_THROW(test_class_->CalculateSpectralShape(matrix, bad_cols));
      EXPECT_ANY_THROW(test_class_->CalculateSpectralShape(bad_cols, matrix));
    }
  }
}

} // namespace


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[register_multi_point
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_MULTI_POINT

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/multi/geometries/register/multi_point.hpp>
#include <boost/geometry/multi/io/wkt/wkt.hpp>

typedef boost::tuple<float, float> point_type;

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_MULTI_POINT(std::deque< ::point_type >)

int main()
{
    // Normal usage of std::
    std::deque<point_type> multi_point;
    multi_point.push_back(point_type(1, 1));
    multi_point.push_back(point_type(3, 2));

    // Usage of Boost.Geometry
    std::cout << "WKT: "  << boost::geometry::wkt(multi_point) << std::endl;

    return 0;
}

//]

//[register_multi_point_output
/*`
Output:
[pre
WKT: MULTIPOINT((1 1),(3 2))
]
*/
//]


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>

#include <boost/graph/breadth_first_search.hpp>

#include <fstream>

typedef CGAL::Simple_cartesian<double>                               Kernel;
typedef Kernel::Point_3                                              Point;
typedef CGAL::Polyhedron_3<Kernel,CGAL::Polyhedron_items_with_id_3>  Polyhedron;

typedef boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Polyhedron>::vertex_iterator   vertex_iterator;

int main(int, char** argv) {

  Polyhedron P;
  std::ifstream in(argv[1]);
  in >> P ;

  // associate indices to the vertices using the "id()" field of the vertex.
  vertex_iterator vb, ve;
  int index = 0;

  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vit and ve
  for(boost::tie(vb,ve)=vertices(P); vb!=ve; ++vb ){
    vertex_descriptor  vd = *vb;
    vd->id() = index++;
  }

  // This is the vector where the distance gets written to
  std::vector<int> distance(P.size_of_vertices());

  // Here we start at an arbitrary vertex
  // Any other vertex could be the starting point
  boost::tie(vb,ve)=vertices(P);
  vertex_descriptor  vd = *vb;

  std::cout << "We compute distances to " << vd->point() << std::endl;

  // bfs = breadth first search explores the graph
  // Just as the distance_recorder there is a way to record the predecessor of a vertex
  boost::breadth_first_search(P,
			      vd,
			      visitor(boost::make_bfs_visitor(boost::record_distances(make_iterator_property_map(distance.begin(),
														 get(boost::vertex_index, P)),
										      boost::on_tree_edge()))));

  // Traverse all vertices and show at what distance they are
  for(boost::tie(vb,ve)=vertices(P); vb!=ve; ++vb ){
    vd = *vb;
    std::cout <<  vd->point() << "  is " << distance[vd->id()] << " hops away" << std::endl;
  }

  return 0;
}


/*
 * Array can  perform coefficient-wise operations, which might not have a linear algebraic meaning.
 * Refs:
 * https://eigen.tuxfamily.org/dox/group__TutorialArrayClass.html
 * */

#include <iostream>
#include <Eigen/Dense>

#define PRINT(x) std::cout << #x << ": " << std::endl << (x) << std::endl << std::endl
#define PRINT_SIZE(x) std::cout << #x << " is of size " << x.rows() << "x" << x.cols() << std::endl << std::endl
#define SECTION(x) std::cout << "======================== " << x << " =======================" << std::endl << std::endl

int main() {

  using namespace Eigen;

  SECTION("Initialisations");
  ArrayXXf m1(2,2); // Array<double,Dynamic,Dynamic>
  m1 = Array22f::Ones();
  PRINT(m1);
  Array22f m2;
  m2 << 1, 2, 3, 4;

  // https://eigen.tuxfamily.org/dox/group__TutorialAdvancedInitialization.html
  ArrayXXf table(10, 4);
  table.col(0) = ArrayXf::LinSpaced(10, 0, 90);
  table.col(1) = M_PI / 180 * table.col(0);
  table.col(2) = table.col(1).sin();
  table.col(3) = table.col(1).cos();
  std::cout << "table: " << std::endl;
  std::cout << "\tDegrees\t\tRadians\t\tSine\t\tCosine\n";
  std::cout << table << std::endl;

  SECTION("Operations");
  PRINT(m1+5);
  PRINT(m1+m2);
  PRINT(m1*2);
  PRINT(m1*m2);

  SECTION("Reduction");
  Array33f m3 = Array33f::Random();
  PRINT(m3);
  PRINT(m3.abs().sqrt());
  PRINT(m3.min(m3.abs().sqrt()));
  // more coefficient-wise & Array operators https://eigen.tuxfamily.org/dox/group__QuickRefPage.html
  // boolean https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
  PRINT((m3 > 0).all());
  PRINT((m3 > 0).any());
  PRINT((m3 > 0).count());

  SECTION("Types converting");
  // convert type
  MatrixXf m(2,2);
  MatrixXf n(2,2);
  MatrixXf result(2,2);
  m << 1,2,
  3,4;
  n << 5,6,
  7,8;
  PRINT(m);
  PRINT(n);
  result = m * n;
  std::cout << "-- Matrix m*n: --" << std::endl << result << std::endl << std::endl;
  result = m.array() * n.array();
  std::cout << "-- Array m*n: --" << std::endl << result << std::endl << std::endl;
  result = m.cwiseProduct(n);
  std::cout << "-- With cwiseProduct: --" << std::endl << result << std::endl << std::endl;
  result = m.array() + 4;
  std::cout << "-- Array m + 4: --" << std::endl << result << std::endl << std::endl;
  result = (m.array() + 4).matrix() * m;
  std::cout << "-- Combination 1: --" << std::endl << result << std::endl << std::endl;
  result = (m.array() * n.array()).matrix() * m;
  std::cout << "-- Combination 2: --" << std::endl << result << std::endl << std::endl;

  ArrayXXd arr_result = result.array().cast<double>();
  PRINT(arr_result);
}


//
// Add --log_level=message to see the messages!
//
#define BOOST_TEST_MODULE "Functions Tests"

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include "core/types.h"
#include "core/random.h"
#include "core/utils.h"
#include "core/functions.h"

#include "timer.h"
#include "test_utils.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace boost::numeric;
using namespace yann;
using namespace yann::test;

struct FunctionsTestFixture
{
  const size_t min_steps = 5;

  FunctionsTestFixture()
  {

  }
  ~FunctionsTestFixture()
  {

  }

  void softmax_vector(const RefConstVector & input, RefVector output, const Value & beta)
  {
    YANN_CHECK(is_same_size(input, output));
    YANN_CHECK_EQ(input.rows(), 1); // RowMajor layout, breaks for ColMajor

    Value max = input.maxCoeff(); // adjust the computations to avoid overflowing
    Value sum = exp((input.array() - max) * beta).sum();
    output.array() = (exp((input.array() - max) * beta)) / sum;
  }

  void softmax(const RefConstMatrix & input, RefMatrix output, const Value & beta = 3.0)
  {
    YANN_CHECK(is_same_size(input, output));
    for(MatrixSize ii = 0; ii < input.rows(); ++ii) {
      softmax_vector(input.row(ii), output.row(ii), beta);
    }
  }

  pair<Matrix, Value> test_cost_function(
      const unique_ptr<CostFunction> & cost_function,
      const RefConstMatrix & actual0, const RefConstMatrix & expected, const Value & cost0,
      double learning_rate,
      const size_t & epochs,
      bool use_softmax = false)
  {
    YANN_CHECK(is_same_size(actual0, expected));

    // setup
    Matrix delta, actual;
    delta.resizeLike(expected);
    actual = actual0;
    Value cost = 0;

    // ensure we don't do allocations in eigen
    {
      BlockAllocations block;

      // check activation_function
      cost = cost_function->f(actual0, expected);
      BOOST_CHECK_CLOSE(cost, cost0, TEST_TOLERANCE);

      // check activation_derivative
      size_t progress_step = max(epochs / 10, min_steps);
      for (size_t ii = 0; ii < epochs; ++ii) {
        cost_function->derivative(actual, expected, delta);
        actual -= learning_rate * delta;
        if(use_softmax) {
          softmax(actual, actual);
        }
        cost = cost_function->f(actual, expected);

        if (ii % progress_step == 0) {
          BOOST_TEST_MESSAGE("epoch=" << ii << " out of " << epochs << " cost=" << cost);
        }
      }
    }

    return make_pair(actual, cost);
  }

  pair<Vector, Vector> test_activation_function(
      const unique_ptr<ActivationFunction> & activation_function,
      const unique_ptr<CostFunction> & cost_function,
      const Vector & input0, const Vector & output0, const Vector & expected,
      double learning_rate = 1.0, const size_t & epochs = 10,
      int alloc_check_flags = BlockAllocations::None)
  {
    YANN_CHECK_EQ(input0.size(), output0.size());
    YANN_CHECK_EQ(input0.size(), expected.size());

    // setup
    const size_t size = input0.size();
    Vector output(size);
    Vector delta(size);
    Vector input(input0), gradient(size), cost_derivative(size);

    // ensure we don't do allocations in eigen
    {
      BlockAllocations block(alloc_check_flags);

      // check activation_function
      activation_function->f(input0, output);
      BOOST_CHECK(output0.isApprox(output, TEST_TOLERANCE));

      // check activation_derivative
      size_t progress_step = max(epochs / 10, min_steps);
      for (size_t ii = 0; ii < epochs; ++ii) {
        // calculate gradient
        activation_function->derivative(output, delta);
        cost_function->derivative(output, expected, cost_derivative);
        gradient.array() = cost_derivative.array() * delta.array();

        input -= learning_rate * gradient;

        // next iteration
        activation_function->f(input, output);
        if (ii % progress_step == 0) {
          BOOST_TEST_MESSAGE("epoch=" << ii << " out of " << epochs << " cost=" << cost_function->f(output, expected));
        }
      }
    }

    return make_pair(input, output);
  }

};
// struct FunctionsTestFixture

BOOST_FIXTURE_TEST_SUITE(FunctionsTest, FunctionsTestFixture);

////////////////////////////////////////////////////////////////////////////////////////////////
//
// cost functions tests
//
BOOST_AUTO_TEST_CASE(QuadraticCost_Test)
{
  const size_t size = 2;
  Vector actual0(size);
  Vector expected(size);
  Value cost0;
  const double learning_rate = 0.25;
  size_t epochs = 50;

  actual0 << 11.0, 1;
  expected << 1.0, 6.0;
  cost0 = 125;

  pair<Vector, Value> res = test_cost_function(
      make_unique<QuadraticCost>(),
      actual0, expected, cost0,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("expected=" << expected << " actual=" << res.first << " cost=" << res.second);
  BOOST_CHECK(expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK_SMALL(res.second, TEST_TOLERANCE);
}

BOOST_AUTO_TEST_CASE(ExponentialCost_Test)
{
  const size_t size = 2;
  Vector actual0(size);
  Vector expected(size);
  Value cost0;
  const double learning_rate = 0.001;
  size_t epochs = 100;

  actual0 << 11.0, 1;
  expected << 1.0, 6.0;
  cost0 = 349.03430;

  pair<Vector, Value> res = test_cost_function(
      make_unique<ExponentialCost>(100.0),
      actual0, expected, cost0,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("expected=" << expected << " actual=" << res.first << " cost=" << res.second);
  BOOST_CHECK(expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK_CLOSE(100.00, res.second, TEST_TOLERANCE);
}

BOOST_AUTO_TEST_CASE(CrossEntrypyCost_Test)
{
  const size_t size = 2;
  Vector actual0(size);
  Vector expected(size);
  Value cost0;
  const double learning_rate = 0.1;
  size_t epochs = 15;

  // f(actual, expected) = sum(-(expected * ln(actual) + (1 - expected) * ln(1 - actual)))
  actual0 << 0.9, 0.3;
  expected << 0.3, 0.8;
  cost0 = 2.677931;

  pair<Vector, Value> res = test_cost_function(
      make_unique<CrossEntropyCost>(),
      actual0, expected, cost0,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("expected=" << expected << " actual=" << res.first << " cost=" << res.second);
  BOOST_CHECK(expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK_CLOSE(1.111266, res.second, TEST_TOLERANCE);
}

BOOST_AUTO_TEST_CASE(HellingerDistanceCost_Test)
{
  const size_t size = 2;
  Vector actual0(size);
  Vector expected(size);
  Value cost0;
  const double learning_rate = 0.75;
  size_t epochs = 15;

  actual0 << 0.9, 0.1;
  expected << 0.3, 0.7;
  cost0 = 0.43150;
  pair<Vector, Value> res = test_cost_function(
      make_unique<HellingerDistanceCost>(0.0001),
      actual0, expected, cost0,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("expected=" << expected << " actual=" << res.first << " cost=" << res.second);
  BOOST_CHECK(expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK_SMALL(res.second, TEST_TOLERANCE);
}

BOOST_AUTO_TEST_CASE(SquaredHingeLoss_Test)
{
  const size_t size = 4;
  Vector actual0(size);
  Vector actual_expected(size);
  Vector expected(size);
  Value cost0;
  const double learning_rate = 0.5;
  size_t epochs = 10;

  actual0   << 0.6, 0.1, 0.3, 0.0;
  expected  << 0.0, 1.0, 0.0, 0.0;
  actual_expected << 0.049, 0.852, 0.049, 0.049;
  cost0  = 0.81;
  pair<Vector, Value> res = test_cost_function(
      make_unique<SquaredHingeLoss>(),
      actual0, expected, cost0,
      learning_rate, epochs, true); // use softmax

  BOOST_TEST_MESSAGE("expected=" << expected << " actual=" << res.first << " cost=" << res.second);
  BOOST_CHECK(actual_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK_CLOSE(0.02177566296, res.second, TEST_TOLERANCE);
}

////////////////////////////////////////////////////////////////////////////////////////////////
//
// activations functions tests
//
BOOST_AUTO_TEST_CASE(IdentityFunction_Test)
{
  const size_t size = 2;
  Vector input0(size);
  Vector output0(size);
  Vector input_expected(size);
  Vector expected(size);
  const double learning_rate = 0.75;
  size_t epochs = 1000;

  input0   << -5,  10;
  output0  << -5, 10;
  expected << 20, -5;
  input_expected << 20, -5;
  pair<Vector, Vector> res = test_activation_function(
      make_unique<IdentityFunction>(),
      make_unique<QuadraticCost>(),
      input0, output0, expected,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("intput expected=" << input_expected << " actual=" << res.first);
  BOOST_TEST_MESSAGE("output expected=" << expected << " actual=" << res.second);
  BOOST_CHECK(input_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK(expected.isApprox(res.second, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(SigmoidFunction_Test)
{
  const size_t size = 2;
  Vector input0(size);
  Vector output0(size);
  Vector input_expected(size);
  Vector expected(size);
  const double learning_rate = 0.75;
  size_t epochs = 1000;

  input0 << 10, 10;
  output0 << 1.0, 1.0;
  expected << 0.8, 0.1;
  input_expected << 1.3862943611, -2.1972245773; // x = -ln(1/y - 1)
  pair<Vector, Vector> res = test_activation_function(
      make_unique<SigmoidFunction>(),
      make_unique<QuadraticCost>(),
      input0, output0, expected,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("intput expected=" << input_expected << " actual=" << res.first);
  BOOST_TEST_MESSAGE("output expected=" << expected << " actual=" << res.second);
  BOOST_CHECK(input_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK(expected.isApprox(res.second, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(FastSigmoidFunction_Test)
{
  const size_t size = 2;
  Vector input0(size);
  Vector output0(size);
  Vector input_expected(size);
  Vector expected(size);
  const double learning_rate = 0.75;
  size_t epochs = 1000;

  input0 << 10, 10;
  output0 << 1.0, 1.0;
  expected << 0.8, 0.1;
  input_expected << 1.3862943611, -2.1972245773; // x = -ln(1/y - 1)
  pair<Vector, Vector> res = test_activation_function(
      make_unique<FastSigmoidFunction>(10000), // increase internal table size to improve accuracy
      make_unique<QuadraticCost>(),
      input0, output0, expected,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("intput expected=" << input_expected << " actual=" << res.first);
  BOOST_TEST_MESSAGE("output expected=" << expected << " actual=" << res.second);
  BOOST_CHECK(input_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK(expected.isApprox(res.second, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(ReluFunction_Test)
{
  const size_t size = 2;
  Vector input0(size);
  Vector output0(size);
  Vector input_expected(size);
  Vector expected(size);
  const double learning_rate = 0.75;
  size_t epochs = 1000;

  input0   << -5,  10;
  output0  << -0.5, 10;
  expected << 20, -5;
  input_expected << 20, -50;
  pair<Vector, Vector> res = test_activation_function(
      make_unique<ReluFunction>(0.1),
      make_unique<QuadraticCost>(),
      input0, output0, expected,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("intput expected=" << input_expected << " actual=" << res.first);
  BOOST_TEST_MESSAGE("output expected=" << expected << " actual=" << res.second);
  BOOST_CHECK(input_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK(expected.isApprox(res.second, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(TahhFunction_Test)
{
  const size_t size = 2;
  Vector input0(size);
  Vector output0(size);
  Vector input_expected(size);
  Vector expected(size);
  const double learning_rate = 0.75;
  size_t epochs = 1000;

  input0   << 1,  10;
  output0  << 0.99992, 1.71589;
  expected << 0.1, -0.5;
  input_expected << 0.08753, -0.45018;
  pair<Vector, Vector> res = test_activation_function(
      make_unique<TanhFunction>(1.7159, 0.6666),
      make_unique<QuadraticCost>(),
      input0, output0, expected,
      learning_rate, epochs);

  BOOST_TEST_MESSAGE("intput expected=" << input_expected << " actual=" << res.first);
  BOOST_TEST_MESSAGE("output expected=" << expected << " actual=" << res.second);
  BOOST_CHECK(input_expected.isApprox(res.first, TEST_TOLERANCE));
  BOOST_CHECK(expected.isApprox(res.second, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_SUITE_END()


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4d m = Vector4d(1,2,3,4).asDiagonal();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is m.fixed<2, 2>(2, 2):" << endl << m.block<2, 2>(2, 2) << endl;
m.block<2, 2>(2, 0) = m.block<2, 2>(2, 2);
cout << "Now the matrix m is:" << endl << m << endl;

  return 0;
}


#include <deque>
#include <vector>
#include <list>
#include <iostream>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptor/indexed.hpp>

int main()
{
    using namespace boost;

    const char * tasks[] = {
        "pick up kids from school",
        "buy groceries (and snack)",
        "get cash at ATM",
        "drop off kids at soccer practice",
        "cook dinner",
        "pick up kids from soccer",
        "eat dinner"
    };

    const int n_tasks = sizeof(tasks) /sizeof(char*);

    adjacency_list<listS, vecS, directedS> g(n_tasks);
    add_edge(0, 3, g);
    add_edge(1, 3, g);
    add_edge(1, 4, g);
    add_edge(2, 1, g);
    add_edge(3, 5, g);
    add_edge(4, 6, g);
    add_edge(5, 6, g);

    std::deque<int> topo_order;

    topological_sort(g, std::front_inserter(topo_order),
            vertex_index_map(identity_property_map()));

    for(const auto& [index, i] : topo_order
            | boost::adaptors::indexed())
    {
        std::cout << tasks[i.head] << std::endl;
    }

    return 0;
}


#include "normal_modes.h"

#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <list>
#include <array>
#include <armadillo>

#include "id_map.h"
#include "dump_reader.h"

typedef std::vector<std::array<double,3> > coord_block;

void normal_mode_analysis( dump_reader &r,  py_int N, void *eigenvalues,
                           void *eigenvectors )
{
	std::list< coord_block > blocks;
	block_data b;

	while( r.next_block( b ) ){
		id_map im( b.ids, b.N );
		py_int Nparticles = b.N;

		coord_block block( Nparticles );
		for( int i = 0; i < Nparticles; ++i ){
			int id  = i + 1;
			int idx = im[id];

			block[i][0] = b.x[i][0];
			block[i][1] = b.x[i][1];
			block[i][2] = b.x[i][2];
		}

		blocks.push_back( block );
	}
	std::cerr << "Done reading dump file...\n";

	get_normal_modes( blocks, N, eigenvalues, eigenvectors );

}

void read_blocks_from_pipe( const char *pname,
                            std::list< coord_block > &bs )
{
	std::ifstream in(pname);
	std::string line;
	std::ofstream out("pipe_test2");

	/*
	  Expected format:
	  First line: Number of particles
	  N times 3 coordinates, ordered along id.
	*/

	int block_idx = 1;

	// Make sure you exhaust the pipe:
	while( in ){
		std::getline( in, line );
		int Nparticles = 0;
		std::stringstream ss(line);
		ss >> Nparticles;

		if( Nparticles <= 0 ) break;

		coord_block block(Nparticles);

		for( int i = 0; i < Nparticles; ++i ){
			std::getline( in, line );
			ss.str("");
			ss.clear();
			ss << line;
			ss  >> block[i][0] >> block[i][1] >> block[i][2];
		}
		block_idx++;
		bs.push_back( block );
	}
	std::cerr << "Grabbed " << bs.size() << " blocks in total.\n";
}

void average_positions( const std::list< coord_block > &block_data, py_int N,
                        std::vector<double> &X_avg )
{
	int Nparticles = block_data.begin()->size();
	for( int i = 0; i < N; ++i ) X_avg[i] = 0.0;

	std::cerr << "Averaging positions...\n";
	for( const coord_block &b : block_data ){
		for( int i = 0; i < Nparticles; ++i ){
			X_avg[3*i + 0] += b[i][0];
			X_avg[3*i + 1] += b[i][1];
			X_avg[3*i + 2] += b[i][2];
		}
	}
}

void get_normal_modes( const std::list< coord_block > &block_data, py_int N,
                       void *eigenvalues, void *eigenvectors,
                       std::vector<double> *X_avg_ptr )
{
	// Perform the normal mode analysis...
	int Nparticles = block_data.begin()->size();
	double c = 1.0 / static_cast<double>( block_data.size() );
	if( 3*Nparticles != N ){
		std::cerr << "Something fishy is going on...\n";
		std::cerr << "Nparticles = " << Nparticles << ", 3*Nparticles = "
		          << 3*Nparticles << " != N = " << N << "!\n";
		return;
	}

	std::vector<double> X_avg;
	std::cerr << "Averaging positions...\n";
	if( !X_avg_ptr ){
		X_avg.resize(N);
		X_avg_ptr = &X_avg;
		average_positions( block_data, N, *X_avg_ptr );
	}else{
		X_avg = *X_avg_ptr;
		average_positions( block_data, N, *X_avg_ptr );
	}

	// Extract covariance matrix:
	std::cerr << "Constructing covariance matrix...\n";
	arma::mat covar(N,N);

	for( int i = 0; i < N; ++i ){
		for( int j = 0; j < N; ++j ){
			covar(i, j) = 0.0;
		}
	}

	for( const coord_block &b : block_data ){
		for( int i = 0; i < Nparticles; ++i ){
			double dxi = b[i][0] - X_avg[3*i];
			double dyi = b[i][1] - X_avg[3*i+1];
			double dzi = b[i][2] - X_avg[3*i+2];

			for( int j = 0; j < Nparticles; ++j ){
				double dxj = b[j][0] - X_avg[3*j];
				double dyj = b[j][1] - X_avg[3*j+1];
				double dzj = b[j][2] - X_avg[3*j+2];

				covar(3*i  , 3*j  ) += dxi*dxj*c;
				covar(3*i  , 3*j+1) += dxi*dyj*c;
				covar(3*i  , 3*j+2) += dxi*dzj*c;

				covar(3*i+1, 3*j  ) += dyi*dxj*c;
				covar(3*i+1, 3*j+1) += dyi*dyj*c;
				covar(3*i+1, 3*j+2) += dyi*dzj*c;

				covar(3*i+2, 3*j  ) += dzi*dxj*c;
				covar(3*i+2, 3*j+1) += dzi*dyj*c;
				covar(3*i+2, 3*j+2) += dzi*dzj*c;
			}
		}
	}

	// Perform the decomposition:
	std::cerr << "Performing decomposition in normal modes...\n";
	const char *method = "std";
	arma::vec eigval(N);
	arma::mat eigvec(N,N);
	arma::eig_sym( eigval, eigvec, covar, method );

	std::cerr << "Writing " << N << " eigenvalues to " << eigenvalues
	          << " and eigenvectors to " << eigenvectors << "\n";

	double **eigenvecs = static_cast<double**>(eigenvectors);
	double *eigenvals  = static_cast<double*> (eigenvalues);

	for( int i = 0; i < N; ++i ){
		eigenvals[i] = eigval(i);
		for( int j = 0; j < N; ++j ){
			eigenvecs[i][j] = eigvec(i,j);
		}
	}
	std::cerr << "Done!\n";

}

void normal_mode_analysis( const char *pname, void *eigenvalues,
                           void *eigenvectors, py_int N  )
{

	std::list< coord_block > block_data;

	read_blocks_from_pipe( pname, block_data );

	get_normal_modes( block_data, N, eigenvalues, eigenvectors );
}


#include <vector>

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layers/random_affine_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template <typename Dtype>
void RandomAffineLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//do nothing
}

template <typename Dtype>
void RandomAffineLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//set in batch size
	uint32_t batchSize = this->layer_param_.random_affine_param().batch_size();
	vector<int> shape_top(4);
	shape_top[0] = batchSize;
	shape_top[1] = 1;
	shape_top[2] = 2;
	shape_top[3] = 3;
	top[0]->Reshape(shape_top);

	//check valid bottom
	if(bottom.size() == 1){
		CHECK_EQ(top[0]->shape(0), bottom[0]->shape(0)) << "Bottom batch size not the same";
		CHECK_EQ(top[0]->shape(1), bottom[0]->shape(1)) << "Bottom should have 1 channel";
		CHECK_EQ(top[0]->shape(2), bottom[0]->shape(2)) << "Bottom not 2x3 affine";
		CHECK_EQ(top[0]->shape(3), bottom[0]->shape(3)) << "Bottom not 2x3 affine";
	}

	//set working variables
	vector<int> shape_transform(4);
	shape_transform[0] = batchSize;
	shape_transform[1] = 1;
	shape_transform[2] = 3;
	shape_transform[3] = 3;

	t1_.Reshape(shape_transform);
	t2_.Reshape(shape_transform);
	t3_.Reshape(shape_transform);
	bottomExpanded.Reshape(shape_transform);
	t4_.Reshape(shape_transform);

	//transform params
	vector<int> shape_param(4);
	shape_param[0] = batchSize;
	shape_param[1] = 1;
	shape_param[2] = 1;
	shape_param[3] = 1;

	translation_x.Reshape(shape_param);
	translation_y.Reshape(shape_param);
	rotation.Reshape(shape_param);
	scale.Reshape(shape_param);
	flipping_rand.Reshape(shape_param);
}

template <typename Dtype>
void RandomAffineLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//read in transformation distributions
	Dtype max_translation_x = this->layer_param_.random_affine_param().max_translation_x();
	Dtype max_translation_y = this->layer_param_.random_affine_param().max_translation_y();
	Dtype max_rotation = this->layer_param_.random_affine_param().max_rotation();
	Dtype min_scale = this->layer_param_.random_affine_param().min_scale();
	Dtype max_scale = this->layer_param_.random_affine_param().max_scale();
	bool horizontalFlipping = this->layer_param_.random_affine_param().horizontal_flipping();

	//helpers
	Dtype* t1 = t1_.mutable_cpu_data();
	Dtype* t2 = t2_.mutable_cpu_data();
	Dtype* t3 = t3_.mutable_cpu_data();
	Dtype* bottomExpandedData = bottomExpanded.mutable_cpu_data();
	Dtype* t4 = t4_.mutable_cpu_data();
	Dtype* top_data = top[0]->mutable_cpu_data();

	caffe_set(t1_.count(), (Dtype)0, t1);
	caffe_set(t2_.count(), (Dtype)0, t2);
	caffe_set(t3_.count(), (Dtype)0, t3);
	caffe_set(bottomExpanded.count(), (Dtype)0, bottomExpandedData);
	caffe_set(t4_.count(), (Dtype)0, t4);

	int N = top[0]->shape(0);

	//generate transform params
	Dtype* tx = translation_x.mutable_cpu_data();
	Dtype* ty = translation_y.mutable_cpu_data();
	Dtype* rot = rotation.mutable_cpu_data();
	Dtype* scal = scale.mutable_cpu_data();
	Dtype* flip = flipping_rand.mutable_cpu_data();

	caffe_rng_uniform(N,-max_translation_x,max_translation_x,tx);
	caffe_rng_uniform(N,-max_translation_y,max_translation_y,ty);
	caffe_rng_uniform(N,-max_rotation,max_rotation,rot);
	caffe_rng_uniform(N,min_scale,max_scale,scal);
	if(horizontalFlipping){
		caffe_rng_uniform(N,(Dtype)0,(Dtype)1,flip);
	}
	else{
		caffe_set(N, (Dtype)0, flip);
	}

	for(int n=0;n<N;n++){
		//translation
		t1[9*n + 2] = tx[n];
		t1[9*n + 5] = ty[n];
		t1[9*n + 8] = 1;
		//rotation
		Dtype ang = rot[n];
		t1[9*n] = cos(ang); t1[9*n + 1] = -sin(ang);
		t1[9*n + 3] = sin(ang); t1[9*n + 4] = cos(ang);

		//scale
		Dtype scaleFactor = scal[n];
		scaleFactor = 1/scaleFactor;
		if(flip[n] > 0.5){
			t2[9*n] = scaleFactor;
			t2[9*n + 4] = -scaleFactor;
			t2[9*n + 8] = 1;
		}
		else{
			t2[9*n] = scaleFactor;
			t2[9*n + 4] = scaleFactor;
			t2[9*n + 8] = 1;
		}

		caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasNoTrans, 3, 3, 3, (Dtype)1.0,
		      &t1[9*n], &t2[9*n], (Dtype)0.0, &t3[9*n]);

		//transfer to top
		if(bottom.size() == 1){
			caffe_copy(6, &(bottom[0]->mutable_cpu_data()[6*n]), &bottomExpandedData[9*n]);
			bottomExpandedData[9*n + 8] = 1;

			caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasNoTrans, 3, 3, 3, (Dtype)1.0,
		      		&t3[9*n], &bottomExpandedData[9*n], (Dtype)0.0, &t4[9*n]);
			caffe_copy(6, &t4[9*n], &top_data[6*n]);
		}
		else{
			caffe_copy(6, &t3[9*n], &top_data[6*n]);
		}
	}
}

template <typename Dtype>
void RandomAffineLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& top) {
	//do nothing
}

#ifdef CPU_ONLY
STUB_GPU(RandomAffineLayer);
#endif

INSTANTIATE_CLASS(RandomAffineLayer);
REGISTER_LAYER_CLASS(RandomAffine);

}  // namespace caffe