/*
 * pose implementation
 *
 * Copyright (C) Stanislav Serebryakov
 *
 * Released under the GPL version 3.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <assert.h>
#include <math.h>

// OpenCV
#if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__)))
#include <cv.h>
#include <highgui.h>
#elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2)
#include <opencv/cv.h>
#else
#include <opencv2/opencv.hpp>
#endif

// GL: GLFW (window etc, ala glut) and FTGL (text rendering)
#include <GL/glfw.h>
#include <ftgl.h>

// PMD-related stuff
#include "pmdsdk2.h"
#include "cvpmd.h"
#include "pmdWrap.h"
#include "history.h"

// SLAM6D
#include "slam6d/point.h"
#include "slam6d/ptpair.h"
#include <vector>


FTGLPixmapFont *font; // opengl text rendring

void usage(char* progName) {
    printf("usage: %s [options]\n", progName);
    printf("options:\n \
\t--ui 0|1\t disable|enable ui (default: 1)\n\
\t--gl 0|1\t disable|enable 3d rendering (default: 1)\n\
\t--cfg file\t parse configuration from file (default: pmdc.conf)\n\
\t--help\t\t display this help.\n");
}



void render( History *history, CvPoint3D32f *camPts,  int trPtsCnt
           , CvMat *rot,  CvMat *trn
           , CvPoint3D32f **camPts3D, char **pts3DStatus
           , int reprojected, bool poseEstimated);

/*
   reprojects PMD-aquired 3D points on the web cam image
*/

//FIXME this function should be dispached >_>
int projectImageToPMD( CvMat *rot,  CvMat *trn,  CvMat *intrinsic
                     , CvPoint3D32f **pmdPts,  CvSize pmdSz
                     , CvSize camSz, CvPoint **pmd2imgIdxs
                     , CvPoint2D32f *trackedPts 
                     , int *trPtsCnt, CvPoint3D32f *trackedPts3D, char *pts3DStatus
                     , CvPoint *tr2pmdIdxs) {
    /* TODO:
       assert null image, not clean image, bad image depth
       */
    // pmd point's matrix
    CvMat *pt = cvCreateMat(3, 1, CV_32FC1);
    // point in cam coorinate system
    CvMat *reprojPt = cvCreateMat(3, 1, CV_32FC1);
    // points on cam screen
    CvMat *camPt3 = cvCreateMat(3, 1, CV_32FC1); 
    CvMat *camPt  = cvCreateMat(2, 1, CV_32FC1); 
    // rotation matrix
    CvMat *rotMat = cvCreateMat(3, 3, CV_32FC1);
    cvRodrigues2(rot, rotMat, NULL);

    // clear previous found tracked pts
    for(int k = 0; k < *trPtsCnt; k++) pts3DStatus[k] = 0;
    
    //int l = 0; // count corresponding points
    int reprojected = 0;

    for(int j = 0; j < pmdSz.width; j++)
      for(int i = 0; i < pmdSz.height; i++) {
          if( fabs(pmdPts[i][j].x) > 20.0
           || fabs(pmdPts[i][j].y) > 20.0 
           || fabs(pmdPts[i][j].z) > 20.0) continue; //FIXME: more realistic limit, status = false
          // convert to mat 
          CV_MAT_ELEM(*pt, float, 0, 0) = (float)pmdPts[i][j].x;
          CV_MAT_ELEM(*pt, float, 1, 0) = (float)pmdPts[i][j].y;
          CV_MAT_ELEM(*pt, float, 2, 0) = (float)pmdPts[i][j].z;
          
          // reproject to the camera coordinate system
          cvMatMulAdd(rotMat, pt, trn, reprojPt);
          // project to the image
          cvMatMul(intrinsic, reprojPt, camPt3);
//          cvConvertPointsHomogenious(camPt3, camPt);
/*          cvProjectPoints2(pt, rot, trn, intrinsic, NULL, camPt
                          , NULL, NULL, NULL, NULL, NULL);*/
          float scale = (float)CV_MAT_ELEM(*camPt3, float, 2, 0);
          float xf = CV_MAT_ELEM(*camPt3, float, 0, 0) / scale;
          float yf = CV_MAT_ELEM(*camPt3, float, 1, 0) / scale;
          int x = (int)xf;
          int y = camSz.height - (int)yf; //FIXME revise coordinates

          if((x < 0) || (x > camSz.width) ||
             (y < 0) || (y > camSz.height)) {
              pmd2imgIdxs[i][j].x = -1;
              pmd2imgIdxs[i][j].y = -1;
              continue;
          } else { // point is projected to cam
            // contains which PMD point is seen from this pixel
            pmd2imgIdxs[i][j].x = x;
            pmd2imgIdxs[i][j].y = y;
          }

          //find tracked 3d points (fused here, can be dispached)
          for(int k = 0; k < *trPtsCnt; k++) 
              if(pts3DStatus[k]) continue; // kill to accumulate (*)
              else 
                if( (abs((trackedPts[k].x - (float)x)) < 3)   //TODO: distance SHOULD be chousen depending on depth!!!
                 && (abs((trackedPts[k].y - (float)y)) < 3)) { //FIXME: hardcoded distance
                    //TODO: we can accumulate points here to get more presise results (*)
                  trackedPts3D[k].x = pmdPts[i][j].x;
                  trackedPts3D[k].y = pmdPts[i][j].y;
                  trackedPts3D[k].z = pmdPts[i][j].z;
                  pts3DStatus[k] = 1;
                  tr2pmdIdxs[k].x = i; //FIXME: cvPoint(i,j)?
                  tr2pmdIdxs[k].y = j;
                  reprojected++;
                  break; // kill to accumulate (*)
              }
      }
 
    cvReleaseMat(&pt);
    cvReleaseMat(&reprojPt);
    cvReleaseMat(&camPt);
    cvReleaseMat(&rotMat);

    return reprojected;
}

