/*------------------------------------------------------------------------------

  LIDARLite Arduino Library
  GetDistancePwm

  This example shows how to read distance from a LIDAR-Lite connected over the
  PWM interface.

  Connections:
  LIDAR-Lite 5 Vdc (red) to Arduino 5v
  LIDAR-Lite Ground (black) to Arduino GND
  LIDAR-Lite Mode control (yellow) to Arduino digital input (pin 3)
  LIDAR-Lite Mode control (yellow) to 1 kOhm resistor lead 1
  1 kOhm resistor lead 2 to Arduino digital output (pin 2)

  (Capacitor recommended to mitigate inrush current when device is enabled)
  680uF capacitor (+) to Arduino 5v
  680uF capacitor (-) to Arduino GND

  See the Operation Manual for wiring diagrams and more information:
  http://static.garmin.com/pumac/LIDAR_Lite_v3_Operation_Manual_and_Technical_Specifications.pdf

------------------------------------------------------------------------------*/
int VAK_A = 1;
int VAK_B = 2;
int VAK_C = 3;

int currentState;

double pulseWidth;

void setup()
{
  Serial.begin(115200); // Start serial communications

  pinMode(2, OUTPUT); // Set pin 2 as trigger pin
  digitalWrite(2, LOW); // Set trigger LOW for continuous read

  pinMode(3, INPUT); // Set pin 3 as monitor pin
}

void loop()
{
  pulseWidth = pulseIn(3, HIGH); // Count how long the pulse is high in microseconds

  // If we get a reading that isn't zero, let's print it
  if(pulseWidth != 0)
  {
    pulseWidth = pulseWidth / 1000; // 10usec = 1 cm of distance
    printVak(pulseWidth);

    Serial.println(pulseWidth); // Print the distance
  }
}

void printVak(double pulseWidth){
  if(pulseWidth < VAK_A){
    if(currentState != VAK_A)
      Serial.println("Vak A");
      currentState = VAK_A;
  } else if(pulseWidth < VAK_B && pulseWidth > VAK_A){
    if(currentState != VAK_B)
      Serial.println("Vak B");
      currentState = VAK_B;
  } else if(pulseWidth < VAK_C && pulseWidth > VAK_B){
    if(currentState != VAK_C)
      Serial.println("Vak C");
      currentState = VAK_C;
  }
}