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

#include <stdio.h>
#include <stdlib.h>

#include <cv.h>
#include <highgui.h>

#include "pmdsdk2.h"
#include "cvpmd.h"

int main(int argc, char **argv)
{
    int totalFrames = 0;
    int ui = 1;
    printf("%i\n", argc);
    if(argc > 1) {
        totalFrames = atoi(argv[1]);
        ui = 0;
    }
    if(argc > 2) ui = 1;

    //FIXME: here is lots of old code, should be removed


    PMD *pmd = initPMD("../o3d.L32.pcp", "192.168.0.69");
    
    CvCapture *capture = cvCaptureFromCAM(1); //FIXME: should be passed via argc

    CvSize pmdSz = pmdGetSize(pmd);
    printf("pmd sz: %i x %i\n", pmdSz.width, pmdSz.height);
    IplImage *imgCamColor = cvQueryFrame(capture);
    IplImage *imgPMD = cvCreateImage(pmdGetSize(pmd), 8, 1);
    IplImage *imgPMDA = cvCreateImage(cvGetSize(imgPMD), 8, 1);
    IplImage *imgCam = cvCreateImage(cvGetSize(imgCamColor), 8, 1);
    CvPoint3D32f **pmdPts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*));
    for(int i = 0; i < pmdSz.height; i++) pmdPts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f));

    CvMat *intrinsicCam  = (CvMat*)cvLoad("../intrinsic-cam-6x4.xml");
    CvMat *distortionCam = (CvMat*)cvLoad("../distortion-cam-6x4.xml");
    CvMat *intrinsicPMD  = (CvMat*)cvLoad("../intrinsic-pmd-6x4.xml");
    CvMat *distortionPMD = (CvMat*)cvLoad("../distortion-pmd-6x4.xml");
    if(!intrinsicCam || !distortionCam|| !intrinsicPMD || !distortionPMD) {
        fprintf(stderr, "ERROR: can't load intrinsic and/or distortion xml files!\n");
        return 1;
    }


//    FILE *vPMDA = fopen("./s001.arv", "wb"); // ala dot Amplitudes Raw Video NOTE: i'm not sure it is amplitude :P
//    FILE *vPMDAf = fopen("./s001.farv", "wb"); // same but Floating 
    FILE *vPMDI = fopen("./s001.irv", "wb"); // same but Intensities
    FILE *vPMDIf = fopen("./s001.firv", "wb");
    FILE *headers = fopen("./s001.head", "wb");
    CvVideoWriter *vCam = cvCreateVideoWriter( "./s001.avi"
                                             , CV_FOURCC('D', 'I', 'V', 'X') 
                                             , 25, cvGetSize(imgCam), 1);

    FILE *pmdPtsFile = fopen("./s001.3dp", "w");

    if(ui) {
        cvNamedWindow("Cam", 0);
        cvNamedWindow("PMD", 1);
    }

    printf("DEBUG: init done, going to grab %i frames.\n", totalFrames);

    int frames = 0;
    while(1) {
        frames++;
        if(0 == frames % 100) printf("%i frames grabbed...\n", frames);
        // Image retriving 
        pmdUpdate(pmd->hnd);
        imgCamColor = cvQueryFrame(capture);
       
        pmdQueryImageAsync(pmd, imgPMD);
        fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDIf);

        //pmdQueryAmplitudesAsync(pmd, imgPMDA);
        //fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDAf);


        pmdRetriveDistancesAsync(pmd);
        pmdProjectArrayToCartesian(pmd, intrinsicPMD, pmdPts);

        ImageHeaderInformation *header = retriveHeader();

        
        if(ui) {
            cvShowImage("Cam", imgCamColor);
            cvShowImage("PMD", imgPMD);
        }

        //FIXME: order col/str
        for(int i = 0; i < pmdSz.height; i++) 
            fwrite(pmdPts[i], sizeof(CvPoint3D32f), pmdSz.width, pmdPtsFile);

        fwrite(imgPMD->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDI);
        //fwrite(imgPMDA->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDA);
        fwrite(header, sizeof(ImageHeaderInformation), 1, headers);

        cvWriteFrame(vCam, imgCamColor);

        if(totalFrames && (frames >= totalFrames)) break;
        if(ui) if(27 == cvWaitKey(5)) break;
    }
//    fclose(vPMDA);
//    fclose(vPMDAf);
    fclose(vPMDI);
    fclose(vPMDIf);
    fclose(pmdPtsFile);
    fclose(headers);
    printf("grabbed %i frames.\n", frames);
    printf("See s001* files (you want to rename them).\n");
}