#include "FT_VehSpeedCalc.h"

typedef struct{
    unsigned int N;
    unsigned int VehSpd;
}s_VehSpeedInfo_Buffer;

s_VehSpeedInfo_Buffer MsgToSpd;
s_VehSpeedInfo_Buffer SpdToMsg;

unsigned int FT_WheelRadius = 309;
unsigned int FT_WheelTeeth = 68;
unsigned int FT_WheelRatio = 5135;

void SetWheelInfo(unsigned int Radius, unsigned int Teeth){
    unsigned long wTemp;

    wTemp = Radius;
    wTemp = 2 * 314 * wTemp / Teeth;
    wTemp = wTemp * 36 * 50;
    wTemp = wTemp / 1000;

    FT_WheelRatio = wTemp;
}
unsigned int TestMsg;
void FT_VehSpeedCalc_Main(unsigned int Data, unsigned int *DestData, unsigned char Dir){
    unsigned long wTemp;

    switch(Dir){
        case CANMESSAGE_TO_SPEED:{
            
        }break;

        case SPEED_TO_CANMESSAGE:{
            SpdToMsg.VehSpd = Data;
            wTemp = SpdToMsg.VehSpd;
            wTemp =  wTemp * 1000 * 2;
            wTemp = wTemp / FT_WheelRatio;
            TestMsg = wTemp;
            SpdToMsg.N += wTemp;
            if(SpdToMsg.N > 64255){
                SpdToMsg.N -= 64255;
            }
            *DestData = SpdToMsg.N;
        }break;

        default:
            ;
    }
}

void FT_Fill_VehSpeedCANMessage(unsigned int val, unsigned char *Data){
    Data[0] = val;
    Data[1] = val >> 8;
    Data[2] = val;
    Data[3] = val >> 8;
    Data[4] = val;
    Data[5] = val >> 8;
    Data[6] = val;
    Data[7] = val >> 8;
}