/******************************************************************************
  * \attention
  *
  * <h2><center>&copy; COPYRIGHT 2020 STMicroelectronics</center></h2>
  *
  * Licensed under ST MYLIBERTY SOFTWARE LICENSE AGREEMENT (the "License");
  * You may not use this file except in compliance with the License.
  * You may obtain a copy of the License at:
  *
  *        www.st.com/myliberty
  *
  * Unless required by applicable law or agreed to in writing, software 
  * distributed under the License is distributed on an "AS IS" BASIS, 
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied,
  * AND SPECIFICALLY DISCLAIMING THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *
******************************************************************************/


/*
 *      PROJECT:   ST25R3916 firmware
 *      Revision: 
 *      LANGUAGE:  ISO C99
 */

/*! \file st25r3916_aat.c
 *
 *  \author 
 *
 *  \brief ST25R3916 Antenna Tuning 
 *
 * The antenna tuning algorithm tries to find the optimal settings for 
 * the AAT_A and AAT_B registers, which are connected to variable capacitors 
 * to tune the antenna matching.
 *
 */
 
 
/*
******************************************************************************
* INCLUDES
******************************************************************************
*/
#include "st25r3916_aat.h"
#include "utils.h"
#include "st_errno.h"
#include "st25r3916.h"
#include "st25r3916_com.h"
#include "platform.h"
#include "rfal_chip.h"


/*
******************************************************************************
* GLOBAL DEFINES
******************************************************************************
*/
#define ST25R3916_AAT_CAP_DELAY_MAX           10                  /*!< Max Variable Capacitor settle delay */

/*
******************************************************************************
* GLOBAL MACROS
******************************************************************************
*/
#define st25r3916AatLog(...)     /* platformLog(__VA_ARGS__) */   /*!< Logging macro */

/*
******************************************************************************
* LOCAL FUNCTION PROTOTYPES
******************************************************************************
*/
static ReturnCode aatHillClimb(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus);
static int32_t aatGreedyDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir);
static int32_t aatSteepestDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir, int32_t previousDir2);

static ReturnCode aatMeasure(uint8_t serCap, uint8_t parCap, uint8_t *amplitude, uint8_t *phase, uint16_t *measureCnt);
static uint32_t aatCalcF(const struct st25r3916AatTuneParams *tuningParams, uint8_t amplitude, uint8_t phase);
static ReturnCode aatStepDacVals(const struct st25r3916AatTuneParams *tuningParams,uint8_t *a, uint8_t *b, int32_t dir);


 

/*******************************************************************************/
ReturnCode st25r3916AatTune(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus)
{
    ReturnCode err;
    const struct st25r3916AatTuneParams *tp = tuningParams;
    struct st25r3916AatTuneResult *ts = tuningStatus;
    struct st25r3916AatTuneParams defaultTuningParams = 
    {
        .aat_a_min=0,
        .aat_a_max=255,
        .aat_a_start=127,
        .aat_a_stepWidth=32,
        .aat_b_min=0,
        .aat_b_max=255,
        .aat_b_start=127,
        .aat_b_stepWidth=32,

        .phaTarget=128,
        .phaWeight=2,
        .ampTarget=196,
        .ampWeight=1,

        .doDynamicSteps=true,
        .measureLimit=50,
    };
    struct st25r3916AatTuneResult defaultTuneResult;
    
    if ((NULL != tp) && (
          (tp->aat_a_min > tp->aat_a_max   )
       || (tp->aat_a_start < tp->aat_a_min )
       || (tp->aat_a_start > tp->aat_a_max )
       || (tp->aat_b_min > tp->aat_b_max   )
       || (tp->aat_b_start < tp->aat_b_min )
       || (tp->aat_b_start > tp->aat_b_max )
       ))
    {
        return ERR_PARAM;
    }

    if (NULL == tp)
    { /* Start from current caps with default params */
        st25r3916ReadRegister(ST25R3916_REG_ANT_TUNE_A, &defaultTuningParams.aat_a_start);
        st25r3916ReadRegister(ST25R3916_REG_ANT_TUNE_B, &defaultTuningParams.aat_b_start);
        tp = &defaultTuningParams;
    }

    if (NULL == ts){ts = &defaultTuneResult;}

    ts->measureCnt = 0; /* Clear current measure count */
 
    err = aatHillClimb(tp, ts);

    return err;
}