static inline float cvPoint3D32fNorm(CvPoint3D32f pt) {
    return sqrt( (pt.x - pt.x)*(pt.x - pt.x)
                +(pt.y - pt.y)*(pt.y - pt.y)
                +(pt.z - pt.z)*(pt.z - pt.z));
}

float dpthS( IplImage *img
           , CvPoint ptI, CvPoint dp
           , CvPoint3D32f **pts 
           , CvPoint ptD, CvSize pmdSz
           , float sD, float sC) {
    CvSize imgSz = cvGetSize(img);
    if( ptD.x + dp.x > pmdSz.width - 1 || ptD.x + dp.x < 0
     || ptD.y + dp.y > pmdSz.height - 1 || ptD.y + dp.y < 0
     || ptI.x + dp.x > imgSz.width - 1 || ptI.x + dp.x < 0
     || ptI.y + dp.y > imgSz.height - 1 || ptI.y + dp.y < 0) return 0.0;

    uchar *pI = &((uchar*) (img->imageData + img->widthStep * ptI.y))[ptI.x*3];
    uchar *pJ = &((uchar*) (img->imageData + img->widthStep * (ptI.y+dp.y)))[(ptI.x+dp.x)*3];
    float dr = (float)pI[2] - (float)pJ[2];
    float dg = (float)pI[1] - (float)pJ[1];
    float db = (float)pI[0] - (float)pJ[0];
    float wij = exp(-(sqrt(dr*dr + dg*dg + db*db))/sC);
    float dpI = cvPoint3D32fNorm(pts[ptD.x][ptD.y]);
    float dpJ = cvPoint3D32fNorm(pts[ptD.x + dp.x][ptD.y + dp.y]); 
    float s = exp(-wij*(dpI - dpJ)*(dpI - dpJ)/sD);
    return s;
}



#define DPTHS(dx, dy) (dpthS( img, idx, cvPoint((dx),(dy)) \
                            , pmdPts \
                            , pmdIdx, pmdSz \
                            , sigmaDepth, sigmaColor))


void outliersDepthAndColor( CvPoint3D32f **pmdPts, IplImage *img, CvSize pmdSz
                          , CvPoint2D32f *trackedPts, int trPtsCnt, CvPoint* tr2pmdIdxs
                          , char *pts3DStatus // this actually return parameter
                          , float sigmaDepth, float threshold, float sigmaColor
                          ) {
   if(threshold < 0.0) return;   
   // depth score outliers removal, see pmdc.conf comments
   for(int k = 0; k < trPtsCnt; k++) {
       //FIXME: check array bounds
       CvPoint pmdIdx = tr2pmdIdxs[k];
       CvPoint idx = cvPointFrom32f(trackedPts[k]);
       float s00, s01, s02;
       s00 = DPTHS(-1, -1); 
       s01 = DPTHS( 0, -1);
       s02 = DPTHS( 1, -1);
       float s10 = DPTHS(-1,  0); 
       float s12 = DPTHS( 1,  0);
       float s20 = DPTHS(-1,  1); 
       float s21 = DPTHS( 0,  1);
       float s22 = DPTHS( 1,  1);
       float score = s00 + s01 + s02 
                   + s10 + 0.0 + s12
                   + s20 + s21 + s22;
       printf("score = %f\n", score);
       if(score < threshold) pts3DStatus[k] = 0;
   }                  

}

