/*
 * msp_Nooploop.c
 *
 *  Created on: Feb 18, 2024
 *      Author: BM
 */

#include "MSP/msp_Nooploop.h"

uint8_t  Dist_FDCAN_CH_1;
NooploopPara*  Nooploop_CAN;
uint8_t Nooploop_Num=0;

void GF_MSP_Nooploop_CANRx_Callback();
void Nooploop_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data);/*///FrameID, int Length_d,uint8_t *Txdata  //CAN data transmission function*/

uint8_t GF_MSP_Nooploop_Init(uint8_t FDCAN_CH, NooploopPara* BJ_Distance,uint8_t Num_Nooploop_p)
{
	Dist_FDCAN_CH_1=FDCAN_CH;
	Nooploop_CAN=BJ_Distance;
	Nooploop_Num = Num_Nooploop_p;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_FDCAN1_RxFifo0Callback, GF_MSP_Nooploop_CANRx_Callback);
	return 1;
}



int Re_count_03=0;
int Value_count_03=0;

int Re_count_04=0;
int Value_count_04=0;

double Sum_Value_03=0;
double Sum_Value_04=0;

int Re_count_05=0;

void GF_MSP_Nooploop_CANRx_Callback()
{
	int ID_A_T;
	ID_A_T=(int)CAN_ID;
	double Dist_03[16]={0,0};

	switch(ID_A_T)
	{
	case 515:
		Value_count_04=0;
		Re_count_04=0;
		Sum_Value_04=0;

		if ((CAN_Buf[3])==0)
		{
			Dist_03[Re_count_03]=(double)(CAN_Buf[0] | (CAN_Buf[1])<<8| (CAN_Buf[2])<<16);
			Sum_Value_03=Sum_Value_03+Dist_03[Re_count_03];
			Value_count_03++;
		}
		Re_count_03++;
		if(Re_count_03>=15)
		{
			Re_count_03=0;
			Nooploop_CAN->Forward_Dist=Sum_Value_03/(Value_count_03+1);
			Sum_Value_03=0;
		}
	//	Re_count_05++;
		break;
	case 516:
		Value_count_03=0;
		Re_count_03=0;
		Sum_Value_03=0;
		if ((CAN_Buf[3])==0)
		{
			Dist_03[Re_count_04]=(double)(CAN_Buf[0] | (CAN_Buf[1])<<8| (CAN_Buf[2])<<16);
			Sum_Value_04=Sum_Value_04+Dist_03[Re_count_04];
			Value_count_04++;
		}
		Re_count_04++;
		if(Re_count_04>=15)
		{
			Re_count_04=0;
			Nooploop_CAN->Backward_Dist=Sum_Value_04/(Value_count_04+1);
			Sum_Value_04=0;
			Value_count_04=0;
		}
		break;
	default:
//		Value_count_03=0;
//		Re_count_03=0;
//		Sum_Value_03=0;
//
//		Value_count_04=0;
//		Re_count_04=0;
//		Sum_Value_04=0;
		break;
	}

}


void GF_MSP_Nooploop_Inqurey(uint32_t Motor_ID_1)
{
	uint8_t Pos_can_buff_Motor_To[8];
	int Length_date_1=8;
	Pos_can_buff_Motor_To[0]=0xFF;
	Pos_can_buff_Motor_To[1]=0xFF;
	Pos_can_buff_Motor_To[2]=0xFF;
	Pos_can_buff_Motor_To[3]=Motor_ID_1;

	Pos_can_buff_Motor_To[4]=0xFF;
	Pos_can_buff_Motor_To[5]=0xFF;
	Pos_can_buff_Motor_To[6]=0xFF;
	Pos_can_buff_Motor_To[7]=0xFF;

	Nooploop_FDCANfunction(0X402, Length_date_1,Pos_can_buff_Motor_To);
}


void Nooploop_FDCANfunction(uint32_t ID,uint8_t Length,uint8_t *Data)
{
	GF_BSP_FDCAN_Senddata(Dist_FDCAN_CH_1,ID,Length,Data);
}