/*******************************************************************************/
static ReturnCode aatHillClimb(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus)
{
    ReturnCode  err = ERR_NONE;
    uint32_t f_min;
    int32_t direction, gdirection;
    uint8_t amp,phs;
    struct st25r3916AatTuneParams tp = *tuningParams; // local copy to obey const

    tuningStatus->aat_a = tuningParams->aat_a_start;
    tuningStatus->aat_b = tuningParams->aat_b_start;

    /* Get a proper start value */
    aatMeasure(tuningStatus->aat_a,tuningStatus->aat_b,&amp,&phs,&tuningStatus->measureCnt);
    f_min = aatCalcF(&tp, amp, phs);
    direction = 0;

    st25r3916AatLog("%d %d: %d***\n",tuningStatus->aat_a,tuningStatus->aat_b,f_min);

    do {
        direction = 0; /* Initially and after reducing step sizes we don't have a previous direction */
        do {
            /* With the greedy step below always executed aftwards the -direction does never need to be investigated */
            direction = aatSteepestDescent(&f_min, &tp, tuningStatus, direction, -direction);
            if (tuningStatus->measureCnt > tp.measureLimit)
            {
                err = ERR_OVERRUN;
                break;
            }
            do
            {
                gdirection = aatGreedyDescent(&f_min, &tp, tuningStatus, direction);
                if (tuningStatus->measureCnt > tp.measureLimit) {
                    err = ERR_OVERRUN;
                    break;
                }
            } while (0 != gdirection);
        } while (0 != direction);
        tp.aat_a_stepWidth /= 2U; /* Reduce step sizes */
        tp.aat_b_stepWidth /= 2U;
    } while (tp.doDynamicSteps && ((tp.aat_a_stepWidth>0U) || (tp.aat_b_stepWidth>0U)));
    
    return err;
}

/*******************************************************************************/
static int32_t aatSteepestDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir, int32_t previousDir2)
{
    int32_t i;
    uint8_t amp,phs;
    uint32_t f;
    int32_t bestdir = 0; /* Negative direction: decrease, Positive: increase. (-)1: aat_a, (-)2: aat_b */

    for (i = -2; i <= 2; i++)
    {
        uint8_t a = tuningStatus->aat_a , b = tuningStatus->aat_b;

        if ((0==i) || (i==-previousDir) || (i==-previousDir2))
        { /* Skip no direction and avoid going backwards */
            continue;
        }
        if (0U!=aatStepDacVals(tuningParams, &a, &b, i))
        { /* If stepping did not change the value, omit this direction */
            continue;
        }

        aatMeasure(a,b,&amp,&phs,&tuningStatus->measureCnt);
        f = aatCalcF(tuningParams, amp, phs);
        st25r3916AatLog("%d : %d %d: %d",i,a, b, f);
        if (f < *f_min)
        { /* Value is better than all previous ones */
            st25r3916AatLog("*");
            *f_min = f;
            bestdir = i;
        }
        st25r3916AatLog("\n");
    }
    if (0!=bestdir) 
    { /* Walk into the best direction */
        aatStepDacVals(tuningParams, &tuningStatus->aat_a, &tuningStatus->aat_b, bestdir);
    }
    return bestdir;
}

/*******************************************************************************/
static int32_t aatGreedyDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir)
{
    uint8_t amp,phs;
    uint32_t f;
    uint8_t a = tuningStatus->aat_a , b = tuningStatus->aat_b;

    if (0U != aatStepDacVals(tuningParams, &a, &b, previousDir))
    { /* If stepping did not change the value, omit this direction */
        return 0;
    }

    aatMeasure(a,b,&amp,&phs,&tuningStatus->measureCnt);
    f = aatCalcF(tuningParams, amp, phs);
    st25r3916AatLog("g : %d %d: %d",a, b, f);
    if (f < *f_min)
    { /* Value is better than previous one */
        st25r3916AatLog("*\n");
        tuningStatus->aat_a = a;
        tuningStatus->aat_b = b;
        *f_min = f;
        return previousDir;
    }
    
    st25r3916AatLog("\n");
    return 0;
}

