/*************************************************************************
$Archive: /pacs/OnBoard/FWCtrl.c $
$Revision: 1.14 $  $Date: 2009/04/23 13:51:12 $  
$Author: amazy $ 
$Log: FWCtrl.c,v $
Revision 1.14  2009/04/23 13:51:12  amazy
6.029

 * 
 * 10    9/10/08 12:05 Amazy
 * 
 * 9     6/07/07 11:01a Amazy
 * v6.018
 * 
 * 8     5/03/07 4:02p Amazy
 * added thresholds for the FW and an additional parameter to specify the
 * direction of the move
 * 
 * 7     11/27/06 6:49p Amazy
 * syntax errors corrected
 * 
 * 6     27/11/06 9:59 Amazy
 * 
 * 5     27/11/06 9:58 Amazy
 * When doing a switch-on of a fw, the grating controller was marked as OFF but was not disabled !  This caused some problem while switching-on
 * the grating controller later (it was then enabled immediately !).  In the CommandSwitchOnFwSpec and CommandSwitchOnFwPhoto, the instruction
 * CLEAR_BIT(gpGratingPidController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON) has been replaced by DisableGratingController() (there is actually
 * no need to switch-off the grating controller)
 *
 * 34    3/14/06 4:31p Pacs Egse
 * Version 6.001
 * Cleaned and commented
*************************************************************************/

#ifdef SIMULATOR
#include "virtuosoSim.h"
#else // SIMULATOR
#include "v_macro.h"
#include "node1.h"
#include "k_struct.h"
#endif

#include "params.h"
#include "error.h"
#include <math.h>


/****************************************************************
GLOBAL VARIABLES
****************************************************************/
int gSinArray[NB_STEPS_IN_PHASE];
float gFloatSinArray[NB_STEPS_IN_PHASE];

/****************************************************************
FUNCTIONS DECLARATION
****************************************************************/

void MoveFwSteps(FwControllerParams* p_controller, int nbSteps);
void InitializeFwControllerParams(FwControllerParams* p_controller);
void ValidateFWSpecHk(int validity);
void ValidateFWPhotoHk(int validity);
void StopFWMove(FwControllerParams* p_controller);
void InitSinusArray();

extern void StopGratMove();
extern void DisableGratingController();

// commands function
BOOL CommandMoveFWPhoto(CommandParameter param);
BOOL CommandMoveFWPhotoToLocation(CommandParameter param);
BOOL CommandMoveFWSpec(CommandParameter param);
BOOL CommandMoveFWSpecToLocation(CommandParameter param);
BOOL CommandSwitchOnFWSpec(CommandParameter paramNotUsed);
BOOL CommandSwitchOnFWPhoto(CommandParameter paramNotUsed);


/****************************************************************
FUNCTIONS IMPLEMENTATION
****************************************************************/

/*
FUNCTION : void ValidateFWSpecHk(int validity)
**********************************************

AUTHOR : Amazy

Validates/invalidates the hk related to spectro FW
*/
void ValidateFWSpecHk(int validity)
{
	//the first time you power on a mechanism (FW or GRAT), you must validate the hall sensors
	if (validity)
	{
		VALIDATE_HK(DMC_FWGRAT_HALLA);
		VALIDATE_HK(DMC_FWGRAT_HALLB);
	}
}

/*
FUNCTION : void ValidateFWPhotoHk(int validity)
***********************************************

AUTHOR : Amazy

Validates/invalidates the hk related to photo FW
*/
void ValidateFWPhotoHk(int validity)
{
	//the first time you power on a mechanism (FW or GRAT), you must validate the hall sensors
	if (validity)
	{
		VALIDATE_HK(DMC_FWGRAT_HALLA);
		VALIDATE_HK(DMC_FWGRAT_HALLB);
	}
}


/*
FUNCTION : void InitSinusArray()
********************************

AUTHOR : Amazy

A sine table is used to control the FW, to control the grating in open loop and to generate chopper transition.
It is initialized here.
Sines are stored as values between [-32767 and 32767]
*/
void InitSinusArray()
{
	double sineTemp = 0.0;
	int i= 0;
	
	for (i=0; i < NB_STEPS_IN_PHASE; ++i)
	{
		sineTemp = (sin((double)i/NB_STEPS_IN_PHASE*6.283185));
		gSinArray[i] = (int)(sineTemp*32767);
		gFloatSinArray[i] = (float)((sineTemp - 1.0) / 2.0);
	}
}


