// 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: %5.1f,%5.1f",
//				       		   m_dSpotX, m_dSpotY );
//			QThread::postEvent( m_pReceiver, new ThreadEvent( qsMsg ) );

    	if( m_dSpotX != -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
		getSpot();
		int x,y;
		calcSteps( x, y );

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

		double dRefX, dRefY;
		m_pPanel->getCorrectedRef( dRefX, dRefY );
		double dX, dY;
		dX = dRefX - m_dSpotX;
		dY = dRefY - m_dSpotY;
		
		if( (fabs(dX) > 1.5) || (fabs(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_dCenterX = m_dCenterY = 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(), pActualSpot->getNumPixel()) );
  }
  if( iNumLEDs != 0)
	{
		m_dCenterX = dX / iNumLEDs;
		m_dCenterY = dY / iNumLEDs;
	}
//  qDebug("Center at: %5.1f %5.1f -- with %d LEDs",m_dCenterX, m_dCenterY, iNumLEDs );
}

/** No descriptions */
void LaserAdjustThread::getDistToRef( double& p_dX, double& p_dY )
{
	double dRefX;
	double dRefY;
	m_pPanel->getCorrectedRef( dRefX, dRefY );

	// Adapt spot for shift of camera center.
	double dDx, dDy;
  dDx = m_dCenterX - CAMERA_X;
  dDy = m_dCenterY - CAMERA_Y;
  m_dSpotX -= dDx;
  m_dSpotY -= dDy;
//	qDebug("Spot at x,y: %5.1f %5.1f", m_dSpotX, m_dSpotY );

	// Now we calculate the distance of the real spot to the stored reference position
	p_dX = dRefX - m_dSpotX;
	p_dY = dRefY - m_dSpotY;
//	qDebug("Dx Dy: %5.1f %5.1f", p_dX, p_dY );
}

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

	getDistToRef( dX, dY );
	m_pPanel->calcSteps( dX, dY, p_iX, p_iY );

	qDebug("Steps till reference: %4d %4d", p_iX, p_iY );
  QCString qsMsg;
	qsMsg.sprintf( "%3d %3d %5.1f %5.1f %5.1f %5.1f %5.1f %5.1f %5.1f %5.1f %5d %5d",
				 				 m_pPanel->i(), m_pPanel->j(),
								 m_dCenterX, m_dCenterY,
								 m_dSpotX, m_dSpotY,
				  			 m_pPanel->getLaserX(), m_pPanel->getLaserY(),
							 	 dX, 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_dSpotX = pSpot->getX();
		m_dSpotY = pSpot->getY();
	}
	else
	{
		m_dSpotX = -1.;
		m_dSpotY = -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;
}

