/***************************************************************************
                          amcmirrorpanel.cpp  -  description
                             -------------------
    begin                : Sat Nov 23 2002
    copyright            : (C) 2002 by Martin Merck
    email                : merck@astro.uni-wuerzburg.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is part of the MAGIC control software for the AMC.       *
 *   The software is only intended for use in the MAGIC telescope project  *
 *   All rights remain with the author.					   *
 ***************************************************************************/

#include "amcmirrorpanel.h"
#include <math.h>
#include <qstring.h>

extern bool	    g_zInfinityFocus;

AMCMirrorPanel::AMCMirrorPanel( int i, int j, int iType, int iPort, int iBox, int iDriver)
		: m_i(i), m_j(j), m_iType(iType), m_iPort(iPort), m_iBox(iBox), m_iDriver(iDriver)
{
	m_zReversed = false;
	m_zLaserOn = false;
	m_zMotorPower = true;
	m_iX = m_iY = 0;
	m_iRefX = m_iRefY = 0;
	m_dLaserX = m_dLaserY = 0.0;
	m_dAxisX = m_dAxisY = 0.0;
	m_dSlopeX = m_dSlopeY = 0.0;
	m_dConversionX = m_dConversionY = 0.0;
}

AMCMirrorPanel::~AMCMirrorPanel(){
}


/**
 * Here we calculation a correction for the laser spot position.
 *
 * The correction is composed of to components.
 * The first component cimes from the parabolic shape of the mirror.
 * Each mirror has a different focal length for the roque lamp spot
 * depending on its radial distance from the center of the mirror.
 * Correction one corrects for the error we introduce by moving the
 * mirror spot to a fixed focal plane during the roque lamp focusing.
 *
 * The second correction is to take into account the different position
 * of the plane where the roque lamp focusing is done and the plaane of the
 * winston cones.
 *
 * All calculations are done in mm.
 * We return the correction in milimeters we hace to move the laser spot along
 * the projection of the mirror center vector onto the camera plane.
 * This coorection is half the movement of the mirror spot.
 */
