/**
* @file ALGO_MilkContent_ATY.c
*
* @param Project ALGO_Algorithm_ATY_LIB
*
* @author ATY
*
* @copyright
*       - Copyright 2017 - 2025 MZ-ATY
*       - This code follows:
*           - MZ-ATY Various Contents Joint Statement -
*               <a href="https://mengze.top/MZ-ATY_VCJS">
*                        https://mengze.top/MZ-ATY_VCJS</a>
*           - CC 4.0 BY-NC-SA -
*               <a href="https://creativecommons.org/licenses/by-nc-sa/4.0/">
*                        https://creativecommons.org/licenses/by-nc-sa/4.0/</a>
*       - Your use will be deemed to have accepted the terms of this statement.
*
* @brief Familiar functions of MilkContent algorithm
*
* @version
*       - 1_01_221217 > ATY
*           -# Preliminary version, first Release
********************************************************************************
*/

#ifndef __ALGO_MilkContent_ATY_C
#define __ALGO_MilkContent_ATY_C

#include "ALGO_MilkContent_ATY.h"

/******************************* For user *************************************/

/******************************************************************************/


uint8_t detectContent_Milk = COW_MILK;
__CODE MilkCoefficient_S coeSave[MILK_KIND_MAX - 1] = {
    // P/L/S/FM/FPl/FPo/SC/PH
    {0.3925, 0.5251, 0.0727, -0.071, 0.0588, -0.52},    // 2-COW_MILK
    // {0.367, 0.55, 0.083, 0, 0.52},   // 2-COW_MILK
    {0.5987, 0.3683, 0.066, -0, -0, -0.52},  // 3-SHEEP_MILK
    // {0.475, 0.45, 0.075, 0.52},  // 3-SHEEP_MILK
    {0.3972, 0.5205, 0.0802, -0.071134, 0.0874, -0.52},    // 4-UHT_MILK
};  // Use: coeSave[kind - 2]

MilkContent_S milcDefault = {0};

float dampAtRefT = 0;
float speedAtRefT = 0;


/**
 * @brief  Calc Milk Density and Fat From US
 *
 * @param usSpeed us speed at current temperature
 * @param usDamp damp percent at current temperature, 50-100
 * @param refT ref temperature
 */
void MilkDensityFatFromUS(float usSpeed, float usDamp, float refT, float offset)
{
    if(usSpeed > 0)
    {
        if(detectContent_Milk == COW_MILK)
        {
            /*
            30.589  1524.097
            40.5036 1535.311
            49.490  1523.449
            */
            // Speed=-0.13T^2+10.376T+(1328.33) (3.18,2.97P g/100ml)
            float speedParam_C = 0;

#define P_A 0.13
#define P_B 10.376
            speedParam_C = usSpeed
                + P_A * milcDefault.conTemperature * milcDefault.conTemperature
                - P_B * milcDefault.conTemperature;

#define WEIGHT_DAIRY ((usSpeed - PureWaterSpeed(milcDefault.conTemperature)) / (1520 - PureWaterSpeed(milcDefault.conTemperature)))
#define WEIGHT_WATER (1 - WEIGHT_DAIRY)
            speedAtRefT = (WEIGHT_WATER * PureWaterSpeed(refT))
                + (WEIGHT_DAIRY * (speedParam_C - P_A * refT * refT + P_B * refT));

            // y = -0.044959 x2 + 138.798999 x - 106,102.073260  (5.68 Plus)
            milcDefault.conDensity =
                (-106102 + 138.8 * speedAtRefT - 0.045 * speedAtRefT * speedAtRefT)
                + offset;
               // / PureWaterDensity(refT) * 1000;
        }
        else if(detectContent_Milk == PURE_WATER){
            speedAtRefT = PureWaterSpeed(refT);
            milcDefault.conDensity = PureWaterDensity(milcDefault.conTemperature)
                + ((usSpeed - PureWaterSpeed(milcDefault.conTemperature)) * 1.6);
                // milcDefault.conDensity = PureWaterDensity(milcDefault.conTemperature);
        }

        if(milcDefault.conDensity < 0)
            milcDefault.conDensity = 0;
    }
    else
        milcDefault.conDensity = 0;

    if(detectContent_Milk > PURE_WATER && usDamp > 0 && milcDefault.conDensity > 1000)
    {
        /*
        30.477 1529.69
        35.400 1530.448
        39.528 1531.863
        44.571 1534.34
        49.471 1537.76
        */
        // y=0.0189x^2-1.0874x+(1545.3) (3.18,2.97P g/100ml)
        float dampParam_C = 0;

        dampParam_C = usDamp
            - 0.0189 * milcDefault.conTemperature * milcDefault.conTemperature
            + 1.0874 * milcDefault.conTemperature;
        dampAtRefT = dampParam_C + 0.0189 * refT * refT - 1.0874 * refT;

        // Fat = fat + (fat - (Density - 1000) / 10) = 2 * fat - (Denaity - 1000) / 10
        // if(dampAtRefT < 40 && milcDefault.conDensity - PureWaterDensity(refT) > 20)
        //     milcDefault.conFat = 4.5;
        // else
        milcDefault.conFat = dampAtRefT * RefFat / 100;
        // milcDefault.conFat = (0.1500 * (milcDefault.conDensity - 1000)) + 0.0160; // Temp debug
        // milcDefault.conFat = milcDefault.conFat
        //     + ((milcDefault.conFat - (milcDefault.conDensity - 1006) / 10)
        //         * (1 - (milcDefault.conFat / RefFat)));

        if(milcDefault.conFat > 4.6)
            milcDefault.conFat = 4.6;
        else if(milcDefault.conFat < 0)
            milcDefault.conFat = 0;
    }
    else
        milcDefault.conFat = 0;

    MilkContentFromDensity();
}





