/***************************************************************************
                          videoframe.cpp  -  description
                             -------------------
    begin                : Fri Nov 22 2002
    copyright            : (C) 2002 by 
    email                : 
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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 "amcdefs.h"
#include "amcframegrabber.h"
#include "calibratepaneldialog.h"
#include "linearregression.h"
#include "threadevent.h"
#include "videoframe.h"
#include "videospot.h"
#include "videospotthread.h"
#include "videothread.h"
#include <math.h>
#include <qframe.h>
#include <qfile.h>
#include <qpainter.h>
#include <qdatetime.h>
#include <qtextstream.h>
#include <qtimer.h>
#include <qimage.h>
#include <kapp.h>

extern KApplication*			g_theApp;
extern double g_dRefX, g_dRefY;

VideoFrame::VideoFrame( QWidget *parent, const char *name, AMCFrameGrabber* p_pFG )
	 : QFrame( parent, name )
{

  qDebug("Thread for VideoFrame PID: %d - PPID: %d", getpid(), getppid() );

  qDebug("Creating Image arrays");
	m_pImage = new QImage( MY_WIDTH, MY_HEIGHT, MY_DEPTH*8 );
	m_pRawImage = new QImage( MY_WIDTH, MY_HEIGHT, MY_DEPTH*8 );
	m_pDarkFrame = new QByteArray( BUFSIZE );

	m_iFrame = 0;
	m_iGoodFrame = 0;
	m_iDarks = 0;
	m_iFrameNum = 0;

	m_ix = m_iy = -1;
	m_dRefX = m_dRefY = -1.;
  m_dCenterX = m_dCenterY = -1.;
	m_pCalibDlg = 0;
	m_qlSpotList.setAutoDelete( true );
	m_qlSpots.setAutoDelete( true );
	
  qDebug("Creating VideoThread");
	m_pVideoThread = new VideoThread( this, p_pFG );
	m_pVideoThread->init( BUFSIZE );
  qDebug("Creating SpotThread");
	m_pSpotThread = new VideoSpotThread( this, m_pRawImage, (uchar*) m_pDarkFrame->data() );
  qDebug("Starting SpotThread");
	m_pSpotThread->start();
  qDebug("Starting VideoThread");
	m_pVideoThread->start();
}

VideoFrame::~VideoFrame()
{
	// Stop all runing threads
	halt( true );
	qDebug("deleting video thread");
	delete m_pVideoThread;
	qDebug("deleting spot thread");
	delete m_pSpotThread;

	qDebug("Deleting image and dark frame buffers");
	delete m_pDarkFrame;
	delete m_pImage;
	delete m_pRawImage;
}

/** overridden painter for the frame */
void VideoFrame::drawContents( QPainter* windowPainter )
{
//	qDebug("In drawContents");
	// Draw image
	QRect rect = contentsRect();
	windowPainter->drawImage( 0,0, *m_pImage );

	// Overlay camera center if it was determined from the LEDs
	if( m_dCenterX > 0 )
	{
		windowPainter->setPen( Qt::white );
		windowPainter->drawEllipse( m_dCenterX-2., m_dCenterY-2., 5, 5);
		windowPainter->drawEllipse( m_dCenterX-4., m_dCenterY-4., 9, 9);
	}

	// Overlay Spots
	windowPainter->setPen( Qt::red );
	for( VideoSpot* pActualSpot = m_qlSpots.first();
	  	 pActualSpot; pActualSpot = m_qlSpots.next() )
	{
		if( pActualSpot->isValid() )
			windowPainter->drawEllipse( pActualSpot->getX()-4, pActualSpot->getY()-4, 9, 9);
	}

	// Overlay Leds
	windowPainter->setPen( Qt::yellow );
	for( VideoSpot* pActualSpot = m_qlLEDs.first();
	  	 pActualSpot; pActualSpot = m_qlLEDs.next() )
	{
		if( pActualSpot->isValid() )
			windowPainter->drawRect( pActualSpot->getX()-4, pActualSpot->getY()-4, 9, 9);
	}
	windowPainter->drawEllipse( LED1_X-4, LED1_Y-4, 9, 9);
	windowPainter->drawEllipse( LED2_X-4, LED2_Y-4, 9, 9);
	windowPainter->drawEllipse( LED3_X-4, LED3_Y-4, 9, 9);
	windowPainter->drawEllipse( LED4_X-4, LED4_Y-4, 9, 9);

	// Draw the calibration points and the regression fits.
	if( m_pCalibDlg != 0 )
	{
		QList<VideoSpot>* pLineList = m_pCalibDlg->getLineList();
		windowPainter->setPen( Qt::magenta );
		for( VideoSpot* pActualSpot = pLineList->first();
	  		 pActualSpot; pActualSpot = pLineList->next() )
		{
			if( pActualSpot->isValid() )
				windowPainter->drawEllipse( pActualSpot->getX()-4, pActualSpot->getY()-4, 9, 9);
		}

		int iX1, iX2;
		windowPainter->setPen( Qt::yellow );
		LinearRegression* pRegression = m_pCalibDlg->getXRegression();
		if( pRegression->isValid() )
		{
  		if( ! pRegression->getReversed() ) {
				iX1 = (int) ((   0. - pRegression->getAxis()) / pRegression->getSlope());
				iX2 = (int) (( 479. - pRegression->getAxis()) / pRegression->getSlope());
  			windowPainter->drawLine( iX1, 0, iX2, 479 );
			}	
  		else
			{
				iX1 = (int) pRegression->getAxis();
				iX2 = (int) ( pRegression->getAxis() + 479 * pRegression->getSlope() );
  			windowPainter->drawLine( iX1, 0, iX2, 479 );	
			}
		}

		pRegression = m_pCalibDlg->getYRegression();
		if( pRegression->isValid() )
		{
  		if( ! pRegression->getReversed() )
			{
				iX1 = (int) ((   0. - pRegression->getAxis()) / pRegression->getSlope());
				iX2 = (int) (( 479. - pRegression->getAxis()) / pRegression->getSlope());
  			windowPainter->drawLine( iX1, 0, iX2, 479 );	
			}
  		else
			{
				iX1 = (int) pRegression->getAxis();
				iX2 = (int) ( pRegression->getAxis() + 479 * pRegression->getSlope() );
  			windowPainter->drawLine( iX1, 0, iX2, 479 );	
//				qDebug( "Drawing line: (%d,%d) - (%d,%d)", iX1, 0, iX2, 479 );
			}
		}
		
	}
	
	// Draw a blue cross for the global reference point
	windowPainter->setPen( Qt::blue );
	windowPainter->drawLine( g_dRefX-4., g_dRefY, g_dRefX+4., g_dRefY);
	windowPainter->drawLine( g_dRefX, g_dRefY-4., g_dRefX, g_dRefY+4.);

	// Draw a green rectangle showing the reference point for this panel
	if( m_dRefX > 0)
	{
		// Draw a green rectangle showing the reference point for this panel
		windowPainter->setPen( Qt::green );
		if( m_dCenterX > 1.0 )
		{
			// Adapt spot for shift of camera center.
			double dDx, dDy;
			dDx = m_dCenterX - CAMERA_X;
			dDy = m_dCenterY - CAMERA_Y;
			windowPainter->drawRect( m_dRefX+dDx-4., m_dRefY+dDy-4., 9., 9. );
		} else
			windowPainter->drawRect( m_dRefX-4., m_dRefY-4., 9., 9. );
	}

	// Draw a cyan rectangle showing the corrected reference point for this panel
	if( m_dRef1X > 0)
	{
		windowPainter->setPen( Qt::cyan );
		if( m_dCenterX > 1.0 )
		{
			// Adapt spot for shift of camera center.
			double dDx, dDy;
			dDx = m_dCenterX - CAMERA_X;
			dDy = m_dCenterY - CAMERA_Y;
			windowPainter->drawRect( m_dRef1X+dDx-4., m_dRef1Y+dDy-4., 9., 9. );
		} else
			windowPainter->drawRect( m_dRef1X-4., m_dRef1Y-4., 9., 9. );

	}

	// Draw a blue rectangle showing the Region Of Interest (ROI)
	// which is used to find the spot
	if( ! m_qrROI.isEmpty() )
	{
		windowPainter->setPen( Qt::blue );
		windowPainter->drawRect( m_qrROI );
	}

}

