//*****************************************************************************
// (C) Automotive Lighting Reutlingen GmbH
//     Tuebinger Strasse 123, 72762 Reutlingen, Germany
//
// Automotive Lighting Reutlingen GmbH owns all the rights to this work.
// This work shall not be copied, reproduced, used, modified, transferred
// or its information shall not be disclosed without the prior written
// authorization of Automotive Lighting Reutlingen GmbH.
//*****************************************************************************

//-----------------------------------------------------------------------------
/// \file  AvacData.c
///
/// \brief
///
/// \descr
///
///         Additional information can be found in the design description (Link:
///         <a href="file:///\\Dereu1nquest1\XXX_Docu\Gen\03_MDRD\MDDD_XXX.doc">MDDD</a>)
///
/// \author Alf Traulsen  (taf2rt)
///         mailTo:alf.traulsen@al-lighting.com
//-----------------------------------------------------------------------------
#define RTE_MICROSAR_PIM_EXPORT

#include <AL_Helper/AL_Helper.h>
#include "Std_Types.h"
#include <Afs/Afs.h>
#include <Avac/AvacData.h>
#include <Avac/AvacAlgDyn1.h>
#include <VehHub/RTE_GlobalType_temp.h>


#include "Rte_Type.h"
#undef RTE_APPLICATION_HEADER_FILE
#include "Rte_ctaaAvac.h"
#undef RTE_APPLICATION_HEADER_FILE



#define AvacData_WheelBase 2950
#define SensorCaliData 0


#define ctaaAvac_START_SEC_VAR_INIT_UNSPECIFIED
# include "ctaaAvac_MemMap.h"
// ----------------------------------------------------------------------------
// Variables
// ----------------------------------------------------------------------------
boolean               AvacData_boCodingValid         = TRUE;
boolean               AvacData_boPitchConfigValid    = TRUE;
uint8                 AvacData_ucGradientLimiterUp   = AVAC_StaticAngleGradFast;
uint8                 AvacData_ucGradientLimiterDown = AVAC_StaticAngleGradFast;

const tRomPara_AvacAlgoDynCfg*        CodBlockDavacPara    = NULL;      // active avac config data
const tRomPara_AvacAlgoGenCfg*        CodBlockAvacGenGfg   = NULL;


STATIC_AL tisAvacPosition       AvacPositionOutput   = { 0 };
STATIC_AL uint8                 AvacNVM_vehicleConfg[4] = {0,0,0,0};
STATIC_AL sint16                NvmSensorCaliData    = 0;
STATIC_AL uint16                VehicleWheelBase     = 0;

#define ctaaAvac_STOP_SEC_VAR_INIT_UNSPECIFIED
# include "ctaaAvac_MemMap.h"


//=============================================================================
// Functions
//=============================================================================
void Avac_GetInput(void);

static uint8 UaApplCalcPitchAngle(sint32 front_rear_delta, tPitchCan* pitch);
static sint16 CalcAcceleration(const uint16 unSpeed);

 ///////////////////////////////////////////////////////////////////////////////
 // Avac debug code, generate fake input
 //////////////////////////////////////////////////////////////////////////////

#define DEBUG_AVAC_DATA STD_ON

#if (DEBUG_AVAC_DATA == STD_OFF)
sint16 dummySpeed;
sint16 dummyAngle;
#define DEBUG_AVAC_MAX_STEP 1000
uint16 steps = 0;
#endif

#define ctaaAvac_START_SEC_CODE
# include "ctaaAvac_MemMap.h"

