#include "stdint.h"
#include "board.h"
#include "tdc_tof.h"

// ultrasonic speed from 0 dgree to 56 dgree
static const uint16_t USSpdX10Ary[] = {
14023, 14073, 14122, 14169, 14216, 14261, 14305, 14348, 
14391, 14432, 14472, 14511, 14549, 14587, 14623, 14658, 
14693, 14727, 14760, 14791, 14823, 14853, 14882, 14911, 
14939, 14966, 14992, 15018, 15043, 15067, 15090, 15113, 
15135, 15157, 15177, 15197, 15217, 15235, 15253, 15271, 
15288, 15304, 15320, 15335, 15349, 15363, 15377, 15389, 
15402, 15413, 15425, 15435, 15446, 15455, 15464, 15473,
15481
};

#define NTC50K_START_DGR     (0.5f)//modified by yuewei 20260630 0.0->0.5

void Ohm2Dgr2USSpd(uint16_t ohmX10, float *p)
{
    int16_t start, end, mid;
    uint16_t TmpU16;
    float TmpFloat;

    start = 0;
    end   = pTdcItf->ntc_num;

    while((start+1)<end){
        mid = (start + end)>>1;     // 1<= mid <=NTC50K_SUM-1
        TmpU16 = pTdcItf->ntc_ohm_ary[mid];

        // descending 
        if (TmpU16>ohmX10){
            start = mid;
        } else {
            end = mid;
        }
    }

    TmpFloat = pTdcItf->ntc_slp_ary[start]*(ohmX10 - pTdcItf->ntc_ohm_ary[start]);
    *p++ = NTC50K_START_DGR + start + TmpFloat;
    //*p   = (USSpdX10Ary[start] + (USSpdX10Ary[end] - USSpdX10Ary[start])*TmpFloat)*0.1f;//delete by yuewei 20260529
}