/* 包含的头文件 --------------------------------------------------------------*/
#include "tof.h"

/************************************************
函数名称 ： TOF_ReadNByte
功    能 ： TOF读N字节
参    数 ： Dev_Addr ----- 设备地址
            Data_Addr ----- 数据地址
            pData ---- 数据
            Length --- 长度
返 回 值 ： 无
*************************************************/
void TOF_ReadNByte(u16 Dev_Addr,u16 Data_Addr, u8 *pData, u16 Length)
{
  u16 cnt;
  /* 1.开始 */
  I2C_Start();
  /* 2.设备地址/写 */
  I2C_WriteByte(Dev_Addr<<1 | TOF_WR);
  /* 3.数据地址 */
  I2C_WriteByte((u8)(Data_Addr&0x00FF)); //数据地址(8位)
//TOF数据准备时间40us
  uDelay(50);
  /* 4.重新开始 */
  I2C_Start();
  /* 5.设备地址/读 */
  I2C_WriteByte(Dev_Addr<<1 | TOF_RD);
  /* 6.读一字节数据 */
  for(cnt=0; cnt<(Length-1); cnt++)
  {
    *pData = I2C_ReadByte(I2C_ACK);     //连续读取(Length-1)字节(产生应答)
    pData++;
  }
  *pData = I2C_ReadByte(I2C_NOACK);     //读取最后1字节(产生非应答)
  /* 7.停止 */
  I2C_Stop();
}

/************************************************
函数名称 ： U8Table_to_u16_mm
功    能 ： 转换传感器数据为u16
参    数 ： buf-----存放结果的数组
              buf[0]-----高8位数据
              buf[1]-----低8位数据
返 回 值 ： u16
*************************************************/
u16 U8Table_to_u16_mm(u8 *buf)
{
  u16 distance=buf[0]*256+buf[1];
	#ifdef WasteBin
		if (distance == TOF_DIS_INVALID) 
		distance=TOF_DIS_LIMIT;
	#else
		if ((distance == TOF_DIS_INVALID)|| (distance > TOF_DIS_OVER)) 
		distance=TOF_DIS_OVER;
	#endif
	
  return distance;
}

/************************************************
函数名称 ： Get_TOF_Distance
功    能 ： 读取传感器数据
参    数 ： Dev_Addr-----设备地址
返 回 值 ： u16-----距离
*************************************************/
u16 Get_TOF_Distance(u16 Dev_Addr)
{
  u8 buf[TOF_DIS_LENGTH+1];
  TOF_ReadNByte(Dev_Addr,TOF_DIS_ADDR, buf, TOF_DIS_LENGTH);
  return U8Table_to_u16_mm(buf);
}

/************************************************
函数名称 ： Get_Real_TOF_Distance
功    能 ： 获取距离，低级防抖
参    数 ： Dev_Addr-----设备地址
返 回 值 ： u16-----距离
*************************************************/
u16 Get_Real_TOF_Distance(u16 Dev_Addr)
{
  u16 distance=0;
  for(int i=0;i<3;i++)
  {
    distance=Get_TOF_Distance(Dev_Addr);
      if((distance<=TOF_DIS_OVER)&&(distance>0))//数据过滤
      {
        break;
      }
      if(i>=2)
      {
        distance=0;
      }
  }
  return distance;
}

/************************************************
函数名称 ： Get_Real_TOF_Distance_to_u8Table
功    能 ： 获取距离，存放入2字节数组
参    数 ： Dev_Addr-----设备地址
            buf-----存放结果的数组
              buf[0]-----高8位数据
              buf[1]-----低8位数据
返 回 值 ： 无
*************************************************/
void Get_Real_TOF_Distance_to_u8Table(u16 Dev_Addr,u8 *buf)
{
  u16 real_distance=Get_Real_TOF_Distance(Dev_Addr);
  buf[1]=real_distance&0x00FF;
  buf[0]=real_distance>>8&0x00FF;
}

/************************************************
函数名称 ： Get_Real_TOF_Distance_to_u8_Low_mm
功    能 ： 获取距离数据低8位，单位mm，丢失高8位数据
参    数 ： Dev_Addr-----设备地址
返 回 值 ： u8-----距离
*************************************************/
u8 Get_Real_TOF_Distance_to_u8_Low_mm(u16 Dev_Addr)
{
  u16 real_distance=Get_Real_TOF_Distance(Dev_Addr);
  return real_distance&0x00FF;
}

/************************************************
函数名称 ： Get_Real_TOF_Distance_to_u8_Lose_cm
功    能 ： 获取距离，单位cm，mm精度丢失
参    数 ： Dev_Addr-----设备地址
返 回 值 ： u8-----距离
*************************************************/
u8 Get_Real_TOF_Distance_to_u8_Lose_cm(u16 Dev_Addr)
{
  u16 real_distance=Get_Real_TOF_Distance(Dev_Addr);
  return (real_distance/10)&0x00FF;
}

/************************************************
函数名称 ： Get_Real_TOF_Distance_to_bool
功    能 ： 获取距离状态
参    数 ： Dev_Addr-----设备地址
返 回 值 ： bool-----状态
*************************************************/
bool Get_Real_TOF_Distance_to_bool(u16 Dev_Addr)
{
  u16 real_distance=Get_Real_TOF_Distance(Dev_Addr);
  if(real_distance<TOF_DIS_EN)
  {
    return TRUE;
  }
  else
  {
    return FALSE;
  }
}

/************************************************
函数名称 ： Get_TOF_Count
功    能 ： 获取总数量和判定数量
参    数 ： buf-----存放结果的数组
              buf[0]-----总数量
              buf[1]-----判定数量
            level-----判定档位
返 回 值 ： 无
*************************************************/
void Get_TOF_Count(u8 *buf,u16 level)
{
  u8 all_count=0;
  u8 in_count=0;
  for(u8 i=1;i<=TOF_DEV_MAX_COUNT;i++)
  {
    u16 distance=Get_Real_TOF_Distance(i*2);
    if((distance==0)||(i>TOF_DEV_MAX_COUNT))
    {
      break;
    }
    else
    {
      all_count++;
      if(distance<level)
      {
        in_count++;
      }
    }
  }
  buf[0]=all_count;
  buf[1]=in_count;
}

/************************************************
函数名称 ： Get_TOF_Data
功    能 ： 获取传感器数据
参    数 ： 
             
返 回 值 ： 无
*************************************************/
void Get_TOF_Data(u8 *buf,u16 count)
{
	if(count <= TOF_DEV_MAX_COUNT)
	{
		for(uint8_t i = 0; i< count; i++)
		{
			buf[i] = Get_Real_TOF_Distance_to_u8_Low_mm(2*(i+1));
		}
	}
	
}