void AvacData_Init(void)
{
  tisCodingStatus CodingStatus;
  
  uint8 i;
  uint8 *data;
  data = Rte_Pim_PimCalibrationZeroLevel();
  for (i = 0; i < 4; i++)
  {
    AvacNVM_vehicleConfg[i] = *(data+i);
  }
  
  /*check pitch sensor calibration data valid, provided by CalibrationZeroLevel*/
  if ((AvacNVM_vehicleConfg[0] == NVM_AXIS_CALI_INVALID) ||
      (AvacNVM_vehicleConfg[1] == NVM_AXIS_CALI_INVALID) ||
      (AvacNVM_vehicleConfg[2] == NVM_AXIS_CALI_INVALID) ||
      (AvacNVM_vehicleConfg[3] == NVM_AXIS_CALI_INVALID))
  {
    AvacData_boPitchConfigValid = FALSE;
  }
  else
  {
    NvmSensorCaliData = (sint16)(((AvacNVM_vehicleConfg[0] + LEVELING_SENSER_OFFSET + AvacNVM_vehicleConfg[1] + LEVELING_SENSER_OFFSET) -
                                  (AvacNVM_vehicleConfg[2] + LEVELING_SENSER_OFFSET + AvacNVM_vehicleConfg[3] + LEVELING_SENSER_OFFSET)) >> 1);
     AvacData_boPitchConfigValid = TRUE;
  }

  //check coding valid
  (void)Rte_Read_PpareCodMCodingStatus_sCodingStatus(&CodingStatus);
  if (CodingStatus.eCodingStatus == E_OK)
  {
    VehicleWheelBase   = applicationCodingData.System.VehicleData.WheelBase;
    CodBlockDavacPara  = &applicationCodingData.System.AvacAlgoDynCfg;
    CodBlockAvacGenGfg = &applicationCodingData.System.AvacAlgoGenCfg;
    AvacData_boCodingValid = TRUE;
  }
  else
  {
    AvacData_boCodingValid = FALSE;
  }

}