int motionMeanAndVariance(CvPoint3D32f **camPts3D, char **pts3DStatus, int trPtsCnt, float *mean, float *var) {    

//    float *mean = meanAndVariance;
//    float *var = &(meanAndVariance[3]);
//    float motion[3];
    float dx, dy, dz;
    float magSq;
    float motionSum = 0; //[3] = {0.0, 0.0, 0.0};
    float motionSqrSum = 0; //[3] = {0.0, 0.0, 0.0};

    int sz = 0; // pairs count    
    for(int i = 0; i < trPtsCnt; i++) 
        if(pts3DStatus[1][i] && pts3DStatus[0][i]) { 
            sz++;

            dx = camPts3D[1][i].x - camPts3D[0][i].x;
            dy = camPts3D[1][i].y - camPts3D[0][i].y;
            dz = camPts3D[1][i].z - camPts3D[0][i].z;

            magSq = dx*dx + dy*dy + dz*dz; 

            motionSum += sqrt(magSq); //FIXME: optimisation, we can use it without sqrt

            motionSqrSum += magSq; //thus, it would be sqr here

/*          motionSum[0] += motion[0];
            motionSum[1] += motion[1];
            motionSum[2] += motion[2];

            motionSqrSum[0] += motion[0]*motion[0];
            motionSqrSum[1] += motion[1]*motion[1];
            motionSqrSum[2] += motion[2]*motion[2];*/
    }

    if(0 == sz) return 0;
    // mean
    *mean = motionSum / sz;
/*  mean[0] = motionSum[0] / (float)sz;
    mean[1] = motionSum[1] / (float)sz;
    mean[2] = motionSum[2] / (float)sz;*/
        
    // variance

    for(int i = 0; i < trPtsCnt; i++) 
        if(pts3DStatus[1][i] && pts3DStatus[0][i]) {                   
            dx = camPts3D[1][i].x - camPts3D[0][i].x;
            dy = camPts3D[1][i].y - camPts3D[0][i].y;
            dz = camPts3D[1][i].z - camPts3D[0][i].z;

            magSq = dx*dx + dy*dy + dz*dz; 

            *var += (magSq - *mean)*(magSq - *mean);
        }

    *var /= sz;
        // = motionSqrSum / sz - (*mean)*(*mean);

/*  var[0] = motionSqrSum[0] / (float)sz - mean[0]*mean[0];
    var[1] = motionSqrSum[1] / (float)sz - mean[1]*mean[1];
    var[2] = motionSqrSum[2] / (float)sz - mean[2]*mean[2];*/
    return 1;
}

void outliersSpeedSigma(CvPoint3D32f **camPts3D, char **pts3DStatus, int trPtsCnt,  float mean, float var) {

        float dx, dy, dz;
        float mag;
        float sigma = sqrt(var);
        for(int i = 0; i < trPtsCnt; i++) 
           if(pts3DStatus[1][i] && pts3DStatus[0][i]) { 
               dx = camPts3D[1][i].x - camPts3D[0][i].x;
               dy = camPts3D[1][i].y - camPts3D[0][i].y;
               dz = camPts3D[1][i].z - camPts3D[0][i].z;

               mag = sqrt(dx*dx + dy*dy + dz*dz); 
                                                                                              
               if(fabs(mag - mean) > sigma) {
                 pts3DStatus[0][i] = 0;
               }
           }
}


