// CalibrtwocamaDlg.cpp : implementation file
//

#include "stdafx.h"
#include "Calibrtwocama.h"
#include "CalibrtwocamaDlg.h"
#include "math.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
#define CHANNEL1	"VID1"
#define CHANNEL2	"VID2"
#define EURESYS_SURFACE_COUNT 2

FILE *cps; //file to write corner points

//for visual number effect to see the corners that were found
char* numbers[] = 
{ "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", "64", "65", "66", "67", "68", "69", "70", 

"71", "72", "73", "74", "75", "76", "77", "78", "79", "80",

"81", "82", "83", "84", "85", "86", "87", "88", "89", "90"

};
/////////////////////////////////////////////////////////////////////////////
// CCalibrtwocamaDlg dialog

CCalibrtwocamaDlg::CCalibrtwocamaDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CCalibrtwocamaDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CCalibrtwocamaDlg)
	m_nImageNumber = 0;
	m_nXHeight = 0;
	m_bDisplayCorners = FALSE;
	m_bDisplayRT = FALSE;
	m_nLeftCamNo = 0;
	m_nRightCamNO = 0;
	m_nXWidth = 0;
	m_dGridHeight = 0.0;
	m_dGridWidth = 0.0;
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	//initialize the multi-cameras parametres
	m_Channel1 = 0 ;
	m_Channel2 = 0 ;

    m_pCurrent1 = NULL ;
	m_pCurrent1 = NULL ;
	
	
    m_SizeX = 0;
    m_SizeY = 0;
    m_BufferPitch = m_SizeX;
    m_bScreenRefreshCompleted1 = TRUE;
	m_bScreenRefreshCompleted2 = TRUE;


	m_pBitmapInfo = (BITMAPINFO *) new BYTE[sizeof(BITMAPINFO)];
    m_pBitmapInfo->bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
    m_pBitmapInfo->bmiHeader.biPlanes = 1;
    m_pBitmapInfo->bmiHeader.biBitCount = 24;
    m_pBitmapInfo->bmiHeader.biCompression = BI_RGB;
    m_pBitmapInfo->bmiHeader.biSizeImage = 0;
    m_pBitmapInfo->bmiHeader.biXPelsPerMeter = 0;
    m_pBitmapInfo->bmiHeader.biYPelsPerMeter = 0;
    m_pBitmapInfo->bmiHeader.biClrUsed = 0;
    m_pBitmapInfo->bmiHeader.biClrImportant = 0;
    m_pBitmapInfo->bmiHeader.biWidth = 0;
    m_pBitmapInfo->bmiHeader.biHeight = 0;

    m_ChannelState = MC_ChannelState_ORPHAN;
	
	lfg=0;rfg=0;
	//initialize the calibration parametres
	m_nImageNumber   = 0;
	p2p_InputImages  = 0;
	m_cstrImageNames = NULL;
	
	m_distortion   = NULL;
	m_cameraMatrix = NULL;
	m_rotMatrs	   = NULL;
	m_transVects   = NULL;
	
	uveff  = NULL;
	XYZeff = NULL;
}

void CCalibrtwocamaDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CCalibrtwocamaDlg)
	DDX_Text(pDX, IDC_NoImages, m_nImageNumber);
	DDV_MinMaxInt(pDX, m_nImageNumber, 0, 100);
	DDX_Text(pDX, IDC_CROSS_HEIGHT, m_nXHeight);
	DDV_MinMaxInt(pDX, m_nXHeight, 0, 100);
	DDX_Check(pDX, IDC_DISPLAY_CORNERS, m_bDisplayCorners);
	DDX_Check(pDX, IDC_DISPLAY_RT, m_bDisplayRT);
	DDX_Text(pDX, IDC_LCAMNO, m_nLeftCamNo);
	DDV_MinMaxInt(pDX, m_nLeftCamNo, 0, 100);
	DDX_Text(pDX, IDC_RCAMNO, m_nRightCamNO);
	DDV_MinMaxInt(pDX, m_nRightCamNO, 0, 100);
	DDX_Text(pDX, IDC_CROSS_WIDTH, m_nXWidth);
	DDV_MinMaxInt(pDX, m_nXWidth, 0, 100);
	DDX_Text(pDX, IDC_GRID_HEIGHT, m_dGridHeight);
	DDX_Text(pDX, IDC_GRID_WIDTH, m_dGridWidth);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CCalibrtwocamaDlg, CDialog)
	//{{AFX_MSG_MAP(CCalibrtwocamaDlg)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_STOP, OnStop)
	ON_BN_CLICKED(IDC_ACTIVE, OnActive)
	ON_BN_CLICKED(IDC_LEFTCA, OnLeftca)
	ON_BN_CLICKED(IDC_RIGHTCA, OnRightca)
	ON_BN_CLICKED(IDC_SAVE, OnSave)
	ON_BN_CLICKED(IDC_LOAD_SELECTED, OnLoadSelected)
	ON_BN_CLICKED(IDC_LOAD, OnLoad)
	ON_BN_CLICKED(IDC_CALIBRATE, OnCalibrate)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCalibrtwocamaDlg message handlers

