/***************************************************************************
                          calibratepaneldialog.cpp  -  description
                             -------------------
    begin                : Tue Sep 2 2003
    copyright            : (C) 2003 by Martin Merck
    email                : merck@astro.uni-wuerzburg.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include <qcheckbox.h>
#include <qgroupbox.h>
#include <qlayout.h>
#include <qlineedit.h>
#include <qlabel.h>
#include <qlist.h>
#include <qmessagebox.h>
#include <qmultilineedit.h>
#include <qprogressbar.h>
#include <qpushbutton.h>
#include <qstring.h>
#include "amcerror.h"
#include "amcframegrabber.h"
#include "amcmirrorpanel.h"
#include "amcmotor.h"
#include "amcserialport.h"
#include "calibratepaneldialog.h"
#include "errorinfoevent.h"
#include "linearregression.h"
#include "magicmirror.h"
#include "progressinfoevent.h"
#include "videoframe.h"
#include "videospot.h"
#include <math.h>

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

extern int						g_iRefX, g_iRefY;
extern AMCSerialPort* g_pPort[];
extern MagicMirror*		g_theMirror;

CalibratePanelDialog::CalibratePanelDialog(QWidget *parent, const char *name, AMCFrameGrabber* p_pFG )
	: CalibratePanelDialogBase(parent, name, true), m_pFG( p_pFG )
{
	PanelProgress->setTotalSteps( 39 - 1 );
	m_pData = new uchar[ m_pFG->getBuffsize()];
	m_pDark = new uchar[ m_pFG->getBuffsize()];
	m_pXLinearRegression = new LinearRegression();
	m_pYLinearRegression = new LinearRegression();

	m_pVideoFrame = new VideoFrame( this, "Video", m_pFG );
  m_pVideoFrame->setMinimumSize( QSize( MY_WIDTH, MY_HEIGHT ) );
  m_pVideoFrame->setMaximumSize( QSize( MY_WIDTH, MY_HEIGHT ) );
  m_pVideoFrame->setFrameShape( QFrame::NoFrame );
	m_pVideoFrame->setCalibDlg( this );
	((QGridLayout*) this->layout())->addWidget( m_pVideoFrame, 0, 0 );
	connect( m_pVideoFrame, SIGNAL( spot(int, int ) ), this ,SLOT( setSpotInfoSlot( int, int ) ) );
	connect( m_pVideoFrame, SIGNAL( gref(int, int ) ), this ,SLOT( setGRefInfoSlot( int, int ) ) );

	m_qlSpotList.setAutoDelete( true );
	m_qlLineList.setAutoDelete( true );

	if( (g_iRefX != -1) && (g_iRefY != -1) )
		buttonGoto->setEnabled( true );
	
	m_iSpotX = -1;
	m_iSpotY = -1;
	m_iVSpotX = -1;
	m_iVSpotY = -1;
	setGRefInfoSlot( g_iRefX, g_iRefY );
}

CalibratePanelDialog::~CalibratePanelDialog()
{
	// If laser was on we will turn it on again.
	if( m_zLaser )
	{
  	try
  	{
  		m_pMotor->switchLaser( true );
  		m_pPanel->setLaserOn( true );
  	}
  	catch( AMCError e )
  	{
  	}
	}

	qDebug("Deleting video frame");
	delete m_pVideoFrame;
	qDebug("Deleting regression objects");
	delete m_pXLinearRegression;
	delete m_pYLinearRegression;
	qDebug("Deleting image buffers");
	delete[] m_pData;
	delete[] m_pDark;
	qDebug("Deleting motor");
	delete m_pMotor;
	qDebug("Destructing CalibaratePanelDialog");
}

/** No descriptions */
void CalibratePanelDialog::setPanel( AMCMirrorPanel* p_pPanel )
{
	m_pPanel = p_pPanel;

	QString info;
	PanelBox->setTitle( info.sprintf( "Panel: %d,%d", m_pPanel->i(), m_pPanel->j() ) );
	PortLabel->setText( info.sprintf( "Device: /dev/ttyS%d", m_pPanel->port()+3 ) );
	BoxLabel->setText( info.sprintf( "Box: %02d", m_pPanel->box() ) );
	DriverLabel->setText( info.sprintf( "Driver: %d", m_pPanel->driver() ) );

	AMCSerialPort* pPort = g_pPort[ m_pPanel->port()-1 ];
	m_pMotor = new AMCMotor( pPort );

	m_zLaser = m_pPanel->isLaserOn();
}