/** No descriptions */
void VideoFrame::saveFrame()
{
	QString fileName;
	fileName.sprintf("Frame%02d.png",m_iFrameNum);
	m_pImage->save( fileName, "PNG");

	fileName.sprintf("Spots%02d.txt",m_iFrameNum++);
	QFile file( fileName );
	if( file.open( IO_WriteOnly ) )
	{
		QTextStream stream( &file );

		QString str;

		str.sprintf("LEDs\n" );
  	stream << str;
		for( VideoSpot* pActualSpot = m_qlLEDs.first();
		  	 pActualSpot; pActualSpot = m_qlLEDs.next() )
		{
			if( pActualSpot->isValid() )
			{
				str.sprintf("%6.2f %6.2f %d\n", pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel() );
				stream << str;
			}
		}

		str.sprintf("\nSpots\n" );
  	stream << str;
		for( VideoSpot* pActualSpot = m_qlSpots.first();
		  	 pActualSpot; pActualSpot = m_qlSpots.next() )
		{
			if( pActualSpot->isValid() )
			{
				str.sprintf("%6.2f %6.2f %d\n", pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel() );
				stream << str;
			}
		}

	}
}

/** Save a dark frame */
void VideoFrame::takeDark()
{
	qDebug("Taking dark frame");
	uchar* pFrame = m_pImage->bits();
	m_pVideoThread->getLastFrame( pFrame );
	uchar* pDark = m_pDarkFrame->data();

	for(int ind=0; ind< (MY_HEIGHT * MY_WIDTH * MY_DEPTH); ind++, pDark++, pFrame++ )
	{
		int help = *pDark;
		help = (help * m_iDarks) + (int) *pFrame;
		*pDark = (uchar) (help / (m_iDarks+1));
	}
	m_iDarks++;
}

