#include "calibratelasermovementdialog.h"
#include "amcerror.h"
#include "amcmirrorpanel.h"
#include "amcmotor.h"
#include "amcframegrabber.h"
#include "amcprogdlg.h"
#include "errorinfoevent.h"
#include "eventdefs.h"
#include "frameevent.h"
#include "linearregression.h"
#include "magicmirror.h"
#include "progressinfoevent.h"
#include "threadcontroller.h"
#include "videospot.h"
#include <qpushbutton.h>
#include <qprogressbar.h>
#include <qdatetime.h>
#include <qgroupbox.h>
#include <qlabel.h>
#include <qlineedit.h>
#include <qlist.h>
#include <qevent.h>
#include <qthread.h>
#include <qimage.h>
#include <qnamespace.h>
#include <qmultilineedit.h>
#include <qlist.h>
#include <math.h>

#define MAX(a,b) ((a>b) ? a : b )

extern AMCSerialPort* g_pPort[];
extern MagicMirror*		g_theMirror;
extern AMCFrameGrabber* g_pFG;


CalibrateLaserMovementDialog::CalibrateLaserMovementDialog( QWidget* parent, const char* name, bool modal, WFlags f )
	: CalibrateLaserMovementDialogBase( parent, name, modal, f )
{
	// Add your code
	TotalProgress->setTotalSteps( g_theMirror->getNumPanels() - 1 );
	PanelProgress->setTotalSteps( 39 - 1 );

  m_pFG = g_pFG;
	m_pData = new uchar[ m_pFG->getBuffsize()];
	m_pDark = new uchar[ m_pFG->getBuffsize()];
	m_pXLinearRegression = new LinearRegression();
	m_pYLinearRegression = new LinearRegression();

	QDateTime theDT = QDateTime::currentDateTime();
	QDate theDate = theDT.date();
	QTime theTime = theDT.time();
	QString qsFileName;
	qsFileName.sprintf( "/home/amc/data/AMC_%04d_%02d_%02d_%02d_%02d_%02d.cal",
                      theDate.year(), theDate.month(), theDate.day(),
                      theTime.hour(), theTime.minute(), theTime.second() );
	m_qfFile.setName( qsFileName );
	if( ! m_qfFile.open( IO_WriteOnly ) )
	{
		qDebug( "ERROR: Can't open file: %s\n", qsFileName.latin1() );
	}
	m_qtsStream.setDevice( &m_qfFile );

	m_qlSpotList.setAutoDelete( true );
}

CalibrateLaserMovementDialog::~CalibrateLaserMovementDialog()
{
	delete[]	m_pData;
	delete[]	m_pDark;
}

void CalibrateLaserMovementDialog::startThread()
{
	TotalProgress->reset();
	buttonStart->setEnabled( false );
	buttonStop->setEnabled( true );
	buttonOk->setEnabled( false );
	m_zRun = true;
	this->start();
}

void CalibrateLaserMovementDialog::outputProgress(QString qsMsg)
{
	QThread::postEvent( this, new ProgressInfoEvent( qsMsg ) );
}

void CalibrateLaserMovementDialog::outputError(QString qsMsg)
{
	QThread::postEvent( this, new ErrorInfoEvent( qsMsg ) );
}