int main(int argc, char **argv) {
    // settings
    bool ui = true;
    bool gl = true;
    bool fps = false;
    const char *config = "./pmdc.conf";

    // args parsing
    for(int i = 1; i < argc; i++) {
        if      (!strcmp(argv[i], "--ui"))           ui = (bool) atoi(argv[++i]);
        else if (!strcmp(argv[i], "--gl"))           gl = (bool) atoi(argv[++i]);
        else if (!strcmp(argv[i], "--fps"))          fps = true;
        else if (!strcmp(argv[i], "--cfg"))          config = argv[++i];
        //TODO: config
        else if (!strcmp(argv[i], "--help") 
             || !strcmp(argv[i], "-h")) {
                usage(argv[0]);
                return 1;
        } else {
            fprintf(stderr, "ERROR: unknown flag: %s\n", argv[i]);
            return 1;
        }
    }
    // pose guess
    CvMat *rotMatGuess = cvCreateMat(3,3, CV_32FC1);
    CvMat *rotGuess = cvCreateMat(3, 1, CV_32FC1); 
    CvMat *trnGuess = cvCreateMat(3, 1, CV_32FC1); 


    /***** init device and allocate images *****/
    PMDCam *pmdc = initPMDCam(config);
    CvSize pmdSz = cvGetSize(pmdc->iPMDI);
    CvSize camSz = cvGetSize(pmdc->iCam);
    printf("pose: pmd init done\n");

    /***** essential matrix *****/    

    CvMat *rot = (CvMat*)cvLoad("../essential-rot.xml"); //FIXME: load path from cfg
    CvMat *trn = (CvMat*)cvLoad("../essential-trn.xml");

   /***** LK-tracking *****/
    IplImage *swapTemp;

    int featuresMax = pmdc->_track.maxFeatures;
    int trPtsCnt = 0; // counts found points

    // eigenvalues for GFTT
    IplImage* eig = cvCreateImage(camSz, 32, 1); 
    IplImage* tmp = cvCreateImage(camSz, 32, 1);
    IplImage* mask = cvCreateImage(camSz, IPL_DEPTH_8U, 1);

    // previous image and pyramides
    IplImage *imgCamPrv     = cvCreateImage(camSz, 8, 1);
    IplImage *imgCamPyr     = cvCreateImage(camSz, 8, 1);
    IplImage *imgCamPyrPrv  = cvCreateImage(camSz, 8, 1);

    // prev and curr tracked points
    CvPoint2D32f *camPts[2];
    camPts[0] = (CvPoint2D32f*) cvAlloc(featuresMax * sizeof(CvPoint2D32f));
    camPts[1] = (CvPoint2D32f*) cvAlloc(featuresMax * sizeof(CvPoint2D32f));
    CvPoint3D32f *camPts3D[2];
    camPts3D[0] = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f));
    camPts3D[1] = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f));

    CvPoint2D32f *swapPts;
    CvPoint3D32f *swapPts3; // i guess i can use void* d:D
    char *swapStatus;
    char *camPtsStatus = (char*)cvAlloc(featuresMax);
    char *pts3DStatus[2];
    pts3DStatus[0]  = (char*) cvAlloc(featuresMax *  sizeof(char));
    pts3DStatus[1]  = (char*) cvAlloc(featuresMax * sizeof(char));
    CvPoint *tr2pmdIdxs = (CvPoint*) cvAlloc(featuresMax * sizeof(CvPoint));

    // 3d rays where points points :P
    CvPoint3D32f *trackedPts = (CvPoint3D32f*) cvAlloc(featuresMax * sizeof(CvPoint3D32f));

    // contains (row,col) of pmd 3D pts 
    CvPoint **pmd2imgIdxs = (CvPoint**) cvAlloc(pmdSz.height * sizeof(CvPoint*));
    for(int i = 0; i < pmdSz.height; i++) 
        pmd2imgIdxs[i] = (CvPoint*) cvAlloc(pmdSz.width * sizeof(CvPoint));

    // pmd history
    History *history = createHistory();

    /***** ui and gl stuff *****/
    if(ui) {
        cvNamedWindow("PMD", 0);
        cvNamedWindow("Cam", 0);
    }
   
    if(gl) {
        glfwInit();
        if(!glfwOpenWindow(640, 480, 8, 8, 8, 8, 24, 0, GLFW_WINDOW)) {
            glfwTerminate();
            fprintf(stderr, "ERROR: can't init glfw window!\n");
            return 1;
        } 
    }
    //FIXME: put this in if(gl)
    font = new FTGLPixmapFont("./djvm.ttf");

    if(font->Error()) {
            fprintf(stderr, "ERROR: can't load font ./djvm.ttf");
            return 1;
    }
    font->FaceSize(20);

    // fps counting
    time_t tic=time(0);
    time_t tac;

    int fpsCnt = 0;    

    //icp pairs
    vector<PtPair> pairs;
    vector<PtPair> motion;
    float mean, var;

    /***** main loop *****/
    /*********************/
    int frames = 0;
    int goodFrames = 0;
    while(1) {
        /***** fps counting *****/
        if(fps) {
            tac = time(0);
            if(tac - tic >= 1) {
                printf("%i FPS\n", fpsCnt);
                fflush(stdout);
                fpsCnt = 0;
                tic=tac;
            }
            fpsCnt++;
        }
        if(grabData(pmdc)) break; // end of seq?
    

        /***** tracking *****/
        //if(trPtsCnt)
        cvCalcOpticalFlowPyrLK( imgCamPrv, pmdc->iCam, imgCamPyrPrv, imgCamPyr
                              , camPts[0], camPts[1], trPtsCnt
                              , cvSize(pmdc->_track.winSz, pmdc->_track.winSz), 3, camPtsStatus, 0
                              , cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03)
                              , pmdc->_track.trackingFlags);
        pmdc->_track.trackingFlags |= CV_LKFLOW_PYR_A_READY;

        // filter out not tracked points        
        int k = 0; 
        for(int i = 0; i < trPtsCnt; i++) 
            if(camPtsStatus[i]) {
                camPts3D[0][k] = camPts3D[0][i];
                pts3DStatus[0][k] = pts3DStatus[0][i];
                camPts[1][k++] = camPts[1][i];
            }

        trPtsCnt = k;            
        
        /***** 3d (re)projection *****/
        cvProjectArrayToCartesian(pmdc->intrinsicCam, camPts[1], trPtsCnt, trackedPts);

        int reprojected = projectImageToPMD( rot, trn, pmdc->intrinsicCam, pmdc->pts, pmdSz, camSz, pmd2imgIdxs
                                           , camPts[1], &trPtsCnt, camPts3D[1], pts3DStatus[1], tr2pmdIdxs);
        double alignError = 0.0;
        bool poseEstimated = false;
        if(reprojected >= pmdc->minPts4Pose) { // we need at least minPts4Pose points

            if(motionMeanAndVariance(camPts3D, pts3DStatus, trPtsCnt, &mean, &var))
                    // TODO: can be fused with centroid computation
                    outliersSpeedSigma(camPts3D, pts3DStatus, trPtsCnt, mean, var);           

            outliersDepthAndColor( pmdc->pts, pmdc->iCamColor, pmdSz, camPts[1], trPtsCnt
                                 , tr2pmdIdxs, pts3DStatus[1], pmdc->sigmaDepth
                                 , pmdc->dpThreshold, pmdc->sigmaColor);


            pairs.clear();
            double centroidM[3] = {0.0, 0.0, 0.0};
            double centroidD[3] = {0.0, 0.0, 0.0};
            double pt1[3];
            double pt2[3];

            for(int i = 0; i < trPtsCnt; i++) 
                if(pts3DStatus[1][i] && pts3DStatus[0][i]) {                   
                    pt1[0] = camPts3D[0][i].x;
                    pt1[1] = camPts3D[0][i].y;
                    pt1[2] = camPts3D[0][i].z;
                    pt2[0] = camPts3D[1][i].x;
                    pt2[1] = camPts3D[1][i].y;
                    pt2[2] = camPts3D[1][i].z;

                    //TODO can be fused -- (+=) :: a -> a -> a
                    centroidM[0] += camPts3D[0][i].x;
                    centroidM[1] += camPts3D[0][i].y;
                    centroidM[2] += camPts3D[0][i].z;
                    centroidD[0] += camPts3D[1][i].x;
                    centroidD[1] += camPts3D[1][i].y;
                    centroidD[2] += camPts3D[1][i].z;

                    PtPair currentPair(pt1, pt2);
                    pairs.push_back(currentPair);
                }

            reprojected = pairs.size();

            if(reprojected >= pmdc->minPts4Pose) { // enough corresponding points
            centroidM[0] /= reprojected; 
            centroidM[1] /= reprojected; 
            centroidM[2] /= reprojected; 
            centroidD[0] /= reprojected; 
            centroidD[1] /= reprojected; 
            centroidD[2] /= reprojected; 

            double transformMat[16];
            try { alignError = pmdc->icp->Point_Point_Align(pairs, transformMat, centroidM, centroidD); }
            catch(...) { fprintf(stderr, "ERROR: matrix is singular!\n"); }

            if(!gl) printf( "%i: align error: %f, 3d pts count: %i, 2d pts count: %i\n"
                          , frames, alignError, reprojected, trPtsCnt);          

            for(int i = 1; i < 16; i++) 
                if (i%4 > 2) continue; // bottom row
                else if(i/4 > 2) CV_MAT_ELEM(*trnGuess, float, i%4, 0) = transformMat[i];
                     else CV_MAT_ELEM(*rotMatGuess, float, i/4, i%4) = transformMat[i]; // right col

            cvRodrigues2(rotMatGuess, rotGuess, NULL);

            if(alignError < pmdc->maxError) 
                    poseEstimated = true;
            }
        } 


        /**** Print pose to file ****/
        //TODO: config option
        if(pmdc->savePoses) { 
            char filename[] = "./dat/scan0000.pose";       
            sprintf(filename, "./dat/scan%04d.pose", frames);
            FILE *pose = fopen(filename, "wb");
	        if(!pose) fprintf(stderr, "cant create file %s!\n", filename);

            if(poseEstimated) {
                fprintf(pose, "%f %f %f\n%f %f %f\n"
                       , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0)
                       , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0)
                       , 100.0*CV_MAT_ELEM(*rotGuess, float, 0, 0)
                       , 100.0*CV_MAT_ELEM(*trnGuess, float, 0, 0)
                       , 100.0*CV_MAT_ELEM(*trnGuess, float, 1, 0)
                       , 100.0*CV_MAT_ELEM(*trnGuess, float, 2, 0));
                goodFrames++;            
            } else {
                fprintf( stderr, "ERROR: %i points found, align error: %f\n"
                       , reprojected, alignError);
                fprintf(pose, "%f %f %f\n%f %f %f\n", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
            }       

            fflush(pose);
            fclose(pose);
        }
        frames++;


        /***** find features *****/
        if(trPtsCnt < pmdc->_track.minFeatures) {
                int trPtsCntFound = featuresMax - trPtsCnt;
                cvSet(mask, cvScalarAll(255));
                for(int i = 0; i < trPtsCnt; i++) 
                    cvCircle(mask, cvPointFrom32f(camPts[1][i]), 20, CV_RGB(0,0,0), -1, 8, 0);
                cvGoodFeaturesToTrack( pmdc->iCam, eig, tmp, camPts[1] + trPtsCnt, &trPtsCntFound
                                     , pmdc->_track.quality, pmdc->_track.minDist, mask, 3, 0, 0.04);
                cvFindCornerSubPix( pmdc->iCam, camPts[1] + trPtsCnt, trPtsCntFound
                                  , cvSize(pmdc->_track.winSz,pmdc->_track.winSz), cvSize(-1,-1)
                                  , cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
                trPtsCnt += trPtsCntFound;
        }

        Frame *f = allocFrame3DData(pmdSz);
        fillFrame(f, pmdc->iCamColor, pmdSz, pmdc->pts, pmd2imgIdxs, rotGuess, trnGuess, alignError);
        history = addFrame(history, f);
        checkHistoryLen(history, pmdc->historyLen);

        bool pause = false;
        do {
             /***** ui and rendring *****/
            if(gl) render( history, trackedPts, trPtsCnt
                         , rot, trn, camPts3D, pts3DStatus
                         , reprojected, poseEstimated);
            if(ui) {
                for(int i = 0; i < trPtsCnt; i++)
                    cvCircle( pmdc->iCamColor, cvPointFrom32f(camPts[1][i])
                            , 3, CV_RGB(255,0,255), -1, 8, 0);

                cvShowImage("PMD", pmdc->iPMDI);
                cvShowImage("Cam", pmdc->iCamColor);
            }

            int key = cvWaitKey(5);
            if(27 == key) return 0; // ESC pressed FIXME: release stuff
            if((int)' ' == key) pause = !pause;
            if(gl) if(glfwGetKey(GLFW_KEY_ESC) == GLFW_PRESS) return 0; //in OpenGL window ;)
        } while(pause);

        CV_SWAP(imgCamPrv, pmdc->iCam, swapTemp);
        CV_SWAP(imgCamPyrPrv, imgCamPyr, swapTemp);
        CV_SWAP(camPts[0], camPts[1], swapPts);
        CV_SWAP(camPts3D[0], camPts3D[1], swapPts3);
        CV_SWAP(pts3DStatus[0], pts3DStatus[1], swapStatus);


    } // while(1)

    // let OS clean all images and matrices
    // TODO: releasePMD(&pmd);
    if(gl) {
        glfwTerminate();
    }
    printf("%i good frames, %i frames total.\n", goodFrames, frames);
    return 0;
}