/*
FUNCTION : void InitializeFwControllerParams(FwControllerParams* p_controller)
******************************************************************************

AUTHOR : Amazy

Initialise the FW controller

PARAMS:
p_controller: the FW controller to initialize
*/
void InitializeFwControllerParams(FwControllerParams* p_controller)
{
	p_controller->CurrentStep = 0;
	p_controller->NbStepsForThisMove = 0;

	p_controller->Rate = 100;
	p_controller->Scaling = 4096; 
	if (p_controller == gpFWPhotoController)
	{
		p_controller->SwitchThresholdControlA = 6725;
		p_controller->SwitchThresholdControlB = 7830;
	}
	else
	{
		p_controller->SwitchThresholdControlA = 8365;
		p_controller->SwitchThresholdControlB = 6175;
	}
	p_controller->SwitchThresholdStatusA = 2000;
	p_controller->SwitchThresholdStatusB = 2000;


	p_controller->CurrentPos = -1; //current position is unknown until we power the fw on
}


/*
FUNCTION : void MoveFwSteps(FwControllerParams* p_controller, int nbSteps)
**************************************************************************

AUTHOR : Amazy

Initialise the move of a FW

PARAMS:
p_controller:	the FW controller to move
nbSteps:		the amplitude of the move (given in 'number of steps in the sine array')

NOTE:
In a first time, the sine array was used only for the FW movement and was only 256 elements long.
Later, when we had to use it for grating open loop, we needed a more accurate sine array and
we have defined an array of 16384 values.
Therefore, one step for a FW is still 1/256th of the array = 64 steps in the 16384 array
*/
void MoveFwSteps(FwControllerParams* p_controller, int nbSteps)
{
	if (nbSteps >= 0)
	{
		p_controller->CurrentInc = +64;
		p_controller->NbStepsForThisMove = nbSteps;
	}
	else
	{
		p_controller->CurrentInc = -64;
		p_controller->NbStepsForThisMove = -nbSteps;
	}
	
	p_controller->InitPhasePos = atan2(gHallSensorB, gHallSensorA)*((double)MAX_STEPS_IN_PHASE)/6.283185;
	p_controller->InitPhasePos += NB_STEPS_IN_PHASE;
	p_controller->InitPhasePos %= NB_STEPS_IN_PHASE;
	p_controller->CurrentStep = 0;
	p_controller->IsrCounter = 0;
	p_controller->CurrentPosInPhaseA = p_controller->InitPhasePos;
	SET_BIT(p_controller->TaskStatus, K_BMASK_FW_CTRL_STATUS_MOVING);
}