/*******************************************************************************/
static uint32_t aatCalcF(const struct st25r3916AatTuneParams *tuningParams, uint8_t amplitude, uint8_t phase)
{
    /* f(amp, pha) = (ampWeight * |amp - ampTarget|) + (phaWeight * |pha - phaTarget|) */
    uint8_t ampTarget = tuningParams->ampTarget;
    uint8_t phaTarget = tuningParams->phaTarget;

    uint32_t ampWeight = tuningParams->ampWeight;
    uint32_t phaWeight = tuningParams->phaWeight;

    /* Temp variables to avoid MISRA R10.8 (cast on composite expression) */
    uint8_t ad = ((amplitude > ampTarget)  ? (amplitude - ampTarget) : (ampTarget - amplitude));
    uint8_t pd = ((phase > phaTarget)      ? (phase - phaTarget)     : (phaTarget - phase));

    uint32_t ampDelta = (uint32_t)ad;
    uint32_t phaDelta = (uint32_t)pd;

    return ((ampWeight * ampDelta) + (phaWeight * phaDelta));
}

/*******************************************************************************/
static ReturnCode aatStepDacVals(const struct st25r3916AatTuneParams *tuningParams,uint8_t *a, uint8_t *b, int32_t dir)
{
    int16_t aat_a = (int16_t)*a, aat_b = (int16_t)*b;
    
    switch (abs(dir))
    { /* Advance by steps size in requested direction */
        case 1:
            aat_a = (dir<0)?(aat_a - (int16_t)tuningParams->aat_a_stepWidth):(aat_a + (int16_t)tuningParams->aat_a_stepWidth);
            if(aat_a < (int16_t)tuningParams->aat_a_min){ aat_a = (int16_t)tuningParams->aat_a_min; }
            if(aat_a > (int16_t)tuningParams->aat_a_max){ aat_a = (int16_t)tuningParams->aat_a_max; }
            if ((int16_t)*a == aat_a) {return ERR_PARAM;}
            break;
        case 2:
            aat_b = (dir<0)?(aat_b - (int16_t)tuningParams->aat_b_stepWidth):(aat_b + (int16_t)tuningParams->aat_b_stepWidth);
            if(aat_b < (int16_t)tuningParams->aat_b_min){ aat_b = (int16_t)tuningParams->aat_b_min; }
            if(aat_b > (int16_t)tuningParams->aat_b_max){ aat_b = (int16_t)tuningParams->aat_b_max; }
            if ((int16_t)*b == aat_b) {return ERR_PARAM;}
            break;
        default:
            return ERR_REQUEST;
    }
    /* We only get here if actual values have changed. In all other cases an error is returned */
    *a = (uint8_t)aat_a; 
    *b = (uint8_t)aat_b;
    
    return ERR_NONE;

}

/*******************************************************************************/
static ReturnCode aatMeasure(uint8_t serCap, uint8_t parCap, uint8_t *amplitude, uint8_t *phase, uint16_t *measureCnt)
{
    ReturnCode err;

    *amplitude = 0; 
    *phase     = 0;

    st25r3916WriteRegister(ST25R3916_REG_ANT_TUNE_A, serCap);
    st25r3916WriteRegister(ST25R3916_REG_ANT_TUNE_B, parCap);

    /* Wait till caps have settled.. */
    platformDelay( ST25R3916_AAT_CAP_DELAY_MAX );
    
    /* Get amplitude and phase .. */
    err = rfalChipMeasureAmplitude(amplitude);
    if (ERR_NONE == err)
    {
        err = rfalChipMeasurePhase(phase);
    }
      
    if( measureCnt != NULL )
    {
        (*measureCnt)++;
    }
    return err;
}