// Global rendering settings
float rotx = 0;
float roty = 0;
float rotz = 0;
float scale = 0.7;
int renderCoords = 1;
int renderCams = 1;
int renderColorPts = 1;
int renderLines = 0;
int renderTracked = 1;
int renderHistory = 1;
int centerCloud = 0;



void renderFrame(Frame *f) {
    assert(f->img->imageData);
    glBegin(GL_POINTS);
    if(!renderColorPts) {
    //FIXME: mess with the indices (i,j)
        for(int j = 0; j < f->sz.width; j++) 
         for(int i = 0; i < f->sz.height; i++)
          glVertex3f(f->pts[i][j].x, f->pts[i][j].y, f->pts[i][j].z);
    glEnd();
    } else {
    uchar *imgCamPix = 0;
    for(int j = 0; j < f->sz.width; j++)
        for(int i = 0; i < f->sz.height; i++) {
            int x = f->status[i][j].x;
            int y = f->status[i][j].y;
            if(x > 0) {
                imgCamPix = &((uchar*)
                    (f->img->imageData + f->img->widthStep * y))[x*3];
                glColor3f( ((float)imgCamPix[2])/255.0
                         , ((float)imgCamPix[1])/255.0
                         , ((float)imgCamPix[0])/255.0); //BGR
            } else glColor3f(1.0, 0.0, 0.0);
            glVertex3f(f->pts[i][j].x, f->pts[i][j].y, f->pts[i][j].z);
        }
    } // if renderColorPts else
}