//-----------------------------------------------------------------------------
/// \brief  AvacData_GetLowBeamOn
///
/// \descr  get low beam state
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
boolean AvacData_GetLowBeamOn(void)
{
  // if light is switched on physically, AVAC must be active.
  boolean     ret_result = FALSE;
  tisLightCmd AvacData_LightCmd;

  Rte_Call_PpaclVehHubLightCmd_ReadData(&AvacData_LightCmd);

  if ((AvacData_LightCmd.sLowerBeam.boValue) &&
      (SIG_VALID == AvacData_LightCmd.sLowerBeam.eQty))
  {
    ret_result = TRUE;
  }
  else
  {
    ret_result = FALSE;
  }

  if (AvacData_boCodingValid == FALSE) {
    AvacData_Init();    // check if calibration data written
  }

  return ret_result;
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_GetAvacS_Ok()
///
/// \descr  static AVAC OK
///
/// \param  -
///
/// \return OK
//-----------------------------------------------------------------------------
boolean AvacData_GetAvacS_Ok()
{
  return TRUE;
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_GetAvacS_En()
///
/// \descr  static AVAC enable
///
/// \param  -
///
/// \return enable
//-----------------------------------------------------------------------------
boolean AvacData_GetAvacS_En()
{
  return AvacData_boCodingValid;
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_GetAvacD_Ok()
///
/// \descr  dynamic AVAC
///
/// \param  -
///
/// \return OK
//-----------------------------------------------------------------------------
boolean AvacData_GetAvacD_Ok()
{
  return TRUE; // TBD AfsMaster implementation
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_GetAvacD_En()
///
/// \descr  dynamic AVAC enable
///
/// \param  -
///
/// \return enable
//-----------------------------------------------------------------------------
boolean AvacData_GetAvacD_En()
{
  return AvacData_boCodingValid;
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_ReadPitch()
///
/// \descr
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
void AvacData_ReadPitch(tAvac_Pitch* pPitch)
{
  tPitchCan             PitchMsg = { 0, 0, SIG_INVALID };
  tisVehicleDynamic     Avac_VehicleDynamic;
  Rte_Call_PpaclVehHubVehicleDynamic_ReadData(&Avac_VehicleDynamic);

  if (SIG_VALID == Avac_VehicleDynamic.sSuspensionHeight.eQty)
  {
    //The suspension height is converted to radians
    (void)UaApplCalcPitchAngle((sint32)(((((uint16)(Avac_VehicleDynamic.sSuspensionHeight.usValueFL) + LEVELING_SENSER_OFFSET) +     //Offset:change from -100 to -127,
                                         ((uint16)(Avac_VehicleDynamic.sSuspensionHeight.usValueFR) + LEVELING_SENSER_OFFSET)) -
                                         (((uint16)(Avac_VehicleDynamic.sSuspensionHeight.usValueRL) + LEVELING_SENSER_OFFSET) +
                                         ((uint16)(Avac_VehicleDynamic.sSuspensionHeight.usValueRR) + LEVELING_SENSER_OFFSET))
                                        ) >> 1),
                                        &PitchMsg);
  }
  // convert to app
  pPitch->nAngle     = PitchMsg.Angle;             // [0.1mrad]
  pPitch->nHeightErr = PitchMsg.HeightErr;         // [0.1mrad]
  pPitch->eSigQty    = (tieSigQty)PitchMsg.eSigQty; // same definition of RTE and App
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_ReadSpeed()
///
/// \descr
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
void AvacData_ReadSpeed(tAvac_Speed* pSpeed)
{
#if (DEBUG_AVAC_DATA == STD_OFF)
  pSpeed->nSpeed = dummySpeed++;
  pSpeed->nAcceleration = 10;
  pSpeed->unMotion = BB_MotionLow;
  pSpeed->eSigQty = SIG_VALID;
  if (steps++ > DEBUG_AVAC_MAX_STEP) {
    dummySpeed = 0;
    dummyAngle = 0;
    steps = 0;
  }
#else

  tSpeedMsg         SpeedMsg;
  tisVehicleDynamic Avac_VehicleSpeed;
  Rte_Call_PpaclVehHubVehicleDynamic_ReadData(&Avac_VehicleSpeed);

  if (SIG_VALID == Avac_VehicleSpeed.sSpeed.eQty)
  {
    SpeedMsg.Speed = Avac_VehicleSpeed.sSpeed.usValue;
  }

  if (SIG_VALID == Avac_VehicleSpeed.sAccelerationRate.eQtyX)
  {
    SpeedMsg.Acceleration = Avac_VehicleSpeed.sAccelerationRate.usAccValueX;
  }
  else if (SIG_VALID == Avac_VehicleSpeed.sSpeed.eQty)
  {
    SpeedMsg.Acceleration = CalcAcceleration(SpeedMsg.Speed);//Acceleration is calculated from Speed
  }
  else
  {
    //do nothing
  }

    
  // convert to app
  pSpeed->nSpeed        = SpeedMsg.Speed;    // [0.1 km/h]
  pSpeed->nAcceleration = SpeedMsg.Acceleration;        // [0.1 m/s^2]
  pSpeed->eSigQty       = Avac_VehicleSpeed.sSpeed.eQty;  // same definition of RTE and app

#endif
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_ReadAccelPreDetect()
///
/// \descr
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
void AvacData_ReadAccelPreDetect(tAvac_AccelPreDetect* pAccelPreDetect)
{
#if (DEBUG_AVAC_DATA == STD_ON)
  pAccelPreDetect->eSigQty = SIG_INVALID;
#else
    tAccelPreDetectMsg  AccelPreDetectMsg;
    (void) Rte_Read_SigPrepRx_AccelPreDetectMsg_Port(&AccelPreDetectMsg);

    // convert to app
    pAccelPreDetect->nValue  = AccelPreDetectMsg.Value;               // [0..100] %
    pAccelPreDetect->nDelta  = AccelPreDetectMsg.Delta;               // [0..100] %
    pAccelPreDetect->eSigQty = (tieSigQty)AccelPreDetectMsg.eSigQty;
#endif
}

//-----------------------------------------------------------------------------
/// \brief  AvacData_ReadBrakePreDetect()
///
/// \descr
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
void AvacData_ReadBrakePreDetect(tAvac_BrakePreDetect* pBrakePreDetect)
{
#if (DEBUG_AVAC_DATA == STD_ON)
  pBrakePreDetect->eSigQty = SIG_INVALID;
#else
    tBrakePreDetectMsg  BrakePreDetectMsg;
    (void) Rte_Read_SigPrepRx_BrakePreDetectMsg_Port(&BrakePreDetectMsg);

    // convert to app
    pBrakePreDetect->nValue  = BrakePreDetectMsg.Value;               // [0..100] %
    pBrakePreDetect->nDelta  = BrakePreDetectMsg.Delta;               // [0..100] %
    pBrakePreDetect->eSigQty = (tieSigQty)BrakePreDetectMsg.eSigQty;
#endif
}

// ---------------------------------------------------------------------------
///
///  \func     Avac_GetInput
///
///  \brief    cyclic function called every 20ms
///
///  \param    none
///
///  \return   none
///
// ---------------------------------------------------------------------------
void Avac_GetInput(void)
{

}

//-----------------------------------------------------------------------------
/// \brief  AvacData_WriteAvacOut()
///
/// \descr
///
/// \param  -
///
/// \return void
//-----------------------------------------------------------------------------
void AvacData_WriteAvacOut(const tAvac_Out* pAvacOut)
{
  /*tisAvacPosition  AvacPositionOutput;*/
  sint16        nAngle;

  // convert internal Avac-Data [0.1mrad] to RTE-Data [0.01 deg]
  nAngle = pAvacOut->nAngle;
  nAngle = (nAngle >= 0) ? (sint16)((((sint32)nAngle * 180) + 157) / 314)
                         : (sint16)((((sint32)nAngle * 180) - 157) / 314);

  AvacPositionOutput.ssVerAngle0 = nAngle;            //left
  AvacPositionOutput.ssVerAngle1 = nAngle;            //Right
  AvacPositionOutput.eAvacSigQty = pAvacOut->eSigQty;

  Rte_Write_PpaseAVACAvacPosition_sValue(&AvacPositionOutput);
}


// input : active front - rear delta
// output: pitch and signal validation
static uint8 UaApplCalcPitchAngle(sint32 front_rear_delta, tPitchCan* pitch)
{
  sint32 delta;
  pitch->eSigQty = SIG_INVALID;

  // offset to calibration data, which is optical nullage
  delta = front_rear_delta - NvmSensorCaliData;
  // angle[0.1 mrad] = arctan(delta/wheelbase) * 10,000
  pitch->Angle = (sint16)(delta * 10000 / VehicleWheelBase);   // calc arctan and convert to 0.1mrad
  // [1/10 mrad] front side up --> positive value
  pitch->HeightErr = 0;
  pitch->eSigQty = SIG_VALID;

  return 1;
}


//-----------------------------------------------------------------------------
/// \brief  Calculate acceleration from speed
///
/// \param  unSpeed Speed [0.1 km/h]
///
/// \return sint16 acceleration [0.1 m/s^2]
//-----------------------------------------------------------------------------
static sint16 CalcAcceleration(const uint16 unSpeed)
{
  static uint16 aunAvrgSpeed[5] = { 0, 0, 0, 0, 0 };

  sint32 lAcc;
  sint32 lDeltaSpeed = (sint32)unSpeed - (sint32)aunAvrgSpeed[4];  // speed difference on T = 100ms (5*20ms)

  // calculate acceleration [0.1 m/s^2]
  // Acc = DeltaSpeed(0.1km/h) / DeltaT(ms) = DeltaSpeed / 100ms
  // Acc = (DeltaSpeed * 1000m / 3600) / 0.1
  // 0.1Acc = DeltaSpeed * 25 / 9
  lAcc = (lDeltaSpeed * 25) / 9;

  // store values
  aunAvrgSpeed[4] = aunAvrgSpeed[3];
  aunAvrgSpeed[3] = aunAvrgSpeed[2];
  aunAvrgSpeed[2] = aunAvrgSpeed[1];
  aunAvrgSpeed[1] = aunAvrgSpeed[0];
  aunAvrgSpeed[0] = unSpeed;

  return (sint16)lAcc;
}

#define ctaaAvac_STOP_SEC_CODE
# include "ctaaAvac_MemMap.h"