//Ultrasone.cpp
//  V3.0

#include "Ultrasone.h"

void Ultrasone::GetRadar(Data *DataLine) 
{
  DataLine->HeadTime = 0;

#ifdef DEBUGUS
  Serial.print("### Ultrasone Direction/Position: ");
  Serial.print(CurrentDirection);
  Serial.print(", ");
  Serial.println(CurrentPosition);
#endif

  if (CurrentDirection == 0) // Left
  {
    if (CurrentPosition > MaxLeft)
    {
      CurrentPosition--;
      DataLine->HeadAngle = CurrentPosition;

      Con.ConvertHead(DataLine);
      SSC32.MoveHead(DataLine);

      Serial.print("### Ultrasone Distance: ");
      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
    }
    else
      CurrentDirection = 1;
  }
  else if(CurrentDirection == 1) // Right
  {
    if (CurrentPosition < MaxRight)
    {
      CurrentPosition++;
      DataLine->HeadAngle = CurrentPosition;

      Con.ConvertHead(DataLine);
      SSC32.MoveHead(DataLine);

      Serial.print("### Ultrasone Distance: ");
      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
    }
    else
      CurrentDirection = 0;
  }
}


/*
    [Vector]
    X = Distance / Cos( CurrentPosition * pi2rad );
    Y = Distance / sin( CurrentPosition / pi2rad );    
*/