double AMCMirrorPanel::calcCorrection()
{
	// First we define some constants used in the calculation
	const double kDistMirror = 17000.0;  		// Distance from camera to mirror center
																					// Nominal focal length for infinity focusing
	const double kDistRoque = 980000.0;     // Distance from the MAGIC telescope to the
																					// Roque lamp.
	const double kFocPlaneRoque = 300.1;    // change of focal plane for roque lamp focusing.
	const double kDistWinstonCones = 35.0;	// Distance from the plane of focusing ( white
																					// paper or styropor surface) to the PMT camera
																					// plane (front of winston cone surface)
  const double kDistInfinityFocus = 30.0; // Difference in focal plane for
																					// 10 km and infinity focusing.

	/*
   * First we calculate the distance of the center of this panel to the center of the
	 * mirror.
	 */
  double dMirrorRadius = sqrt( pow( m_i*1000.0, 2 ) + pow( m_j*1000.0, 2 ) );

	/*
   * dHeightMirror is the height of the center of the selected mirror panel over the
   * tangential plane to the parabolas vertex.
   * Using the equation for a parabola open to the right:
   *                         y^2 = 4ax
	 *    a is the distance from the vertex to the focal point), or focal length of the parabola.
   *
	 * This height is calculated to:
	 *                        x = y^2 / 4a;
   * or
	 *                        dHeightMirror = SQR( dMirrorRadius ) / dLatusRectum;
   *
   * where we used the latus rectum of the parabooa: dLatusRectum = 4 * a.
   *
   * See: http://mathworld.wolfram.com/Parabola.html
   */
	double dLatusRectum = 4.0 * kDistMirror;
	double dHeightMirror = pow( dMirrorRadius, 2. ) / dLatusRectum;

  /*
   * Alpha is the angle betweeen the optical axis and the connection line
	 * between the mirror center and the focal point of the parabola.
	 * (The focal point of the parabola)
   *
	 *
	 *                                             ____-\         -
	 *                                     ____----               ^
	 *                             ____----                   dMirrorRadius
	 *										 ____----      dAlpha                   v
	 *                 x------------------------------------|     -
	 *
   *                 <------------ kDistMirror ------------>
   *                                                   <--->   dHeightMirror
   */
   double dAlpha = atan( dMirrorRadius / ( kDistMirror - dHeightMirror ) );

	/*
	 * For the Roque Lamp focusing the ray from the roque lamp is seen at the panels
   * center under an angle dDelta.
	 *                   dDelta = atan( dMirrorRadius / (kDistRoque - dHeightMirror) );
	 *
   *                                                           ________-\         -
	 *                                           ________--------     __--          ^
	 *                           ________--------                 __--        dMirrorRadius
	 *					 ________--------      dDelta                __--     dGamma        v    	
   *   x-------------------------------------------------x--*-------------|       -
	 *
   *   <--------------------------- kDistRoque --------------------------->
   *                                                     <- dFocusRoque -->
   *                                                     		<-kDistMirror->
   *                                                                    <->   dHeightMirror
   *
   * The reflected ray intersects the optical axis at a focal distance dFocusRoque
   * which is longer then focal length of the parabola.
   * For a spherical mirror (Davis-Cotton design like CT1) this difference would be
   * 300.1 mm which is the distance we shift the camera plane back for the roque lamp
   * focusing.
   * The angle at which this reflected ray intersects the optical axis is:
	 *      dGamma = dAlpha - dDelta;
   *
   * For the parabola this distance is only correct for the vertex and bigger as farer the
   * mirror is away from the vertex. We calculate now the focal length at roque focusing
   * for this mirror dFocusRoque and the difference dDiffFocusRQ between the roque focusing
   * plane amd this focal length.
   */
	double dDelta = atan( dMirrorRadius / ( kDistRoque - dHeightMirror) );
 	double dGamma = dAlpha - dDelta;
  double dFocusRoque = (dMirrorRadius / tan( dGamma )) + dHeightMirror;
	double dDiffFocusRQ = dFocusRoque - kFocPlaneRoque - kDistMirror;

  /*
   * The correction we have to apply results from the error we made by forcing the spots of
   * the individual mirrors onto the focal spot of the vertex for the roque lamp distance.
	 *
   *                                                        ____-\              -
	 *                                                ____---- ..                 ^
	 *                                        ____----     ....                   |
	 *					                      ____----________________________            |
	 *                        ____----    |        ....              ^       dMirrorRadius
	 *                ____----            |    ....              dCorrection      |
	 *			  ____----        	          |....                      V            V
   *   x--------------------------------*---------------------------------|     -
   *
   *   <--------------------------- dFocusRoque -------------------------->
   *                                    <- kDistMirror + kFocPlaneRoque -->
   *   <---  dDiffFocusRQ ------------->                         <--------> dHeightMirror
   *   <-------------- dFocusRoque - dHeightMirror ------------->
   *
   * Using the rule of three we get
   *
	 *         dCorrection / dDiffFocusRQ = dMirrorRadius / ( dFocusRoque - dHeightMirror )
   * or:
   *         dCorrection = (dMirrorRadius / (dFocusRoque- dHeightMirror) ) * dDiffFocusRQ
   */
	double dCorrection1 = (dMirrorRadius / (dFocusRoque- dHeightMirror) ) * dDiffFocusRQ;

  /*
   * The second correction needs to be applied because we can not use the front of the
   * winston cones as reference plane but a plane defined py the styropor panel put on
   * top of the plexiglas window of the camera.
   * This reference plane is 35 mm (kDistWinstonCones) infornt of the real focal plane we
   * should have used.
   * Looking at the scetch below we forced the ray to follow the line with stars, but
   * should have focus the mirror in such a way that it would have followed the dashed
   * line.
	 *
   *                                                        ____--\              -
	 *                                                ____---- ***                 ^
	 *                                        ____----     ****                   |
	 *					                      ____----         ****         ---   |
	 *                        ____----    |        ****              ^       dMirrorRadius
	 *                ____----            |    ****              dCorrection      |
	 *			  ____----        	          |****                      V            V
   *   x--------------------------------*---------------------------------|     -
   *
   *                                    <- kDistMirror + kFocPlaneRoque -->
   *   <----- kDistWinstonCones ------->                             <----> dHeightMirror
   *   																	<- dFocus - dHeightMirror -->
   *
   * Using the rule of three we get
   *
	 *         dCorrection / kDistWinstonCones = dMirrorRadius / ( dFocus - dHeightMirror - kDistWinstonCones )
   * or:
   *         dCorrection = (dMirrorRadius / (dFocus - dHeightMirror - kDistWinstonCones) ) * kDistWinstonCones
   */
  double dFocus = kDistMirror + kFocPlaneRoque;
  double dFocusShift;
  if ( g_zInfinityFocus )
	  dFocusShift = kDistWinstonCones + kDistInfinityFocus;
  else
	  dFocusShift = kDistWinstonCones;

  double dCorrection2 = (dMirrorRadius / (dFocus - dHeightMirror - dFocusShift) ) * dFocusShift;

	return ( (dCorrection1 + dCorrection2) / 2.0 );
}