/**
 * @brief   Analyse milk content from density
 */
void MilkContentFromDensity(void)
{
    // Test algo
    // milcDefault.conDensity = 25.73;
    // milcDefault.conFat = 3.87;

    if(detectContent_Milk == PURE_WATER || milcDefault.conDensity < 1000)
    {
        milcDefault.conSnf = 0;
        milcDefault.conProtein = 0;
        milcDefault.conLactose = 0;
        milcDefault.conSalts = 0;
        milcDefault.conFreeze = 0;
        milcDefault.conWater = 100;
        milcDefault.conSC = 0;
        milcDefault.conPH = 0;
        return;
    }

    if(detectContent_Milk == COW_MILK || detectContent_Milk == SHEEP_MILK || detectContent_Milk == UHT_MILK)
    {
        milcDefault.conSnf = (100 - (
            (99865)
            // Pure water density with temperature: y = -0.0038x2 - 0.0475x + 1000.5 (y = -0.3844x2 - 4.7525x + 100046)
            // (100046 - (4.7525 * milcDefault.conTemperature) - (0.3844 * milcDefault.conTemperature * milcDefault.conTemperature))
            // / (milcDefault.conDensity + 1000)) + (0.075 * milcDefault.conFat)) / 0.378;
            / (milcDefault.conDensity)) + (0.075 * milcDefault.conFat)) / 0.378;
        // milcDefault.conSnf = (100 - (100000 / (milcDefault.conDensity + 1000)) + (0.075 * milcDefault.conFat)) / 0.378;
        // milcDefault.conSnf = (100 - (100000 / (milcDefault.conDensity + 1000)) + (0.075 * milcDefault.conFat)) / 0.3647;


        milcDefault.conProtein = coeSave[detectContent_Milk - 2].coeProtein * milcDefault.conSnf;
        milcDefault.conLactose = coeSave[detectContent_Milk - 2].coeLactose * milcDefault.conSnf;
        milcDefault.conSalts = coeSave[detectContent_Milk - 2].coeSalts * milcDefault.conSnf;
        milcDefault.conFreeze = (coeSave[detectContent_Milk - 2].coeFreezeMul * milcDefault.conSnf) + coeSave[detectContent_Milk - 2].coeFreezePlus;

        if(milcDefault.conFreeze < coeSave[detectContent_Milk - 2].coeFreezePoint)
            milcDefault.conFreeze = coeSave[detectContent_Milk - 2].coeFreezePoint;
        milcDefault.conWater = 100.0 - (100.0 * milcDefault.conFreeze / coeSave[detectContent_Milk - 2].coeFreezePoint);
        milcDefault.conSC = 0;
        milcDefault.conPH = 0;
    }

#ifdef __DEBUG_ALGO_MilkContent_ATY
    printf("\r\n\r\nFinalData:\
        \r\nFat         - %2.5f\
        \r\nDensity     - %2.5f\
        \r\nSNF         - %2.5f\
        \r\nProteinD    - %2.5f\
        \r\nLactose     - %2.5f\
        \r\nSalts       - %2.5f\
        \r\nWater       - %2.5f\
        \r\nFreezing    - %2.5f\
        \r\nTemperature - %2.5f\
        ", milcDefault.conFat, milcDefault.conDensity, milcDefault.conSnf,
        milcDefault.conProtein, milcDefault.conLactose, milcDefault.conSalts,
        milcDefault.conWater, milcDefault.conFreeze, milcDefault.conTemperature);
#endif /* __DEBUG_ALGO_MilkContent_ATY */
}

#endif /* __ALGO_MilkContent_ATY_C */

/******************************** End Of File *********************************/