/**
 * \file Ifx_AngleTrkF32.h
 * \brief Angle-tracking observer for sin/cos analog position sensor
 *
 *
 *
 * \version disabled
 * \copyright Copyright (c) 2013 Infineon Technologies AG. All rights reserved.
 *
 *
 *                                 IMPORTANT NOTICE
 *
 *
 * Use of this file is subject to the terms of use agreed between (i) you or 
 * the company in which ordinary course of business you are acting and (ii) 
 * Infineon Technologies AG or its licensees. If and as long as no such 
 * terms of use are agreed, use of this file is subject to following:


 * Boost Software License - Version 1.0 - August 17th, 2003

 * Permission is hereby granted, free of charge, to any person or 
 * organization obtaining a copy of the software and accompanying 
 * documentation covered by this license (the "Software") to use, reproduce,
 * display, distribute, execute, and transmit the Software, and to prepare
 * derivative works of the Software, and to permit third-parties to whom the 
 * Software is furnished to do so, all subject to the following:

 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer, must
 * be included in all copies of the Software, in whole or in part, and all
 * derivative works of the Software, unless such copies or derivative works are
 * solely in the form of machine-executable object code generated by a source
 * language processor.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.

 *
 * \defgroup library_srvsw_sysse_math_f32_angletrk Angle-Tracking Observer
 * This driver implements \ref library_srvsw_stdif_posif using 3rd order Angle-Tracking Observer algorithm.
 * \ingroup library_srvsw_sysse_math_f32
 *
 */

#ifndef IFX_ANGLETRKF32_H
#define IFX_ANGLETRKF32_H

//________________________________________________________________________________________
// INCLUDES

#include "StdIf/IfxStdIf_Pos.h"
#include "SysSe/Math/Ifx_LowPassPt1F32.h"

//________________________________________________________________________________________
// CONFIGURATION DEFINES

#ifndef IFX_CFG_ANGLETRKF32_SPEED_FILTER
#define IFX_CFG_ANGLETRKF32_SPEED_FILTER (1)
#endif

/** \brief Specify whether the speed reading is filtered by low-pass filter or not */
#ifndef IFX_CFG_ANGLETRKF32_FILTERED_SPEED
#define IFX_CFG_ANGLETRKF32_FILTERED_SPEED (1)
#endif

//________________________________________________________________________________________
// DATA STRUCTURES

/** \brief Angle Tracking Observer configuration */
typedef struct
{
    float32               kp;                /**< \brief Proportional gain */
    float32               ki;                /**< \brief Integrator gain */
    float32               kd;                /**< \brief Derivative gain */
    float32               speedLpfFc;        /**< \brief Cut-off frequency of speed low-pass filter. */
    float32               errorThreshold;    /**< \brief Threshold of error value in the tracking loop */
    sint32                sqrAmplMax;        /**< \brief Maximum value for square of signal amplitudes */
    sint32                sqrAmplMin;        /**< \brief Minimum value for square of signal amplitudes */
    uint16                periodPerRotation; /**< \brief Number of electrical periods per mechanical rotation */
    boolean               reversed;          /**< \brief TRUE: reversed direction, FALSE: straight direction */
    sint32                resolution;        /**< \brief Sensor resolution */
    IfxStdIf_Pos_RawAngle offset;            /**< \brief Offset in ticks. [0 .. (\ref Ifx_AngleTrkF32_Config.resolution - 1)] */
    sint16               *sinIn;             /**< \brief Pointer to SIN input variable */
    sint16               *cosIn;             /**< \brief Pointer to COS input variable */
} Ifx_AngleTrkF32_Config;

typedef struct
{
    float32 kp;                   /**< \brief Proportional gain */
    float32 ki;                   /**< \brief Integrator gain */
    float32 kd;                   /**< \brief Derivative gain */
    float32 errorThreshold;       /**< \brief Threshold of error value in the tracking loop */
    sint32  sqrAmplMax;           /**< \brief Maximum value for square of signal amplitudes */
    sint32  sqrAmplMin;           /**< \brief Minimum value for square of signal amplitudes */
} Ifx_AngleTrkF32_CfgData;