/** Clear dark frames */
void VideoFrame::clearDarks()
{
	m_pDarkFrame->fill(0);
	m_iDarks = 0;
}

/** No descriptions */
void VideoFrame::timerDone()
{
//	qDebug("Frames per second %d %d", m_iFrame, m_iGoodFrame);
	emit framesGrabbed( m_iFrame );
	emit framesProcessed( m_iGoodFrame );
	m_iFrame = 0;
	m_iGoodFrame=0;
}

/** No descriptions */
void VideoFrame::mouseMoveEvent( QMouseEvent* event )
{
	if( (event->state() & LeftButton) == 0 )
		return;

	int x = event->x();
	int y = event->y();
	if( x > m_qrROI.left() )
		m_qrROI.setRight( x );
	else
	{
		if( x == m_qrROI.left() )
			m_qrROI.setRight( event->x() -1 );
		else
			m_qrROI.setLeft( x );
	}
	if( y > m_qrROI.top() )
		m_qrROI.setBottom( y );
	else
	{
		if( y == m_qrROI.bottom() )
			m_qrROI.setBottom( event->y() -1 );
		else
			m_qrROI.setTop( y );
	}
	if ( m_qrROI.isValid() );
	m_pSpotThread->setROI( m_qrROI );
}

/** No descriptions */
void VideoFrame::mousePressEvent( QMouseEvent* event )
{
	if( event->button() == LeftButton )
	{
		m_qrROI.setLeft( event->x() );
		m_qrROI.setRight( event->x() -1 );
		m_qrROI.setTop( event->y() );
		m_qrROI.setBottom( event->y() -1 );
		m_pSpotThread->setROI( m_qrROI );
	}
	if( event->button() == MidButton )
	{
		m_qrROI.setLeft( 0 );
		m_qrROI.setRight( -1 );
		m_qrROI.setTop( 0 );
		m_qrROI.setBottom( -1 );
		m_pSpotThread->setROI( m_qrROI );		
	}
	if( event->button() == RightButton )
	{
		g_dRefX = event->x();
		g_dRefY = event->y();
		emit gref( g_dRefX, g_dRefY );
	}
}

void VideoFrame::customEvent(QCustomEvent *e)
{
  if ( e->type() == THREAD_SPOT_EVENT )
	{
		g_theApp->processEvents();
    m_pSpotThread->getSpotList( &m_qlSpotList );
		m_pSpotThread->getFrame( m_pImage->bits() );
		m_iGoodFrame++;
		repaint( FALSE );

    calcCenter();
		if( m_qlSpots.count() == 1)
		{
			VideoSpot* pSpot = m_qlSpots.first();
			emit spot( pSpot->getX(), pSpot->getY() );
		}
		else
			emit spot( -1, -1 );
		g_theApp->processEvents();

	}
  if ( e->type() == THREAD_FRAME_EVENT )
	{
		m_iFrame++;
		if( ! m_pSpotThread->isActive() )	
		{
			m_pVideoThread->getLastFrame( m_pRawImage->bits() );
			m_pSpotThread->wake();
		}
//else
//	qDebug("Dropping Frame");
		g_theApp->processEvents();
	}
}

/** No descriptions */
void VideoFrame::threshold( int p_iValue )
{
	m_pSpotThread->setSigmas( p_iValue );	
}

/** No descriptions */
void VideoFrame::scaleValue( int p_iValue )
{
	m_pSpotThread->setScalingMax( p_iValue );		
}

void VideoFrame::halt( bool p_zStop )
{
	if ( p_zStop == true )
 	{
		qDebug("Stoping video thread");
		m_pVideoThread->stop();
		m_pVideoThread->wait();		// Wait for video thread to finish
		qDebug("Stoping videospot thread");
		m_pSpotThread->stop();
		m_pSpotThread->wake();
		m_pSpotThread->wait();		// Wait for spot thread to finish
	}
	else
	{
		qDebug("Starting video thread");
		m_pSpotThread->start();
		qDebug("Starting videospot thread");
		m_pVideoThread->start();
	}
}

/** Calculate the location of the central pixel. We do this by identifiying the
    position of the camera LEDs and getting the relative offsets. */
void VideoFrame::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_qlLEDs.clear();
  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;
			m_qlLEDs.append( new VideoSpot( pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel()) );
      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;
			m_qlLEDs.append( new VideoSpot( pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel()) );
      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;
			m_qlLEDs.append( new VideoSpot( pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel()) );
      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;
			m_qlLEDs.append( new VideoSpot( pActualSpot->getX(), pActualSpot->getY(), pActualSpot->getNumPixel()) );
      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 %d %d", m_iCenterX, m_iCenterY);
	}
//  qDebug("Center at: %5.1f %5.1f -- with %d LEDs",m_dCenterX, m_dCenterY, iNumLEDs );
}