/** No descriptions */
void AMCMirrorPanel::calcSteps( double p_dX, double p_dY, int& p_iStepsX, int& p_iStepsY )
{
	double dX = p_dX;
	double dY = p_dY;
	double dAlpha1 = atan( getSlopeX() );
	double dAlpha2 = atan( getSlopeY() );

	if( m_zReversed )
	{
		dX = p_dY;
		dY = p_dX;
//		dAlpha1 = atan( getSlopeY() );
//		dAlpha2 = atan( getSlopeX() );
	}
	qDebug("Dx Dy: %5.1f %5.1f", dX, dY );	
	qDebug("SlopeX, SlopeY: %7.4f %7.4f", getSlopeY(), getSlopeX() );	
	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_iStepsX = (int) ( dFacX * getConversionX() );
	p_iStepsY = (int) ( dFacY * getConversionY() );
	if( m_zReversed )
	{
		if(getSlopeX() < 0.0)
			p_iStepsX = (int) ( dFacX * -1.0 * getConversionX() );
		if(getSlopeY() < 0.0)
			p_iStepsY = (int) ( dFacY * -1.0 * getConversionY() );
	}

  // convert number of steps to an even number
	p_iStepsX = (p_iStepsX >> 1) << 1;
	p_iStepsY = (p_iStepsY >> 1) << 1;
}

/**
 * Get the Reference position corrected for the errors introduced by trying
 * to focus with the roque lamp at a finite distance nad using the wrong
 * plane for the focusing procedure
 */
void AMCMirrorPanel::getCorrectedRef( double& p_dRefX, double& p_dRefY )
{
	const double  kPixelperMMX = -0.44;
	const double  kPixelperMMY = 0.44;

	p_dRefX = getLaserX();
	p_dRefY = getLaserY();
	qDebug( "Laser spot position befor applying correction:   %f %f", p_dRefX, p_dRefY );

  double dCorrection = calcCorrection();
	/*
	 * The angle zeta is the angle of the mirror panel relative to the mirror center.
	 * 0 degrees corresponds to panels (0,x) with x being a positve number.
	 * The other angles are definedclockwise (positive) if looking onto the
	 * mirror plane from the camera.
	 */
	double dZeta = atan2( m_i * 1000.0, m_j * 1000.0 );

	/*	
	 * We assume that the video camera is horizontally alligned with mirror dish.
	 * If we have a rotation of the video image in respect to the mirror frame we
	 * must add an extra correction here.
	 * As the coordinate system of the video image has a reversed Y axis ( origin being
	 * the upper left corner) we have to multiply the Y correction with -1. to get the right
	 * sign.
	 */
	double dCorrSpotX =        sin( dZeta ) * dCorrection;
	double dCorrSpotY = -1.0 * cos( dZeta ) * dCorrection;

  p_dRefX += dCorrSpotX * kPixelperMMX;
  p_dRefY += dCorrSpotY * kPixelperMMY;
	qDebug( "Laser spot position after applying correction:   %f %f", p_dRefX, p_dRefY );
}
