#include <string.h>
#include <stdlib.h>

#include "gpio.h"

#include "uart.h"
#include "tick.h"

#include "tof.h"

#pragma pack(push,1)
struct HeadTOF
{
  unsigned short Header;     // 0x5959
  unsigned short Distance;
  unsigned short Strength;
  unsigned short Temprature; // Temprature/8 - 256;
  unsigned char  Check;      // Summ & 0xFF
};
#pragma pack(pop)

static const unsigned long TOF_Count=8;
static char TOF_Buffer[TOF_Count];
static unsigned long TOF_Size=0;

static float TOF_Range=0;

void TOF_Init()
{
  UART3_Init(115200);
}

void TOF_Update()
{
  char c;
  if(UART3_Recv(&c, 1))
  {
    if(c==' ')
    {
      TOF_Size=1;
      TOF_Buffer[0]=c;
      return;
    }
    
    if(!TOF_Size) return;

    if(c=='\r')
    {
      if(TOF_Size<4) 
      {
        TOF_Size=0;
        return;
      }
      
      TOF_Buffer[TOF_Size]='\0';
      
      char* end;
      float range=strtof(TOF_Buffer, &end);
      if(end-TOF_Buffer==TOF_Size) TOF_Range=range;
      else
        range=0;
      TOF_Size=0;
      return;
    }
    
    TOF_Buffer[TOF_Size++]=c;
    
    if(TOF_Size>=TOF_Count)
    {
      TOF_Size=0;
      return;
    }
    
  }
  
}

void TOF_Update2()
{
  static HeadTOF buffer;
  static unsigned long index=0;
  char* data=(char*)&buffer;
  
  char c;
  while(UART3_Recv(&c, 1))
  {
    if(index<2)
    {
      if(c==0x59)
      {
        data[index++]=c;
        continue;
      }
      else 
      {
        index=0;
        break;
      }
    }
    
    data[index++]=c;
    
    if(index<sizeof(HeadTOF)) continue;
    
    index=0;
    
    unsigned char check=0;
    for(int a=0; a<sizeof(HeadTOF)-1; a++) check+=data[a];
    
    if(buffer.Check!=check) break;
    
    TOF_Range=((float)buffer.Distance)/100;
    
    break;
  }
  
}

float TOF_GetRange()
{
  return TOF_Range;
}