// laseradjustthread.cpp: Implementierung der Klasse LaserThread.
//
//////////////////////////////////////////////////////////////////////

#include "laseradjustthread.h"
#include "motorthreaddialog.h"
#include "threadevent.h"
#include "amcdefs.h"
#include "amcframegrabber.h"
#include "amcmirrorpanel.h"
#include "amcmotor.h"
#include "amcserialport.h"
#include "amcerror.h"
#include "amcerrors.h"
#include "videospot.h"
#include <qmessagebox.h>
#include <qevent.h>
#include <qthread.h>
#include <qstring.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

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

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

//////////////////////////////////////////////////////////////////////
// Konstruktion/Destruktion
//////////////////////////////////////////////////////////////////////

LaserAdjustThread::LaserAdjustThread( QObject* p_pReceiver )
	: m_pPanel( NULL ), m_pReceiver( p_pReceiver ), m_zOn( false )
{
	m_qlSpotList.setAutoDelete( true );
	m_qlSpots.setAutoDelete( true );
}

LaserAdjustThread::~LaserAdjustThread()
{
}

void LaserAdjustThread::run()
{
	QString qsMsg;
	qsMsg.sprintf( "Panel (%d,%d) at Port: %d, Box:%d, Driver:%d",
			           m_pPanel->i(), m_pPanel->j(),
                 m_pPanel->port(), m_pPanel->box(), m_pPanel->driver() );
	QThread::postEvent( m_pReceiver, new ThreadEvent( qsMsg ) );

	QTime t;
	t.start();                          // start clock

	// We use only one motor instance and reuse it for each panel
	AMCSerialPort* pPort = g_pPort[ m_pPanel->port()-1 ];
	AMCMotor* pMotor = new AMCMotor( pPort );

	try
	{
		pMotor->unselectBox();
    usleep(1000);
		pMotor->selectBox( m_pPanel->box() );
		pMotor->selectDriver( m_pPanel->driver() );
		pMotor->getFrequency();
		pMotor->switchLaser( true );
		m_pPanel->setLaserOn( true );

		for( int i=0; i<2; i++ )
		{
			getSpot();

//			qsMsg.sprintf( "Spot at: %d,%d",
//				       		   m_iSpotX, m_iSpotY );
//			QThread::postEvent( m_pReceiver, new ThreadEvent( qsMsg ) );

    	if( m_iSpotX != -1 )
    	{
				int x,y;
				calcSteps( x, y );

				int steps = MAX( abs(x), abs(y) );
				if( ! ((x == 0) && (y == 0)) )			// only move is necesary
				{
					// check that the movement is in the allowed range
          // of the motors (-2500 - 2500)
					if( ( abs(x + m_pPanel->getX()) > 2500 )
							||
						  ( abs(y + m_pPanel->getY()) > 2500 ) )
            break;

					pMotor->moveMotors(x,y);
					pMotor->waitForMotors( pMotor->calcTimeout(steps) );

					m_pPanel->setX( m_pPanel->getX() + x );
					m_pPanel->setY( m_pPanel->getY() + y );
				}
			}
    }

		// check if the laser adjustment converged
 		double dX,dY;
		getSpot();
		getDistToRef(dX, dY);

		pMotor->switchLaser( false );
		m_pPanel->setLaserOn( false );

		if( (dX > 1.5) || (dY > 1.5) )
		{
			AMCError aErr( IDS_ERROR_NO_CONVERGENCE );
			throw aErr;
		}
	}
	catch( AMCError& e )
  {

		qsMsg.sprintf( "AMCError: Panel (%d,%d) Port: %d, Box:%d, Driver:%d -- ",
			       				m_pPanel->i(), m_pPanel->j(),
										m_pPanel->port(), m_pPanel->box(), m_pPanel->driver() );
		QThread::postEvent( m_pReceiver, new ThreadErrorEvent( qsMsg, e ) );

    try
		{
			pMotor->resetBox();
//			pMotor->unselectBox();
			pMotor->selectBox( m_pPanel->box() );
			pMotor->selectDriver( m_pPanel->driver() );
			pMotor->switchLaser( false );
			m_pPanel->setLaserOn( false );
			pMotor->unselectBox();
		}
    catch( AMCError& e2 ){}
	}
	delete pMotor;


//	qsMsg.sprintf( "Time: %dms", t.elapsed() );
//	QThread::postEvent( m_pReceiver, new ThreadEvent( qsMsg ) );
	QThread::postEvent( m_pReceiver, new QCustomEvent( PROGRESS_EVENT ) );
}

void LaserAdjustThread::setPanel(AMCMirrorPanel* p_pPanel)
{
	m_pPanel = p_pPanel;
}

/** Calculate the location of the central pixel. We do this by identifiying the
    position of the camera LEDs and getting the relative offsets. */