void CalibrateLaserMovementDialog::run()
{
	QString qsMsg;

//	buttonStart->setEnabled( false );
  QTime t;
  t.start();                          // start clock

	for(int i=0; i<8; i++)
	{
		QList<AMCMirrorPanel> qlPanels = g_theMirror->panelForPort(i);
		for( AMCMirrorPanel* pActualPanel = qlPanels.first();
			 pActualPanel; pActualPanel = qlPanels.next() )
		{
			checkStop();

			QCustomEvent* pEvent = new QCustomEvent( NEW_PANEL_EVENT );
			pEvent->setData( (void*) pActualPanel );
			QThread::postEvent( this, (QEvent*) pEvent );
			m_pCurrentPanel = pActualPanel;
			PanelProgress->reset();
			qsMsg.sprintf( "Calibrating panel : (%d,%d)",
										 m_pCurrentPanel->i(), m_pCurrentPanel->j());
			m_qtsStream << "# " << endl << "# " <<qsMsg << endl;
			outputProgress( qsMsg );
		  m_pFG->grabFrame( m_pDark );
//			m_aMutex.lock();
//			m_aMutex.unlock();
  	
			AMCSerialPort* pPort = g_pPort[ m_pCurrentPanel->port()-1 ];
			m_pMotor = new AMCMotor( pPort );

			try
			{
	      m_pMotor->unselectBox();
				int iBox = m_pCurrentPanel->box();
				m_pMotor->selectBox( iBox );
	 			int iDriver = m_pCurrentPanel->driver();
				m_pMotor->selectDriver( iDriver );	
				int iFrequency = m_pMotor->getFrequency();
				m_pMotor->switchLaser( true );
				m_pCurrentPanel->setLaserOn( true );

				outputProgress("Centering Panel");
				m_pMotor->centerMotors();
				m_pMotor->waitForMotors( m_pMotor->calcTimeout( 17000 )+2 );
				m_pCurrentPanel->setX( 0 );
				m_pCurrentPanel->setY( 0 );

				int iMinJ = -4000;
				int iMaxJ = -4000;
				double dMinX, dMinY;
				double dMaxX, dMaxY;
				m_pXLinearRegression->reset();
				for( int j= -1200; j<=1200; j+=300)
				{
					checkStop();
					processSpot( j, 0 );
					m_pXLinearRegression->addPoint( m_dSpotX, m_dSpotY );
					if( m_dSpotX != -1. )
					{
						if ( iMinJ < -3000 )
						{
							iMinJ = j;
							dMinX = m_dSpotX;
							dMinY = m_dSpotY;
						}
						else
						{
							iMaxJ = j;
							dMaxX = m_dSpotX;
							dMaxY = m_dSpotY;
						}
					}
					m_qtsStream << j << " " << 0 << " " << m_dSpotX << " " << m_dSpotY << " ";
					m_qtsStream << m_pXLinearRegression->getAxis() << " " << m_pXLinearRegression->getSlope() << endl;
					QCustomEvent* pEvent = new QCustomEvent( X_SPOT_EVENT );
					pEvent->setData( (void*) m_pXLinearRegression );
					QThread::postEvent( this, (QEvent*) pEvent );
				}
				m_pCurrentPanel->setAxisX( m_pXLinearRegression->getAxis() );
				m_pCurrentPanel->setSlopeX( m_pXLinearRegression->getSlope() );
				if( iMinJ != -4000 )
				{
					double dSteps = (double) (iMaxJ - iMinJ);
					if ( dSteps > 0)
					{
						double dDx = (dMaxX - dMinX);
						double dDy = (dMaxY - dMinY);
						double dPixels = sqrt( dDx * dDx + dDy * dDy );
						m_qtsStream << "= " << dDx << "  " << dDy << "  " << dSteps << "  " << dPixels;
						m_qtsStream << "  " << dSteps / dPixels << endl << endl;
						m_pCurrentPanel->setConversionY( dSteps / dPixels );
					}
					else
						m_pCurrentPanel->setConversionY( 0.0 );
       	}

				iMinJ = -4000;
				iMaxJ = -4000;
				m_pYLinearRegression->reset();
				for( int j= -1200; j<=1200; j+=300)
				{
					checkStop();
					processSpot( 0, j );
					m_pYLinearRegression->addPoint( m_dSpotX, m_dSpotY );
					if( m_dSpotX != -1. )
					{
						if ( iMinJ < -3000 )
						{
							iMinJ = j;
							dMinX = m_dSpotX;
							dMinY = m_dSpotY;
						}
						else
						{
							iMaxJ = j;
							dMaxX = m_dSpotX;
							dMaxY = m_dSpotY;
						}
					}
					m_qtsStream << 0 << " " << j << " " << m_dSpotX << " " << m_dSpotY << " ";
					m_qtsStream << m_pYLinearRegression->getAxis() << " " << m_pYLinearRegression->getSlope() << endl;
					QCustomEvent* pEvent = new QCustomEvent( Y_SPOT_EVENT );
					pEvent->setData( (void*) m_pYLinearRegression );
					QThread::postEvent( this, (QEvent*) pEvent );
				}
				m_pCurrentPanel->setAxisY( m_pYLinearRegression->getAxis() );
				m_pCurrentPanel->setSlopeY( m_pYLinearRegression->getSlope() );

				if( iMinJ != -4000 )
				{
					double dSteps = (double) (iMaxJ - iMinJ);
					if ( dSteps > 0)
					{
						double dDx = (dMaxX - dMinX);
						double dDy = (dMaxY - dMinY);
						double dPixels = sqrt( dDx * dDx + dDy * dDy );
						m_qtsStream << "= " << dDx << "  " << dDy << "  " << dSteps << "  " << dPixels;
						m_qtsStream << "  " << dSteps / dPixels << endl << endl;
						m_pCurrentPanel->setConversionY( dSteps / dPixels );
					}
					else
						m_pCurrentPanel->setConversionY( 0.0 );
       	}
				processSpot( 0, 0 );
				m_pMotor->switchLaser( false );
				m_pCurrentPanel->setLaserOn( false );

			}
			catch( AMCError& e)
			{
				qsMsg.sprintf( "Error while processing panel : (%d,%d)",
											 m_pCurrentPanel->i(), m_pCurrentPanel->j());
				qsMsg += e.formatError( 2 );
				m_qtsStream << "! " <<qsMsg << endl;
				outputError( qsMsg );
				
				/* If we had an error we will try 3 times to turn off the Laser.
				 * This is necessary to be sure that the laser is off. Otherwise when
				 * working with the next panel we have to spots and the calibartion
				 * will fail. */
				for( int i=0; i<3; i++)
				{
					try
					{
						m_pMotor->switchLaser( false );
						i = 3;		// If we reach this point we succesfully switched of the laser
											// so we can exit the loop.
					}
					catch( AMCError& e)
					{				 		// Ignore errors in this case.
					}
				}
			}
			
			qsMsg.sprintf( "Done with panel : (%d,%d)",
											 m_pCurrentPanel->i(), m_pCurrentPanel->j());
			outputError( qsMsg );
			QThread::postEvent( this, new QCustomEvent( TOTAL_PROGRESS_EVENT ) );

			delete m_pMotor;
		}
	}

	qsMsg.sprintf( "Total Time: %dms", t.elapsed() );
	outputProgress( qsMsg );
	QThread::postEvent( this, new QCustomEvent( THREAD_END_EVENT ) );
}