/*
CMD_FUNCTION : BOOL CommandMoveFWSpec(CommandParameter param)
*************************************************************

AUTHOR : Amazy

USE :
Start moving the FWSpec

PARAMS :
Uint	: number of steps 
*/
BOOL CommandMoveFWSpec(CommandParameter param)
{
	if (!TEST_ONE_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
	{
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	//maximum 5 complete turn
	if ((param.Int > (5*6*256)) || (param.Int < (-5*6*256)))
	{
		SequencerSendNack(0, INVALID_PARAMETER);
		SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS);
		return FALSE;
	}

	MoveFwSteps(gpFWSpecController, param.Int);
	SequencerSendAck(DMC_MOVE_SPEC_FW_STEP);

	return TRUE;
};

/*
CMD_FUNCTION : BOOL CommandMoveFWSpecToLocation(CommandParameter param)
**********************************************************************

AUTHOR : Amazy

USE :
Start moving the FWSpec to a new location

PARAMS :
Uint	: new absolute position
*/
BOOL CommandMoveFWSpecToLocation(CommandParameter param)
{
	int nbStepsForMove = 0;
	
	if (!TEST_ONE_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
	{
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	if (param.Uint == 0)
	{
		SET_BIT(gpFWSpecController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_A);
		nbStepsForMove = 9*256;
		gpFWSpecController->SimulatedSwitchSensorA = 0x7FFF;
		gpFWSpecController->SimulatedSwitchSensorB = 0x0000;
	}
	else if (param.Uint == 1)
	{
		SET_BIT(gpFWSpecController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_B);
		nbStepsForMove = 9*256;
		gpFWSpecController->SimulatedSwitchSensorA = 0x0000;
		gpFWSpecController->SimulatedSwitchSensorB = 0x7FFF;
	}
	else if (param.Uint == 2)
	{
		SET_BIT(gpFWSpecController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_A);
		nbStepsForMove = -9*256;
		gpFWSpecController->SimulatedSwitchSensorA = 0x7FFF;
		gpFWSpecController->SimulatedSwitchSensorB = 0x0000;
	}
	else if (param.Uint == 3)
	{
		SET_BIT(gpFWSpecController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_B);
		nbStepsForMove = -9*256;
		gpFWSpecController->SimulatedSwitchSensorA = 0x0000;
		gpFWSpecController->SimulatedSwitchSensorB = 0x7FFF;
	}
	else
	{
		SequencerSendNack(0, INVALID_PARAMETER);
		SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS);
		return FALSE;
	}

	//move the FW by one turn.  It will stop as soon as it reaches the position it is searching
	MoveFwSteps(gpFWSpecController, nbStepsForMove);
	SequencerSendAck(DMC_MOVE_SPEC_FW_LOC);

	return TRUE;
};

/*
CMD_FUNCTION : BOOL CommandMoveFWPhoto(CommandParameter param)
*************************************************************

AUTHOR : Amazy

USE :
Start moving the FWPhoto

PARAMS :
Uint	: number of steps 
*/
BOOL CommandMoveFWPhoto(CommandParameter param)
{
	if (!TEST_ONE_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
	{
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	//maximum 5 turns
	if ((param.Int > (5*6*256)) || (param.Int < (-5*6*256)))
	{
		SequencerSendNack(0, INVALID_PARAMETER);
		SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS);
		return FALSE;
	}

	MoveFwSteps(gpFWPhotoController, param.Int);
	SequencerSendAck(DMC_MOVE_PHOTO_FW_STEP);

	return TRUE;
};

/*
CMD_FUNCTION : BOOL CommandMoveFWPhotoToLocation(CommandParameter param)
************************************************************************

AUTHOR : Amazy

USE :
Start moving the FWPhoto to a new location

PARAMS :
Uint	: new absolute position
*/
BOOL CommandMoveFWPhotoToLocation(CommandParameter param)
{
	int nbStepsForMove = 0;
	
	if (!TEST_ONE_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
	{
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	if (param.Uint == 0)
	{
		SET_BIT(gpFWPhotoController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_A);
		nbStepsForMove = 9*256;
		gpFWPhotoController->SimulatedSwitchSensorA = 0x7FFF;
		gpFWPhotoController->SimulatedSwitchSensorB = 0x0000;
	}
	else if (param.Uint == 1)
	{
		SET_BIT(gpFWPhotoController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_B);
		nbStepsForMove = 9*256;
		gpFWPhotoController->SimulatedSwitchSensorA = 0x0000;
		gpFWPhotoController->SimulatedSwitchSensorB = 0x7FFF;
	}
	else if (param.Uint == 2)
	{
		SET_BIT(gpFWPhotoController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_A);
		nbStepsForMove = -9*256;
		gpFWPhotoController->SimulatedSwitchSensorA = 0x7FFF;
		gpFWPhotoController->SimulatedSwitchSensorB = 0x0000;
	}
	else if (param.Uint == 3)
	{
		SET_BIT(gpFWPhotoController->TaskStatus, K_BMASK_FW_CTRL_STATUS_SEARCHING_POS_B);
		nbStepsForMove = -9*256;
		gpFWPhotoController->SimulatedSwitchSensorA = 0x0000;
		gpFWPhotoController->SimulatedSwitchSensorB = 0x7FFF;
	}
	else
	{
		SequencerSendNack(0, INVALID_PARAMETER);
		SetSequencerError(ERR_SEQUENCER_INVALID_PARAMETERS);
		return FALSE;
	}

	//move the FW by one turn.  It will stop as soon as it reaches the position it is searching
	MoveFwSteps(gpFWPhotoController, nbStepsForMove);
	SequencerSendAck(DMC_MOVE_PHOTO_FW_LOC);

	return TRUE;
};

/*
CMD_FUNCTION : BOOL CommandSwitchOnFWSpec(CommandParameter paramNotUsed)
************************************************************************

AUTHOR : Amazy

USE :
Switches On the FW Spec 
*/
BOOL CommandSwitchOnFWSpec(CommandParameter paramNotUsed)
{
	// configure Hk Mux to get the values from the FW Spec hall sensors
	// configure the drive to command the FW Spec motor
	SET_BITS(gpMim->ControlWord, K_BMASK_MIM_HK_MUX_SELECT_MASK | K_BMASK_MIM_DRIVE_SELECT_MASK, K_BMASK_MIM_HK_MUX_SELECT_FW_SPEC | K_BMASK_MIM_DRIVE_SELECT_FW_SPEC); 

	//ask the FPGA to get the value of fw spectr hall sensors
	SET_BITS(gpFPGA->ControlRegister, K_BMASK_TIMING_MUXADR, K_BMASK_TIMING_MUXADR_FWSPEC);
	WriteFpgaRegisters();

	//stop the other FW if it was moving 
	StopFWMove(gpFWPhotoController);
	StopGratMove();

	//update status
	SET_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	CLEAR_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	DisableGratingController();

	VALIDATE_HK(DMC_FWSP_CUR_POS);  // this value is validated the first time we power on the fw.  Afterwards, it remains valid all the time but it is updated only when the fw is powered on
	ValidateFWSpecHk(TRUE);
	ValidateFWPhotoHk(FALSE);

	SequencerSendAck(DMC_SWON_FW_SPEC);

	return TRUE;
};

/*
CMD_FUNCTION : BOOL CommandSwitchOnFWPhoto(CommandParameter paramNotUsed)
*************************************************************************

AUTHOR : Amazy

USE :
Switches On the FW Photo
*/
BOOL CommandSwitchOnFWPhoto(CommandParameter paramNotUsed)
{
	// configure Hk Mux to get the values from the FW Photo hall sensors
	// configure the drive to command the FW Photo motor
	SET_BITS(gpMim->ControlWord, K_BMASK_MIM_HK_MUX_SELECT_MASK | K_BMASK_MIM_DRIVE_SELECT_MASK, K_BMASK_MIM_HK_MUX_SELECT_FW_PHOTO | K_BMASK_MIM_DRIVE_SELECT_FW_PHOTO); 

	//ask the FPGA to get the value of fw photo hall sensors
	SET_BITS(gpFPGA->ControlRegister, K_BMASK_TIMING_MUXADR, K_BMASK_TIMING_MUXADR_FWPHOTO);
	WriteFpgaRegisters();

	//stop the other FW if it was moving 
	StopFWMove(gpFWSpecController);
	StopGratMove();

	//update status
	SET_BIT(gpFWPhotoController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	CLEAR_BIT(gpFWSpecController->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	DisableGratingController();

	VALIDATE_HK(DMC_FWPH_CUR_POS);  // this value is validated the first time we power on the fw.  Afterwards, it remains valid all the time but it is updated only when the fw is powered on
	ValidateFWSpecHk(FALSE);
	ValidateFWPhotoHk(TRUE);

	SequencerSendAck(DMC_SWON_FW_PHOTO);

	return TRUE;
};

/*
FUNCTION : void StopFWMove(FwControllerParams* p_controller)
************************************************************

AUTHOR : Amazy

Stop the move of a FW

PARAMS:
p_controller:	the FW controller to stop
*/
void StopFWMove(FwControllerParams* p_controller)
{
	//clear status bit
	CLEAR_BIT(p_controller->TaskStatus, K_BMASK_FW_CTRL_STATUS_MOVING);

	//reset the move
	p_controller->NbStepsForThisMove = 0;
	p_controller->CurrentStep = 0;

	//reset the DAC
	gpMim->GratFwDAC1 = 0;
	gpMim->GratFwDAC2 = 0;
}