#include "StdAfx.h"
#include "StructureMotion.h"
#include <fstream>
#include "stringfun.h"

CStructureMotion::CStructureMotion(void)
{
}

CStructureMotion::~CStructureMotion(void)
{
}

void CStructureMotion::CreateFrames(int iCount)
{
	for(int k=0; k<m_frames.size(); ++k){
		delete m_frames[k];
	}
	m_frames.resize(iCount);

	for(int k=0; k<m_frames.size(); ++k){
		m_frames[k] = new CVideoFrame;
	}
}

void CStructureMotion::Load(const char* camFile, const char* imgFile, const char* depthFile)
{
	std::ifstream fs;
	fs.open(camFile);

	if(fs.is_open()){
		int iCount = 0;

		fs >> iCount;

		//std::cout << iCount;

		CreateFrames(iCount);

		std::string imgFilePath = imgFile;
		std::string depthFilePath = depthFile;

		for(int k=0; k<iCount; ++k){
			CVideoFrame* pFrame = Frame(k);
			for(int i=0; i<3; ++i)
				for(int j=0; j<3; ++j){
					fs >> pFrame->m_K(i,j);
				}

				for(int i=0; i<3; ++i)
					for(int j=0; j<3; ++j){
						fs >> pFrame->m_R(i,j);
					}
				for(int i=0; i<3; ++i){
					fs >> pFrame->m_T[i];
				}
			
			pFrame->m_iNo = k;
			pFrame->m_sImgName = imgFilePath;
			pFrame->m_sDepthName = depthFilePath;

			fNameIncNum(imgFilePath);
			fNameIncNum(depthFilePath);

			//pFrame->PrintParameters();
		}

		
		//Load the first image
		Frame(0)->LoadColorImage();

		for(int k=0; k<iCount; ++k){
			CVideoFrame* pFrame = Frame(k);
			pFrame->m_iWidth = Frame(0)->m_ColorImg.GetWidth();
			pFrame->m_iHeight = Frame(0)->m_ColorImg.GetHeight();
		}
	}
}