void CalibrateLaserMovementDialog::customEvent(QCustomEvent* e)
{
  if ( e->type() == PROGRESS_INFO_EVENT )
	{             
		ProgressInfoEvent* pie = (ProgressInfoEvent*) e;
		QString qsMsg = pie->getMessage();

		ProgressField->append( qsMsg );
		ProgressField->setCursorPosition( ProgressField->numLines()+1, 0 );
	}
  if ( e->type() == ERROR_INFO_EVENT )
	{
		ErrorInfoEvent* eie = (ErrorInfoEvent*) e;
		QString qsMsg = eie->getMessage();

		ErrorField->append( qsMsg );
		ErrorField->setCursorPosition( ErrorField->numLines()+1, 0 );
	}
  if ( e->type() == NEW_PANEL_EVENT )
	{
		QString info;

		AMCMirrorPanel* pPanel = (AMCMirrorPanel*) e->data();
		PanelBox->setTitle( info.sprintf( "Panel: %d,%d", pPanel->i(), pPanel->j() ) );
		PortLabel->setText( info.sprintf( "Device: /dev/ttyS%d", pPanel->port()+3 ) );
		BoxLabel->setText( info.sprintf( "Box: %02d", pPanel->box() ) );
		DriverLabel->setText( info.sprintf( "Driver: %d", pPanel->driver() ) );
	}
  if ( e->type() == X_SPOT_EVENT )
	{
		QString info;
		LinearRegression* pRegress = (LinearRegression*) e->data();
		XAxisEdit->setText( info.sprintf( "%8.2f", pRegress->getAxis() ) );
		XSlopeEdit->setText( info.sprintf( "%8.2f", pRegress->getSlope() ) );
	}
  if ( e->type() == Y_SPOT_EVENT )
	{
		QString info;
		LinearRegression* pRegress = (LinearRegression*) e->data();
		YAxisEdit->setText( info.sprintf( "%8.2f", pRegress->getAxis() ) );
		YSlopeEdit->setText( info.sprintf( "%8.2f", pRegress->getSlope() ) );
	}
  if ( e->type() == PANEL_PROGRESS_EVENT )
		PanelProgress->setProgress( PanelProgress->progress()+1 );
  if ( e->type() == TOTAL_PROGRESS_EVENT )
		TotalProgress->setProgress( TotalProgress->progress()+1 );
//  if ( e->type() == FRAME_EVENT )
//		saveImage( (FrameEvent*) e );
  if ( e->type() == THREAD_END_EVENT )
		buttonStart->setEnabled( true );

}

/*
void CalibrateLaserMovementDialog::saveImage( FrameEvent* p_pFrameEvent )
{
	int i,j,x,y;
	QPixmap* pPixmap;

  i = p_pFrameEvent->getI();
  j = p_pFrameEvent->getJ();
  x = p_pFrameEvent->getX();
  y = p_pFrameEvent->getY();
  pPixmap = p_pFrameEvent->getPixmap();

	QString fileName;
	fileName.sprintf( "/home/amc/data/image(%d,%d)at[%d,%d].bmp", i, j, x, y );
//	qDebug("Saving "+fileName );
//	fileName.sprintf( "/home/amc/data/image(%d,%d).png", i, j );

	pPixmap->save( fileName, "BMP" );
	delete pPixmap;
}
*/