BOOL CCalibrtwocamaDlg::OnInitDialog()
{
	this->SetWindowText("Two Cameras Calibration ");
	
	CDialog::OnInitDialog();

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here

	int cx=::GetSystemMetrics(SM_CXSCREEN);
	int cy=::GetSystemMetrics(SM_CYSCREEN);
	SetWindowPos(&this->wndTopMost,0,0,0,0,SWP_NOMOVE|SWP_NOSIZE);
	::SetWindowPos(m_hWnd,HWND_TOPMOST,-(GetSystemMetrics(SM_CXBORDER)+1),-(GetSystemMetrics(SM_CYBORDER)+1),
		cx+1,cy+1,SWP_NOZORDER);
    
	//Set the control object position
	GetDlgItem(IDC_VID1)->MoveWindow(0,0,cx/2,cy/2);
	GetDlgItem(IDC_VID2)->MoveWindow(cx/2,0,cx/2,cy/2);
	GetDlgItem(IDC_SHOWCM1)->MoveWindow(80,cy/2+20,320,20);
    GetDlgItem(IDC_SHOWCM2)->MoveWindow(cx/2+80,cy/2+20,320,20);
    GetDlgItem(IDC_ACTIVE)->MoveWindow(20+30,cy/2+40,100,20);
    GetDlgItem(IDC_STOP)->MoveWindow(200+30,cy/2+40,100,20);
	GetDlgItem(IDC_LEFTCA)->MoveWindow(380+30,cy/2+40,100,20);
	GetDlgItem(IDC_LCAMNO)->MoveWindow(480+10,cy/2+40,15,20);
    GetDlgItem(IDC_RIGHTCA)->MoveWindow(560+30,cy/2+40,100,20);
	GetDlgItem(IDC_RCAMNO)->MoveWindow(700,cy/2+40,15,20);
	GetDlgItem(IDC_SAVE)->MoveWindow(560+30+180,cy/2+40,100,20);
	GetDlgItem(IDC_CABSTATIC)->MoveWindow(30,cy/2+65,cx-80,cy/2-65);
	GetDlgItem(IDC_NOIMAGE)->MoveWindow(40,cy/2+70,20,20);
	GetDlgItem(IDC_NoImages)->MoveWindow(70,cy/2+70,20,20);
	GetDlgItem(IDC_LOAD_SELECTED)->MoveWindow(92,cy/2+70,10,20);
    GetDlgItem(IDC_LOAD)->MoveWindow(105,cy/2+70,20,20);
	GetDlgItem(IDC_GRDWIDTH)->MoveWindow(580,cy/2+70,20,20);
	GetDlgItem(IDC_GRID_WIDTH)->MoveWindow(610,cy/2+70,20,20);
	GetDlgItem(IDC_COLNUM)->MoveWindow(40,cy/2+120,20,20);
	GetDlgItem(IDC_CROSS_WIDTH)->MoveWindow(70,cy/2+120,20,20);
	GetDlgItem(IDC_ROWNUM)->MoveWindow(40,cy/2+180,20,20);
	GetDlgItem(IDC_CROSS_HEIGHT)->MoveWindow(70,cy/2+180,20,20);
	GetDlgItem(IDC_GRDHIGT)->MoveWindow(580,cy/2+100,20,20);
	GetDlgItem(IDC_GRID_HEIGHT)->MoveWindow(610,cy/2+100,20,20);
	GetDlgItem(IDC_DISPLAY_CORNERS)->MoveWindow(500,cy/2+140,60,20);
	GetDlgItem(IDC_DISPLAY_RT)->MoveWindow(500,cy/2+180,60,20);
		
	GetDlgItem(IDC_CALIBRATE)->MoveWindow(30,cy/2+260,cx/2-30,60);
    GetDlgItem(IDC_RESULTS)->MoveWindow(cx/2+30,cy/2+260,cx/2-30,60);

    // initializations of interface parameters.
	m_nImageNumber = 0;
	m_nXHeight = 8;
	m_nXWidth  = 6;
	m_dGridHeight = 25;
	m_dGridWidth  = 25;
	m_bDisplayCorners = false;
	m_btnApplyOrder   = true;
	m_bCompansateDistortion=true;
	califlag="";
	
	McOpenDriver (NULL);
	// Activate message box error handling and generate an error log file
    McSetParamInt (MC_CONFIGURATION, MC_ErrorHandling, MC_ErrorHandling_MSGBOX);
    McSetParamStr (MC_CONFIGURATION, MC_ErrorLog, "error.log");
	
	// Channel1
    McCreate(MC_CHANNEL, &m_Channel1);
    McSetParamInt(m_Channel1, MC_DriverIndex, 0);
    McSetParamStr(m_Channel1, MC_Connector,CHANNEL1 );
    // Choose the video standard
    McSetParamStr(m_Channel1, MC_CamFile, "PAL");
    // Choose the pixel color format
    McSetParamInt(m_Channel1, MC_ColorFormat, MC_ColorFormat_RGB24);

    // Choose the acquisition mode
    McSetParamInt(m_Channel1, MC_AcquisitionMode, MC_AcquisitionMode_VIDEO);
	//McSetParamInt(m_Channel1, MC_AcquisitionMode, MC_AcquisitionMode_SNAPSHOT);
    // Choose the way the first acquisition is triggered
    McSetParamInt(m_Channel1, MC_TrigMode, MC_TrigMode_IMMEDIATE);
    // Choose the triggering mode for subsequent acquisitions
    McSetParamInt(m_Channel1, MC_NextTrigMode, MC_NextTrigMode_REPEAT);
    // Choose the number of images to acquire
    McSetParamInt(m_Channel1, MC_SeqLength_Fr, MC_INDETERMINATE);


    // Retrieve image dimensions
    McGetParamInt(m_Channel1, MC_ImageSizeX, &m_SizeX);
    McGetParamInt(m_Channel1, MC_ImageSizeY, &m_SizeY);
    McGetParamInt(m_Channel1, MC_BufferPitch, &m_BufferPitch);

    // The memory allocation for the images is automatically done by Multicam when activating the channel.
    // We only set the number of surfaces to be created by MultiCam.
    McSetParamInt(m_Channel1, MC_SurfaceCount, EURESYS_SURFACE_COUNT);

    // Enable MultiCam signals
    McSetParamInt(m_Channel1, MC_SignalEnable + MC_SIG_SURFACE_PROCESSING, MC_SignalEnable_ON);
    McSetParamInt(m_Channel1, MC_SignalEnable + MC_SIG_ACQUISITION_FAILURE, MC_SignalEnable_ON);

    // Register the callback function
    McRegisterCallback(m_Channel1, GlobalCallback, this);	
	// McGetParamInt (MC_SIG_SURFACE_PROCESSING, MC_SurfaceAddr, (PINT32) &m_pCurrent1);

	McSetParamInt(m_Channel1, MC_ChannelState, MC_ChannelState_ACTIVE);
	

	// Channel 2
	McCreate(MC_CHANNEL, &m_Channel2);
    McSetParamInt(m_Channel2, MC_DriverIndex, 0);
    McSetParamStr(m_Channel2, MC_Connector, CHANNEL2);
    // Choose the video standard
    McSetParamStr(m_Channel2, MC_CamFile, "PAL");
    // Choose the pixel color format
    McSetParamInt(m_Channel2, MC_ColorFormat, MC_ColorFormat_RGB24);

    // Choose the acquisition mode
    McSetParamInt(m_Channel2, MC_AcquisitionMode, MC_AcquisitionMode_VIDEO);
    // Choose the way the first acquisition is triggered
    McSetParamInt(m_Channel2, MC_TrigMode, MC_TrigMode_IMMEDIATE);
    // Choose the triggering mode for subsequent acquisitions
    McSetParamInt(m_Channel2, MC_NextTrigMode, MC_NextTrigMode_REPEAT);
    // Choose the number of images to acquire
    McSetParamInt(m_Channel2, MC_SeqLength_Fr, MC_INDETERMINATE);

    // The memory allocation for the images is automatically done by Multicam when activating the channel.
    // We only set the number of surfaces to be created by MultiCam.
    McSetParamInt(m_Channel2, MC_SurfaceCount, EURESYS_SURFACE_COUNT);

    // Enable MultiCam signals
    McSetParamInt(m_Channel2, MC_SignalEnable + MC_SIG_SURFACE_PROCESSING, MC_SignalEnable_ON);
    McSetParamInt(m_Channel2, MC_SignalEnable + MC_SIG_ACQUISITION_FAILURE, MC_SignalEnable_ON);

    // Register the callback function
    McRegisterCallback(m_Channel2, GlobalCallback, this);	
	// McGetParamInt (MC_SIG_SURFACE_PROCESSING, MC_SurfaceAddr, (PINT32) &m_pCurrent1);

	McSetParamInt(m_Channel2, MC_ChannelState, MC_ChannelState_ACTIVE);

	//left and right cameras sampling number initialization
	m_nLeftCamNo=0;
	m_nRightCamNO=0;
	return TRUE;  // return TRUE  unless you set the focus to a control
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CCalibrtwocamaDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
			if (!m_bScreenRefreshCompleted1)
		{
			DispCam1();
		}
		if (!m_bScreenRefreshCompleted2)
		{
			DispCam2();
		}
	}
}

// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CCalibrtwocamaDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

void WINAPI GlobalCallback(PMCSIGNALINFO SigInfo)
{
    if (SigInfo && SigInfo->Context)
    {
        //CPicoloVideoDoc* pDoc = (CPicoloVideoDoc*) SigInfo->Context;
        //pDoc->Callback (SigInfo);
		CCalibrtwocamaDlg *Dlg = (CCalibrtwocamaDlg*) SigInfo->Context;
		Dlg->Callback(SigInfo);
    }
}
void CCalibrtwocamaDlg::Callback(PMCSIGNALINFO SigInfo)
{
	if (SigInfo->Instance==m_Channel1)
	{
		switch(SigInfo->Signal) {
		 case MC_SIG_SURFACE_PROCESSING:
			// Update "current" surface address pointer
			 McGetParamInt (SigInfo->SignalInfo, MC_SurfaceAddr, (PINT32) &m_pCurrent1); 

			 //----------------------------------------
			//
			// Insert the eVision code here.
			//
			//----------------------------------------

			// Post screen refresh message only if previous refresh completed
			if(m_bScreenRefreshCompleted1)
			{
				m_bScreenRefreshCompleted1 = FALSE;     
				CRect rect;
				GetDlgItem(IDC_VID1)->GetClientRect(&rect);			
				InvalidateRect(rect,FALSE);
			}
			break;
		case MC_SIG_ACQUISITION_FAILURE:
			AfxMessageBox("Acquisition Failure !");
			break;
		default:
			break;
		}
	}
	if (SigInfo->Instance==m_Channel2)
	{
		switch(SigInfo->Signal) {
		 case MC_SIG_SURFACE_PROCESSING:
			// Update "current" surface address pointer
			 McGetParamInt (SigInfo->SignalInfo, MC_SurfaceAddr, (PINT32) &m_pCurrent2); 

			 //----------------------------------------
			//
			// Insert the eVision code here.
			//
			//----------------------------------------

			// Post screen refresh message only if previous refresh completed
			if(m_bScreenRefreshCompleted2)
			{
				m_bScreenRefreshCompleted2 = FALSE;     
				CRect rect;
				GetDlgItem(IDC_VID2)->GetClientRect(&rect);			
				InvalidateRect(rect,FALSE);
			}
			break;
		case MC_SIG_ACQUISITION_FAILURE:
			AfxMessageBox("Acquisition Failure !");
			break;
		default:
			break;
		}
	}
}

CCalibrtwocamaDlg::~CCalibrtwocamaDlg()
{
if (m_pBitmapInfo) 	delete m_pBitmapInfo;
delete[]m_distortion  ;
delete[]m_cameraMatrix;
delete[]m_transVects  ;
delete[]m_rotMatrs    ;
delete[]uveff		  ;
delete[]XYZeff		  ;
delete[]m_cstrImageNames;

if( p2p_InputImages != NULL )
{
	for( int i=0; i<m_nImageNumber; i++ )
	{
		cvReleaseImage( &p2p_InputImages[i] );