// VideoDepthParser.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "VideoDepthParser.h"
#include "StructureMotion.h"


//Link CxImage Lib
#include "../lib/cximagelink.h"


#ifdef _DEBUG
#define new DEBUG_NEW
#endif


// The one and only application object

CWinApp theApp;

using namespace std;

int _tmain(int argc, TCHAR* argv[], TCHAR* envp[])
{
	int nRetCode = 0;

	// initialize MFC and print and error on failure
	if (!AfxWinInit(::GetModuleHandle(NULL), NULL, ::GetCommandLine(), 0))
	{
		// TODO: change error code to suit your needs
		_tprintf(_T("Fatal Error: MFC initialization failed\n"));
		nRetCode = 1;
	}
	else
	{
		// TODO: code your application's behavior here.
		CStructureMotion sfm;

		//Load Camera Parameters, image sequence, and depth sequence
		sfm.Load("../data/cameras.txt","../data/test0000.jpg","../data/_depth0.raw");
		
		CVideoFrame* pFrame;
		int ix,iy;
		CVector3 pt3d;
		//Load frame 0's depth image
		pFrame = sfm.Frame(0);
		pFrame->LoadDepthImg();		

		ix = 149;
		iy = 267;		
		pt3d[0] = ix;
		pt3d[1] = iy;
		pt3d[2] = 1.0 / pFrame->m_DspImg.at(ix,iy); //disparity: inverse depth
		
		//Calibrate image position, assuming skew = 0
		pt3d[0] = (pt3d[0]-pFrame->m_K(0,2)) / pFrame->m_K(0,0) * pt3d[2];
		pt3d[1] = (pt3d[1]-pFrame->m_K(1,2)) / pFrame->m_K(1,1) * pt3d[2];

		//Transform from camera coordinate to world coordinate
		pt3d = pFrame->CameraCoordToWorldCoord(pt3d);

		printf("3D position of Pixel(%d, %d) in frame %d: %f, %f, %f\n", ix, iy, pFrame->m_iNo, pt3d[0],pt3d[1],pt3d[2]);

		//Load frame 60's depth image
		pFrame = sfm.Frame(60);
		pFrame->LoadDepthImg();

		ix = 472;
		iy = 261;		
		pt3d[0] = ix;
		pt3d[1] = iy;
		pt3d[2] = 1.0 / pFrame->m_DspImg.at(ix,iy); //disparity: inverse depth value

		//Calibrate image position, assuming skew = 0
		pt3d[0] = (pt3d[0]-pFrame->m_K(0,2)) / pFrame->m_K(0,0) * pt3d[2];
		pt3d[1] = (pt3d[1]-pFrame->m_K(1,2)) / pFrame->m_K(1,1) * pt3d[2];

		//Transform from camera coordinate to world coordinate
		pt3d = pFrame->CameraCoordToWorldCoord(pt3d);

		printf("3D position of Pixel(%d, %d) in frame %d: %f, %f, %f\n", ix, iy, pFrame->m_iNo, pt3d[0],pt3d[1],pt3d[2]);

	}

	return nRetCode;
}