void render( History *history, CvPoint3D32f *camPts,  int trPtsCnt
           , CvMat *rot,  CvMat *trn
           , CvPoint3D32f **camPts3D, char **pts3DStatus
           , int reprojected, bool poseEstimated) {

    if(glfwGetKey((int)'W') == GLFW_PRESS) roty += 10.0;
    if(glfwGetKey((int)'S') == GLFW_PRESS) roty -= 10.0;
    if(glfwGetKey((int)'A') == GLFW_PRESS) rotx -= 10.0;
    if(glfwGetKey((int)'D') == GLFW_PRESS) rotx += 10.0;
    if(glfwGetKey((int)'Q') == GLFW_PRESS) rotz -= 10.0;
    if(glfwGetKey((int)'E') == GLFW_PRESS) rotz += 10.0;
    if(glfwGetKey((int)'R') == GLFW_PRESS) scale -= 0.1;
    if(glfwGetKey((int)'F') == GLFW_PRESS) scale += 0.1;
    if(glfwGetKey((int)'1') == GLFW_PRESS) renderCoords = !renderCoords;
    if(glfwGetKey((int)'2') == GLFW_PRESS) renderCams = !renderCams;
    if(glfwGetKey((int)'3') == GLFW_PRESS) renderColorPts = !renderColorPts;
    if(glfwGetKey((int)'4') == GLFW_PRESS) renderLines = !renderLines;
    if(glfwGetKey((int)'5') == GLFW_PRESS) renderTracked = !renderTracked;
    if(glfwGetKey((int)'6') == GLFW_PRESS) renderHistory = !renderHistory;
    if(glfwGetKey((int)'C') == GLFW_PRESS) centerCloud = !centerCloud;

    int width, height;
    GLUquadric *quadric;

    glfwGetWindowSize(&width, &height);
    height = height < 1 ? 1 : height;

    glViewport(0, 0, width, height);

    glEnable(GL_BLEND); 
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(70, (double)width/(double)height, 0.01, 100.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    int cj = history->frame->sz.width / 2;
    int ci = history->frame->sz.height / 2;
    CvPoint3D32f **pts = history->frame->pts;
    //   gluLookAt(scale, scale, scale, 
       //     pts[ci][cj].x, pts[ci][cj].y, -pts[ci][cj].z, 0.0, 1.0, 0.0);
    gluLookAt(scale, scale, scale, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

    glScalef(1.0, 1.0, -1.0); // convert opengl coord system to left handed

    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LEQUAL);

    /* text rendering */
    char info[30];
    FTPoint pt(5.0, 5.0);
    sprintf(info, "2D pts: %i", trPtsCnt);
    font->Render(info, -1, pt);

    pt.Y(35.0);
    sprintf(info, "3D pts: %i", reprojected);
    font->Render(info, -1, pt);

    pt.Y(65.0);
    sprintf(info, "Align error: %f", history->frame->alignError);
    font->Render(info, -1, pt);

    glRotatef(rotz, 0.0, 0.0, 1.0);
    glRotatef(roty, 0.0, 1.0, 0.0);
    glRotatef(rotx, 1.0, 0.0, 0.0);

    if(centerCloud) glTranslatef(-pts[ci][cj].x, -pts[ci][cj].y, -pts[ci][cj].z);


    /***** xyz axes *****/
    if(renderCoords) {
    glBegin(GL_LINES);
        glColor3f(1.0, 0.0, 0.0);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(1.0, 0.0, 0.0);
        glColor3f(0.0, 1.0, 0.0);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(0.0, 1.0, 0.0);
        glColor3f(0.0, 0.0, 1.0);
        glVertex3f(0.0, 0.0, 0.0);
        glVertex3f(0.0, 0.0, 1.0);
    glEnd();
    }

    /***** render PMD-points PMD-cam in the O *****/
    fflush(stdout);
    renderFrame(history->frame);
    fflush(stdout);

    if(renderHistory) {
        fflush(stdout);
        History *histI = history;
        glPushMatrix();

        //for(int i = 0; histI->prev; i++) {
        while(histI->prev) {
                glColor3f(1.0, 0.0, 0.0);//, (100.0-i)/100);
                assert(histI);

                //if(histI->frame->alignError < 0.03) { //FIXME_ hardcoded thrs 
                    glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 0, 0), 1.0f, 0.0f, 0.0f);
                    glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 1, 0), 0.0f, 1.0f, 0.0f);
                    glRotatef(-CV_MAT_ELEM(*histI->frame->rot, float, 2, 0), 0.0f, 0.0f, 1.0f);
                    glTranslatef( -CV_MAT_ELEM(*histI->frame->trn, float, 0, 0)
                                , -CV_MAT_ELEM(*histI->frame->trn, float, 1, 0)
                                , -CV_MAT_ELEM(*histI->frame->trn, float, 2, 0));
                //}
                histI = histI->prev;
                renderFrame(histI->frame);
            }
        glPopMatrix();
    }
   
    quadric = gluNewQuadric();


    if(renderCams) {
        glColor3f(0.5, 0.5, 0.0);
        glPushMatrix();
            glScalef(1.0, 1.0, -1.0); // rotated cylinder
            gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10);
        glPopMatrix();
    }

    /***** render Cam and features according to essential matrix [R|t] *****/
    if(renderTracked) {
     glBegin(GL_LINES);
     for(int i = 0; i < trPtsCnt; i++) {
       if(pts3DStatus[1][i] &&
          pts3DStatus[0][i]) {
               glColor3f(1.0, 0.0, 0.0);
               glVertex3f(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z);
               glVertex3f(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z);
               glColor3f(0.0, 0.0, 0.5);
               glPushMatrix();
                glTranslatef(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z);
                gluSphere(quadric, 0.005f, 10, 10);
               glPopMatrix();
               glColor3f(0.5, 0.0, 1.0);
               glPushMatrix();
                glTranslatef(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z);
                gluSphere(quadric, 0.01f, 10, 10);
               glPopMatrix();
       }
     }
     glEnd();
    }
    glBegin(GL_LINES);
     for(int i = 0; i < trPtsCnt; i++) 
       if(pts3DStatus[1][i] &&
          pts3DStatus[0][i]) {
               glColor3f(1.0, 0.0, 0.0);
               glVertex3f(camPts3D[0][i].x, camPts3D[0][i].y, camPts3D[0][i].z);
               glVertex3f(camPts3D[1][i].x, camPts3D[1][i].y, camPts3D[1][i].z);
       }
    glEnd();


    glPushMatrix();
        glTranslatef( -CV_MAT_ELEM(*trn, float, 0, 0)
                    , -CV_MAT_ELEM(*trn, float, 1, 0)
                    , -CV_MAT_ELEM(*trn, float, 2, 0));
        glRotatef(-CV_MAT_ELEM(*rot, float, 0, 0), 1.0f, 0.0f, 0.0f);
        glRotatef(-CV_MAT_ELEM(*rot, float, 1, 0), 0.0f, 1.0f, 0.0f);
        glRotatef(-CV_MAT_ELEM(*rot, float, 2, 0), 0.0f, 0.0f, 1.0f);
        if(renderCams) {
            glColor3f(0.0, 0.5, 0.5);
            glPushMatrix();
            glScalef(1.0, 1.0, -1.0);
            gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10);
            glPopMatrix();
        }

        if(renderLines) {
            glColor3f(1.0, 0.0, 1.0);
            glBegin(GL_LINES);
             for(int i = 0; i < trPtsCnt; i++) {
              glVertex3f(0.0f, 0.0f, 0.0f);
              glVertex3f(-camPts[i].x, camPts[i].y, camPts[i].z); //FIXME: revise coordinates! why -camPts?! 
             }
            glEnd();
        }
    glPopMatrix();

    glPushMatrix();
      glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 2, 0), 0.0f, 0.0f, 1.0f);
      glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 1, 0), 0.0f, 1.0f, 0.0f);
      glRotatef(CV_MAT_ELEM(*history->frame->rot, float, 0, 0), 1.0f, 0.0f, 0.0f);
      glTranslatef( CV_MAT_ELEM(*history->frame->trn, float, 0, 0)
                  , CV_MAT_ELEM(*history->frame->trn, float, 1, 0)
                  , CV_MAT_ELEM(*history->frame->trn, float, 2, 0));
      if(poseEstimated) glColor3f(0.0, 0.0, 1.0);
      else glColor3f(0.0, 0.0, 0.3);
      //render pose and pmd cam
      glScalef(1.0, 1.0, -1.0);
      gluCylinder(quadric, 0.05, 0.0, 0.05, 10, 10);
      glColor4f(1.0, 0.0, 0.0, 0.5);
      gluSphere(quadric, history->frame->alignError, 20, 20);
    glPopMatrix();

    gluDeleteQuadric(quadric);
    glfwSwapBuffers();
}