/** Find the Laser spot */
void CalibrateLaserMovementDialog::findSpot( double& p_dX, double& p_dY )
{
  int threshold = getThreshold();

	m_qlSpotList.clear();
	int top = 0;
	int bottom = MY_HEIGHT;
	int left = 0;
	int right = MY_WIDTH;

  for (int i=top; i <bottom; i++)
	{
     for (int j=left; j < right ; j++)
		{
			int index= (j + i * MY_WIDTH) * MY_DEPTH;
			bool zFound = false;
			uchar data = m_pData[index];
			if( data > threshold )
	 		{
				for( VideoSpot* pActualSpot = m_qlSpotList.first();
		 				 pActualSpot; pActualSpot = m_qlSpotList.next() )
				{
					if( pActualSpot->contains( j, i ) )
					{
						pActualSpot->append( j, i, data );
						zFound = true;
						break;
					}
				}
				if( zFound == false )
				{
					VideoSpot* pSpot = new VideoSpot();
					pSpot->append( j, i, data );
					m_qlSpotList.append( pSpot );
				}

	 		}
		}
	}
	if( m_qlSpotList.count() == 1)
	{
		VideoSpot* pSpot = m_qlSpotList.first();
		p_dX = pSpot->getX();
		p_dY = pSpot->getY();
	}
	else
	{
		p_dX = -1.;
		p_dY = -1.;
	}
}



/** No descriptions */
int CalibrateLaserMovementDialog::getThreshold()
{
	long sumx =0;
	long sumx2 = 0;

	long numPixel = MY_HEIGHT * MY_WIDTH;
	for(int ind=0; ind < ( numPixel * MY_DEPTH); ind+=MY_DEPTH)
	{
		long data = (long) m_pData[ind];
		sumx += data;
		sumx2 += data * data;
	}

	double mean = ((double) sumx) / ((double) (numPixel));
	double var = ((double) sumx2) / ((double) (numPixel)) - mean * mean;
	double stddev = sqrt(var);

  int threshold = (int) (mean + 15 * stddev);
//	qDebug("Mean: %f StdDev: %f   Threshold %d", mean, stddev, threshold );
	return threshold;
}


/** Process Spot */
void CalibrateLaserMovementDialog::processSpot( int p_iX, int p_iY )
{
	QString qsMsg;

	qsMsg.sprintf("Moving to position: (%d,%d)", p_iX, p_iY );
	outputProgress( qsMsg );
	int iX, iY;
	iX = p_iX - m_pCurrentPanel->getX();
	iY = p_iY - m_pCurrentPanel->getY();
	m_pMotor->moveMotors( iX, iY );
	m_pMotor->waitForMotors( m_pMotor->calcTimeout( MAX( abs(iX), abs(iY) ) ) + 1 );			
	m_pCurrentPanel->setX( p_iX );
	m_pCurrentPanel->setY( p_iY );
	
//	outputProgress( "Turning Laser on" );
//	m_pMotor->switchLaser( true );
//	m_pCurrentPanel->setLaserOn( true );

//	msleep(20);
	outputProgress( "Grabing Frame" );
  m_pFG->grabFrame( m_pData );
	QThread::postEvent( this, new FrameEvent( m_pCurrentPanel->i(),
																						m_pCurrentPanel->j(),
																						p_iX, p_iY ) );

//	outputProgress( "Turning Laser off" );
//	m_pMotor->switchLaser( false );
//	m_pCurrentPanel->setLaserOn( false );

	outputProgress( "Finding spot" );

	substractDark();
	findSpot( m_dSpotX, m_dSpotY );
	qsMsg.sprintf("Spot at: (%6.1f,%6.1f)", m_dSpotX, m_dSpotY );
	outputProgress( qsMsg );

	QThread::postEvent( this, new QCustomEvent( PANEL_PROGRESS_EVENT ) );

}

void CalibrateLaserMovementDialog::stop()
{
	m_aMutex.lock();
	m_zRun = false;
	m_aMutex.unlock();
	this->wait();
	buttonStart->setEnabled( true );
	buttonStop->setEnabled( false );
	buttonOk->setEnabled( true );
}

void CalibrateLaserMovementDialog::checkStop()
{
	m_aMutex.lock();
	if( ! m_zRun )
	{
		m_aMutex.unlock();
		this->exit();
	}	
	m_aMutex.unlock();
}

/** No descriptions */
void CalibrateLaserMovementDialog::substractDark()
{
	uchar* pData = m_pData;
	uchar* pDark = m_pDark;
	uchar* pEnd = m_pData + ( MY_HEIGHT * MY_WIDTH * MY_DEPTH);
	for( ; pData < pEnd; pData++, pDark++ )
	{
		int help = (int) *pData - (int) *pDark;
		if(help < 0)
			help = 0;
		*pData = (uchar) help;
	}
}