void LaserAdjustThread::calcCenter()
{
  double dX = 0.0, dX1 = 0.0, dX2 = 0.0, dX3 = 0.0, dX4 = 0.0;
  double dY = 0.0, dY1 = 0.0, dY2 = 0.0, dY3 = 0.0, dY4 = 0.0;
  int iNumLEDs = 0;

  m_iCenterX = m_iCenterY = 1;
  m_qlSpots.clear();

	for( VideoSpot* pActualSpot = m_qlSpotList.first();
	  	 pActualSpot; pActualSpot = m_qlSpotList.next() )
	{
    // Check if this spot is compatible with the spot of the
		// upper left LED
    if ( pActualSpot->contains2( LED1_X, LED1_Y ) )
		{
			dX1 = pActualSpot->getX() + (CAMERA_X - LED1_X);
			dY1 = pActualSpot->getY() + (CAMERA_Y - LED1_Y);
			dX += dX1; dY += dY1;
      iNumLEDs++;
		}
    // Check if this spot is compatible with the spot of the
		// upper right LED
    else if ( pActualSpot->contains2( LED2_X, LED2_Y ) )
		{
			dX2 = pActualSpot->getX() + (CAMERA_X - LED2_X);
			dY2 = pActualSpot->getY() + (CAMERA_Y - LED2_Y);
			dX += dX2; dY += dY2;
      iNumLEDs++;
		}
    // Check if this spot is compatible with the spot of the
		// lower left LED
    else if ( pActualSpot->contains2( LED3_X, LED3_Y ) )
		{
			dX3 = pActualSpot->getX() + (CAMERA_X - LED3_X);
			dY3 = pActualSpot->getY() + (CAMERA_Y - LED3_Y);
			dX += dX3; dY += dY3;
      iNumLEDs++;
		}
    // Check if this spot is compatible with the spot of the
		// lower right LED
    else if ( pActualSpot->contains2( LED4_X, LED4_Y ) )
		{
			dX4 = pActualSpot->getX() + (CAMERA_X - LED4_X);
			dY4 = pActualSpot->getY() + (CAMERA_Y - LED4_Y);
			dX += dX4; dY += dY4;
      iNumLEDs++;
		}
    else
			m_qlSpots.append( new VideoSpot( pActualSpot->getX(), pActualSpot->getY()) );
  }
  if( iNumLEDs != 0)
	{
		m_iCenterX = (int) rint(dX / iNumLEDs);
		m_iCenterY = (int) rint(dY / iNumLEDs);
	}
//  qDebug("Center at: %d %d -- with %d LEDs",m_iCenterX, m_iCenterY, iNumLEDs );
}

/** No descriptions */
void LaserAdjustThread::getDistToRef( double& p_dX, double& p_dY )
{
	int iRefX = m_pPanel->getLaserX();
	int iRefY = m_pPanel->getLaserY();

	// Adapt spot for shift of camera center.
	int iDx, iDy;
  iDx = m_iCenterX - CAMERA_X;
  iDy = m_iCenterY - CAMERA_Y;
  m_iSpotX -= iDx;
  m_iSpotY -= iDy;
//	qDebug("Spot at x,y: %4d %4d", m_iSpotX, m_iSpotY );

	// Now we calculate the distance of the real spot to the stored reference position
	p_dX = (double) (iRefX - m_iSpotX);
	p_dY = (double) (iRefY - m_iSpotY);
//	qDebug("Dx Dy: %4.0f %4.0f", dX, dY );
}

/** No descriptions */
void LaserAdjustThread::calcSteps( int& p_iX, int& p_iY )
{
  double dX, dY;
	getDistToRef(dX, dY );

	double dAlpha1 = atan( m_pPanel->getSlopeX() );
	double dAlpha2 = atan( m_pPanel->getSlopeY() );
//	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 );

	p_iX = (int) ( dFacX * m_pPanel->getConversionX() );
	p_iY = (int) ( dFacY * m_pPanel->getConversionY() );

  // convert number of steps to an even number
	p_iX = (p_iX >> 1) << 1;
	p_iY = (p_iY >> 1) << 1;

	qDebug("Steps till reference: %4d %4d", p_iX, p_iY );
  QCString qsMsg;
	qsMsg.sprintf( "%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %5d %5d",
				 				 m_pPanel->i(), m_pPanel->j(),
								 m_iCenterX, m_iCenterY,
								 m_iSpotX, m_iSpotY,
				  			 m_pPanel->getLaserX(), m_pPanel->getLaserY(),
								 (int) rint(dX), (int) rint(dY),
								 p_iX, p_iY );
	QThread::postEvent( m_pReceiver, new ThreadEvent( qsMsg ) );
}

/** Find the Laser spot */
void LaserAdjustThread::getSpot()
{
	g_pFG->grabFrame( m_pData );
  int threshold = getThreshold();

	m_qlSpotList.clear();
	int top = LED2_Y - 20;
	int bottom = LED3_Y + 20;
	int left = LED1_X - 20;
	int right = LED4_X + 20;

  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 );
				}

	 		}
		}
	}
	calcCenter();
	if( m_qlSpots.count() == 1)
	{
		VideoSpot* pSpot = m_qlSpots.first();
		m_iSpotX = pSpot->getX();
		m_iSpotY = pSpot->getY();
	}
	else
	{
		m_iSpotX = -1;
		m_iSpotY = -1;
		if( m_qlSpots.count() == 0)
		{
			AMCError aErr( IDS_ERROR_NO_SPOT );
			throw aErr;
		}
		else
		{
			AMCError aErr( IDS_ERROR_MULTI_SPOT );
			throw aErr;
		}
	}
}

/** No descriptions */
int LaserAdjustThread::getThreshold()
{
	double sumx =0;
	double sumx2 = 0;

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

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

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