/** No descriptions */
void CalibratePanelDialog::setSpotInfoSlot( int p_iX, int p_iY )
{
	XSpotEdit->setText( QString::number(p_iX) );
	YSpotEdit->setText( QString::number(p_iY) );
	m_iVSpotX = p_iX;
	m_iVSpotY = p_iY;
}

/** No descriptions */
void CalibratePanelDialog::setGRefInfoSlot( int p_iX, int p_iY )
{
	XGRefEdit->setText( QString::number(p_iX) );
	YGRefEdit->setText( QString::number(p_iY) );
	buttonGoto->setEnabled( true );
}

/** No descriptions */
void CalibratePanelDialog::startSlot()
{
	m_qlLineList.clear();
	PanelProgress->reset();
	buttonStart->setEnabled( false );
	buttonGoto->setEnabled( false );
	buttonOk->setEnabled( false );
	this->start();
}

/** No descriptions */
void CalibratePanelDialog::run()
{
	QString info, qsMsg;
  	
	try
	{
	  m_pFG->grabFrame( m_pDark );
		msleep(400);

	  m_pMotor->unselectBox();
		int iBox = m_pPanel->box();
		m_pMotor->selectBox( iBox );
	 	int iDriver = m_pPanel->driver();
		m_pMotor->selectDriver( iDriver );	
		int iFrequency = m_pMotor->getFrequency();

		outputProgress("Centering Panel");
		m_pMotor->centerMotors();
		m_pMotor->waitForMotors( m_pMotor->calcTimeout( 17000 )+2 );
		m_pPanel->setX( 0 );
		m_pPanel->setY( 0 );
		m_pMotor->switchLaser( true );
 		m_pPanel->setLaserOn( true );

		int iMinJ = -4000;
		int iMaxJ = -4000;
		double dMinX, dMinY;
		double dMaxX, dMaxY;
		m_pXLinearRegression->reset();
		for( int j= -2700; j<=2700; j+=300)
		{
			processSpot( j, 0 );
			if( m_qlSpotList.count() == 1)
				m_qlLineList.append( new VideoSpot( m_iSpotX, m_iSpotY ) );
			m_pXLinearRegression->addPoint( m_iSpotX, m_iSpotY );
			if( m_iSpotX != -1 )
			{
				if ( iMinJ < -3000 )
				{
					iMinJ = j;
					dMinX = m_iSpotX;
					dMinY = m_iSpotY;
				}
				else
				{
					iMaxJ = j;
					dMaxX = m_iSpotX;
					dMaxY = m_iSpotY;
				}
			}
			QCustomEvent* pEvent = new QCustomEvent( X_SPOT_EVENT );
			pEvent->setData( (void*) m_pXLinearRegression );
			QThread::postEvent( this, (QEvent*) pEvent );
		}
		m_pPanel->setAxisX( m_pXLinearRegression->getAxis() );
		m_pPanel->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 );
				if( dDx >= 0.0)
					m_pPanel->setConversionX( dSteps / dPixels );
				else
					m_pPanel->setConversionX( -1.0 * dSteps / dPixels );
				qDebug("Conversion X: %f %f %f", dDx, dDy, m_pPanel->getConversionX());
			}
			else
			{
				m_pPanel->setConversionX( 0.0 );
			}

			m_dXConversion = m_pPanel->getConversionX();
			QCustomEvent* pEvent = new QCustomEvent( X_CONV_EVENT );
			QThread::postEvent( this, (QEvent*) pEvent );
    }

		iMinJ = -4000;
		iMaxJ = -4000;
		m_pYLinearRegression->reset();
		for( int j= -2700; j<=2700; j+=300)
		{
			processSpot( 0, j );
			if( m_qlSpotList.count() == 1)
				m_qlLineList.append( new VideoSpot( m_iSpotX, m_iSpotY ) );
			m_pYLinearRegression->addPoint( m_iSpotX, m_iSpotY );
			if( m_iSpotX != -1 )
			{
				if ( iMinJ < -3000 )
				{
					iMinJ = j;
					dMinX = m_iSpotX;
					dMinY = m_iSpotY;
				}
				else
				{
					iMaxJ = j;
					dMaxX = m_iSpotX;
					dMaxY = m_iSpotY;
				}
			}
			QCustomEvent* pEvent = new QCustomEvent( Y_SPOT_EVENT );
			pEvent->setData( (void*) m_pYLinearRegression );
			QThread::postEvent( this, (QEvent*) pEvent );
		}
		m_pPanel->setAxisY( m_pYLinearRegression->getAxis() );
		m_pPanel->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 );
				if( dDx >= 0.0)
					m_pPanel->setConversionY( dSteps / dPixels );
				else
					m_pPanel->setConversionY( -1.0 * dSteps / dPixels );
				qDebug("Conversion Y: %f %f %f", dDx, dDy, m_pPanel->getConversionY());
			}
			else
				m_pPanel->setConversionY( 0.0 );

			m_dYConversion = m_pPanel->getConversionY();
			QCustomEvent* pEvent = new QCustomEvent( Y_CONV_EVENT );
			QThread::postEvent( this, (QEvent*) pEvent );
   	}

		processSpot( 0, 0 );

	}
	catch( AMCError& e)
	{
		qsMsg.sprintf( "Error while processing panel : (%d,%d)",
									 m_pPanel->i(), m_pPanel->j());
		qsMsg += e.formatError( 2 );
		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 );
		 		m_pPanel->setLaserOn( 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.
			}
		}
	}
			
	QThread::postEvent( this, new QCustomEvent( THREAD_END_EVENT ) );
}