typedef struct
{
    IfxStdIf_Pos_RawAngle rawPosition;       /**< \brief raw position in ticks. \note: the value already contains the offset */
    sint32                turn;              /**< \brief number of mechanical turns. FIXME to be implemented */
    IfxStdIf_Pos_Dir      direction;         /**< \brief rotation direction */
    IfxStdIf_Pos_Status   status;            /**< \brief error code (0 = no error) */
    /* configuration */
    IfxStdIf_Pos_RawAngle offset;            /**< \brief raw position offset */
    boolean               reversed;          /**< \brief reverse direction */
    uint16                periodPerRotation; /**< \brief sensor 'electrical' periods per mechanical rotation */
    IfxStdIf_Pos_RawAngle resolution;        /**< \brief resolution of this position sensor interface */
    float32               Ts;                /**< \brief update period in seconds */
    float32               positionConst;     /**< \brief constant for calculating mechanical position (in rad) from raw position */
} Ifx_AngleTrkF32_PosIf;

/** \brief Angle Tracking Observer object */
typedef struct
{
    Ifx_AngleTrkF32_PosIf   base;
    Ifx_AngleTrkF32_CfgData cfgData;
    sint16                  *sinIn; /**< \brief Pointer to SIN input variable */
    sint16                  *cosIn; /**< \brief Pointer to COS input variable */
    float32                  halfTs;
    float32                  angleAtan;
    float32                  angleRef;
    float32                  angleEst;
    float32                  speedEstA;
    float32                  speedEstB;
    float32                  accelEst;
    float32                  angleErr;
    Ifx_LowPassPt1F32       speedLpf; /**< Only used if IFX_CFG_ANGLETRKF32_SPEED_FILTER is set */
} Ifx_AngleTrkF32;

/** \addtogroup library_srvsw_sysse_math_f32_angletrk
 * \{ */

//________________________________________________________________________________________
// PUBLIC FUNCTION PROTOTYPES

/** \name Initialisation functions
 * Example use:
 * \code
 * extern sint16    g_SinInput, g_CosInput;
 * Ifx_AngleTrkF32_Config  driverConfig;
 * Ifx_AngleTrkF32         driverData;
 * Ifx_AngleTrkF32_initConfig(&driverConfig, &g_SinInput, g_CosInput);
 * driverConfig.errorThreshold    = 5.0 / 180 * IFX_PI; // 5 degree error
 * driverConfig.sqrAmplMax        = 1.01 * 1.01;
 * driverConfig.sqrAmplMin        = 0.99 * 0.99;
 * driverConfig.speedLpfFc        = 100;
 * driverConfig.periodPerRotation = 1;
 * driverConfig.reversed          = FALSE;
 * driverConfig.offset            = 0;
 * Ifx_AngleTrkF32_init(&driverData, &driverConfig);
 * \endcode
 * Prototypes:
 * \{ */
IFX_EXTERN void Ifx_AngleTrkF32_init(Ifx_AngleTrkF32 *aObsv, const Ifx_AngleTrkF32_Config *config, float32 Ts);
IFX_EXTERN void Ifx_AngleTrkF32_initConfig(Ifx_AngleTrkF32_Config *config, sint16 *sinIn, sint16 *cosIn);
/** \} */

/** \name Protected functions
 * These functions are not for end-user but may be used by another driver which extends
 * the functionality of library_srvsw_sysse_math_f32_angletrk
 * \{ */
IFX_EXTERN float32 Ifx_AngleTrkF32_step(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn, float32 phase);
IFX_EXTERN void    Ifx_AngleTrkF32_updateStatus(Ifx_AngleTrkF32 *aObsv, sint16 sinIn, sint16 cosIn);
IFX_INLINE float32 Ifx_AngleTrkF32_getLoopSpeed(Ifx_AngleTrkF32 *aObsv);
/** \} */

/** \} */

/** \brief get the speed.
 * \param aObsv Pointer to the Ifx_AngleTrkF32 object
 */
IFX_INLINE float32 Ifx_AngleTrkF32_getLoopSpeed(Ifx_AngleTrkF32 *aObsv)
{
    return aObsv->speedEstB;   /* note: using speedEstB has better dynamic */
    /* return aObsv->speedLpf.out; */
}


/** \addtogroup library_srvsw_sysse_math_f32_angletrk
 * \{ */

