/***********************************************************
*   函數庫說明：差分定位數值計算函數庫                     *
*   版本：      v1.00beta                                  *
*   作者：      王卓然                                     *
*   創建日期：  2008年3月29日                              *
* -------------------------------------------------------- *
*  [支 持 庫]                                              *
*   支持庫名稱：                                           *
*   需要版本：                                             *
*   支持庫說明：                                           *
* -------------------------------------------------------- *
*  [版本更新]                                              *
*   修改：                                                 *
*   修改日期：                                             *
*   版本：                                                 *
* -------------------------------------------------------- *
*  [版本歷史]                                              *
* -------------------------------------------------------- *
*  [使用說明]                                              *
***********************************************************/

/********************
* 頭 文 件 配 置 區 *
********************/
# include "RD_MacroAndConst.h"
# include "DL_Config.h"
# include "RD_UseDLocate_LIB.h"
# include "PF_Config.h"
# include "Math.h"

/********************
*   系 統 宏 定 義  *
********************/

/*------------------*
*   常 數 宏 定 義  *
*------------------*/

//輪子直徑
#ifndef WHEEL_D_L
    #error No define WHEEL_D_L!
#endif
#ifndef WHEEL_D_R
    #error No define WHEEL_D_R!
#endif

//輪間距
#ifndef D_BTW_WHEEL
    #error No define D_BTW_WHEEL
#endif

//每圈線精度
#ifndef N_L
    #error No define N_L
#endif
#ifndef N_R
    #error No define N_R
#endif

//碼盤倍頻數
#ifndef MULTIPLE
    #error No define MULTIPLE
#endif

//計數器 到 弧長
# define K_L    ((PI * (WHEEL_D_L)) / ((MULTIPLE) * (N_L)))
# define K_R    ((PI * (WHEEL_D_R)) / ((MULTIPLE) * (N_R)))




/*------------------*
*   動 作 宏 定 義  *
*------------------*/

/********************
*  自定義類型聲明區 *
********************/

/********************
*  模塊結構體定義區 *
********************/

/********************
*   函 數 聲 明 區  *
********************/
float Get_Absolute_Angle(void);
float fMod(float fNumberA,float fNumberB);
BOOL PROC_Difference_Locate(void);

/********************
*   模塊函數聲明區  *
********************/

/********************
*   模塊變量聲明區  *
********************/

/********************
*   全局變量聲明區  *
********************/
float g_fLastAngle = 0.0;
float g_fX = 0.0;
float g_fY = 0.0;

/********************
*  全局變量聲引用區 *
********************/
extern BOOL g_bRunFlagB;

/***********************************************************
*   函數說明：獲取當前絕對角度函數                         *
*   輸入：    無                                           *
*   輸出：    角度值                                       *
*   調用函數：無                                           *
***********************************************************/
float Get_Absolute_Angle(void)
{
    return 
        (
            (float)g_lCounterR * (float)K_R 
             - (float)g_lCounterL * (float)K_L
         ) * (1.0 / (float)D_BTW_WHEEL);
}

/***********************************************************
*   函數說明：獲取當前相對角度函數                         *
*   輸入：    左編碼器微分量，右編碼器微分量               *
*   輸出：    角度值（弧度）                               *
*   調用函數：無                                           *
***********************************************************/
float Get_Relative_Angle(INT32 lDCL,INT32 lDCR)
{
    if (lDCL == lDCR)
    {
        return 0.0;
    }
    
    return 
        (
            (float)lDCR * (float)K_R 
             - (float)lDCL * (float)K_L
         ) * (1.0 / (float)D_BTW_WHEEL);
}



/***********************************************************
*   函數說明：差分定位計算函數                             *
*   輸入：    無                                           *
*   輸出：    FALSE                                        *
*   調用函數：無                                           *
***********************************************************/
BOOL PROC_Difference_Locate(void)
{
    static INT32 s_lLastCounterL = 0;
    static INT32 s_lLastCounterR = 0;
    
    if ((g_lCounterLImage - s_lLastCounterL)
         == (g_lCounterRImage - s_lLastCounterR))
    {
        INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
        float fR = ((float)nDeltaCounterL * (float)K_L);
        
        g_fX += fR * cos(g_fLastAngle);
        g_fY += fR * sin(g_fLastAngle);
    }
    else
    {
        float fDeltaAngle,fAbsoluteAngle;
        //計算角度微元
        {
            float fTempAngle = Get_Relative_Angle
                            (
                                g_lCounterLImage,
                                g_lCounterRImage
                            );
            fDeltaAngle = (fTempAngle - g_fLastAngle);
            fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
    
            g_fLastAngle = fTempAngle;
        }
        //計算位置微元
        {
            INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
            float fR = (((float)nDeltaCounterL * (float)K_L) * (1.0 / fDeltaAngle) 
                + (float)D_BTW_WHEEL * 0.5);

            fR = 2.0 * fR * sin(fDeltaAngle * 0.5);             //fR->l
        
            g_fX += fR * cos(fAbsoluteAngle);
            g_fY += fR * sin(fAbsoluteAngle);
        }
    }
    
    //數據更新
    s_lLastCounterL = g_lCounterLImage;
    s_lLastCounterR = g_lCounterRImage;
    g_bRunFlagB = TRUE;
    
    return FALSE;
}

/***********************************************************
*   函數說明：實數求模函數                                 *
*   輸入：    被除數，除數                                 *
*   輸出：    取模結果                                     *
*   調用函數：無                                           *
***********************************************************/
float fMod(float fNumberA,float fNumberB)
{
    if (fNumberB < 0)
    {
        if (fNumberA < 0)
        {
            while(fNumberA < fNumberB)
            {
                fNumberA -= fNumberB;
            }
        
            return fNumberA;
        }
        else if (fNumberA > 0)
        {
            while((fNumberA + fNumberB) > fNumberB)
            {
                fNumberA += fNumberB;
            }
            
            return fNumberA;
        }
        else
        {
            return 0.0;
        }
    }
    else if (fNumberB > 0)
    {
        if (fNumberA < 0)
        {
            while((fNumberA + fNumberB) < fNumberB)
            {
                fNumberA += fNumberB;
            }
            
            return fNumberA;
        }
        else if (fNumberA > 0)
        {
            while(fNumberA > fNumberB)
            {
                fNumberA -= fNumberB;
            }
            
            return fNumberA;
        }
        else
        {
            return 0.0;
        }
    }
    else
    {
        return 0.0;
    }
}