/** Process Spot */
void CalibratePanelDialog::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_pPanel->getX();
	iY = p_iY - m_pPanel->getY();
	m_pMotor->moveMotors( iX, iY );
	m_pMotor->waitForMotors( m_pMotor->calcTimeout( MAX( abs(iX), abs(iY) ) ) + 1 );			
	m_pPanel->setX( p_iX );
	m_pPanel->setY( p_iY );
	
//	m_pMotor->switchLaser( true );
//	m_pPanel->setLaserOn( true );

  m_pVideoFrame->halt( true );
	msleep(100);
  int iRes = m_pFG->grabFrame( m_pData );
  m_pVideoFrame->halt( false );

//	m_pMotor->switchLaser( false );
//	m_pPanel->setLaserOn( false );

	outputProgress( "Finding spot" );

	subtractDark();
	findSpot( m_iSpotX, m_iSpotY );
	qsMsg.sprintf("Spot at: (%d,%d)", m_iSpotX, m_iSpotY );
	outputProgress( qsMsg );

	PanelProgress->setProgress( PanelProgress->progress()+1 );

}

/** No descriptions */
void CalibratePanelDialog::subtractDark()
{
	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;
	}
}

/** Find the Laser spot */
void CalibratePanelDialog::findSpot( int& p_iX, int& p_iY )
{
  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_iX = pSpot->getX();
		p_iY = pSpot->getY();
		QThread::postEvent( this, new QCustomEvent( FRAME_EVENT ) );
	}
	else
	{
		p_iX = -1;
		p_iY = -1;
	}
}

/** No descriptions */
int CalibratePanelDialog::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 + m_dTSigmas * stddev);
//	qDebug("Mean: %f StdDev: %f   Threshold %d", mean, stddev, threshold );
	return threshold;
}

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

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