/******************************************************************************/
/*-------------------------Global Function Prototypes-------------------------*/
/******************************************************************************/

/** \brief \see IfxStdIf_Pos_GetAbsolutePosition
 * \param driver driver handle
 * \return absolute position
 */
IFX_EXTERN float32 Ifx_AngleTrkF32_getAbsolutePosition(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetDirection
 * \param driver driver handle
 * \return direction
 */
IFX_EXTERN IfxStdIf_Pos_Dir Ifx_AngleTrkF32_getDirection(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetFault
 * \param driver driver handle
 * \return Fault
 */
IFX_EXTERN IfxStdIf_Pos_Status Ifx_AngleTrkF32_getFault(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetOffset
 * \param driver driver handle
 * \return offset address
 */
IFX_EXTERN sint32 Ifx_AngleTrkF32_getOffset(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetPeriodPerRotation
 * \param driver driver handle
 * \return Period per rotation
 */
IFX_EXTERN uint16 Ifx_AngleTrkF32_getPeriodPerRotation(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetPosition
 * \param driver driver handle
 * \return position
 */
IFX_EXTERN float32 Ifx_AngleTrkF32_getPosition(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetRawPosition
 * \param aObsv Pointer to the Ifx_AngleTrkF32 object
 * \return position in ticks
 */
IFX_EXTERN IfxStdIf_Pos_RawAngle Ifx_AngleTrkF32_getRawPosition(Ifx_AngleTrkF32 *aObsv);

/** \brief \see IfxStdIf_Pos_GetRefreshPeriod
 * \param driver driver handle
 * \return update period
 */
IFX_EXTERN float32 Ifx_AngleTrkF32_getRefreshPeriod(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetResolution
 * \param driver driver handle
 * \return resolution
 */
IFX_EXTERN sint32 Ifx_AngleTrkF32_getResolution(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetTurn
 * \param driver driver handle
 * \return Returns the number of turns
 */
IFX_EXTERN sint32 Ifx_AngleTrkF32_getTurn(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetSensorType
 * \param driver driver handle
 * \return sensor type
 */
IFX_EXTERN IfxStdIf_Pos_SensorType Ifx_AngleTrkF32_getSensorType(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_GetSpeed
 * \param aObsv Pointer to the Ifx_AngleTrkF32 object
 * \return speed
 */
IFX_EXTERN float32 Ifx_AngleTrkF32_getSpeed(Ifx_AngleTrkF32 *aObsv);

/** \brief \see IfxStdIf_Pos_Reset
 * \param driver driver handle
 * \return None
 */
IFX_EXTERN void Ifx_AngleTrkF32_reset(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_ResetFaults
 * \param driver driver handle
 * \return None
 */
IFX_EXTERN void Ifx_AngleTrkF32_resetFaults(Ifx_AngleTrkF32 *driver);

/** \brief \see IfxStdIf_Pos_SetOffset
 * \param aObsv driver handle
 * \param offset offset
 * \return None
 */
IFX_EXTERN void Ifx_AngleTrkF32_setOffset(Ifx_AngleTrkF32 *aObsv, IfxStdIf_Pos_RawAngle offset);

/** \brief \see IfxStdIf_Pos_SetRefreshPeriod
 * \param driver driver handle
 * \param updatePeriod update period
 * \return None
 */
IFX_EXTERN void Ifx_AngleTrkF32_setRefreshPeriod(Ifx_AngleTrkF32 *driver, float32 updatePeriod);

/** \brief \see IfxStdIf_Pos_Update
 * \param aObsv driver handle
 * \return None
 */
IFX_EXTERN void Ifx_AngleTrkF32_update(Ifx_AngleTrkF32 *aObsv);

/** \} */

/******************************************************************************/
/*-------------------------Global Function Prototypes-------------------------*/
/******************************************************************************/

/** \brief Initializes the standard interface "Pos"
 * \param stdif Standard interface position object
 * \param driver Virtual position sensor
 * \return TRUE on success else FALSE
 */
IFX_EXTERN boolean Ifx_AngleTrkF32_stdIfPosInit(IfxStdIf_Pos *stdif, Ifx_AngleTrkF32 *driver);

#endif /* IFX_ANGLETRKF32_H */