void CalibratePanelDialog::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() == X_SPOT_EVENT )
	{
		QString info;
		LinearRegression* pRegress = (LinearRegression*) e->data();
		XAxisEdit->setText( info.sprintf( "%7.2f", pRegress->getAxis() ) );
		XSlopeEdit->setText( info.sprintf( "%7.2f", pRegress->getSlope() ) );
	}

  if ( e->type() == Y_SPOT_EVENT )
	{
		QString info;
		LinearRegression* pRegress = (LinearRegression*) e->data();
		YAxisEdit->setText( info.sprintf( "%7.2f", pRegress->getAxis() ) );
		YSlopeEdit->setText( info.sprintf( "%7.2f", pRegress->getSlope() ) );
	}

  if ( e->type() == X_CONV_EVENT )
	{
		QString info;
		XConvEdit->setText( info.sprintf( "%7.3f", m_dXConversion ) );
	}

  if ( e->type() == Y_CONV_EVENT )
	{
		QString info;
		YConvEdit->setText( info.sprintf( "%7.3f", m_dYConversion ) );
	}

  if ( e->type() == PANEL_PROGRESS_EVENT )
		PanelProgress->setProgress( PanelProgress->progress()+1 );

  if ( e->type() == FRAME_EVENT )
		setSpotInfoSlot( m_iSpotX, m_iSpotY );

  if ( e->type() == THREAD_END_EVENT )
	{
		buttonStart->setEnabled( true );
		if( (g_iRefX != -1) && (g_iRefY != -1) )
		buttonGoto->setEnabled( true );
		buttonOk->setEnabled( true );
	}
}

/** No descriptions */
void CalibratePanelDialog::gotoRefSlot()
{
	if( ( g_iRefX == -1 ) && ( g_iRefY == -1 ) )
		return;
	if( ( m_iVSpotX == -1 ) && ( m_iVSpotY == -1 ) )
		return;
	qDebug("Spot at x,y: %4d %4d", m_iVSpotX, m_iVSpotY );	
	qDebug("Ref  at x,y: %4d %4d", g_iRefX, g_iRefY );	
	double dX = g_iRefX - m_iVSpotX;
	double dY = g_iRefY - m_iVSpotY;
	qDebug("Dx Dy: %4.0f %4.0f", dX, dY );	
	double dAlpha1 = atan( m_pPanel->getSlopeX() );
//	if( m_pPanel->getSlopeX() < 0.0)
//		dAlpha1 *= -1;
	double dAlpha2 = atan( m_pPanel->getSlopeY() );
//	if( m_pPanel->getSlopeY() < 0.0)
//		dAlpha2 *= -1;
	qDebug("alpha1, alpha2: %7.2f %7.2f", dAlpha1 * 180.0 / 3.1415, dAlpha2 * 180.0 / 3.1415 );	
	double dSinA1 = sin( dAlpha1 );
	double dSinA2 = sin( dAlpha2 );
	double dCosA1 = cos( dAlpha1 );
	double dCosA2 = cos( dAlpha2 );

	double dFacX = dX - ( dY * dCosA2 / dSinA2 );
	dFacX /= ( dCosA1 - dSinA1 * dCosA2 / dSinA2 );
	double dFacY = (dY - dFacX * dSinA1) / dSinA2;
	qDebug("Factor x,y: %8.2f %8.2f", dFacX, dFacY );	

	int iStepX = (int) ( dFacX * m_pPanel->getConversionX() );
	int iStepY = (int) ( dFacY * m_pPanel->getConversionY() );

	qDebug("Steps till reference: %4d %4d", iStepX, iStepY );	
	gotoPosition( iStepX, iStepY );
}

/** No descriptions */
void CalibratePanelDialog::gotoPosition( int p_iX, int p_iY ){
	QString qsMsg;
  	
	try
	{
	  m_pMotor->unselectBox();
		int iBox = m_pPanel->box();
		m_pMotor->selectBox( iBox );
	 	int iDriver = m_pPanel->driver();
		m_pMotor->selectDriver( iDriver );	
		int iFrequency = m_pMotor->getFrequency();

		m_pMotor->moveMotors( p_iX, p_iY );
		m_pMotor->waitForMotors( m_pMotor->calcTimeout( MAX( abs(p_iX), abs(p_iY) ) ) + 1 );			
		m_pPanel->setX( m_pPanel->getX() + p_iX );
		m_pPanel->setY( m_pPanel->getY() + p_iY );
	}
	catch( AMCError& e)
	{
		qsMsg.sprintf( "Error while processing panel : (%d,%d)",
									 m_pPanel->i(), m_pPanel->j());
		qsMsg += e.formatError( 2 );
		outputError( qsMsg );
	}
			
}

void CalibratePanelDialog::setTSigmas( double p_dTSigmas )
{
	m_dTSigmas = p_dTSigmas;
	m_pVideoFrame->threshold( p_dTSigmas );
};
