/*
 * RoboPeak RPLIDAR Arduino Example
 * This example shows the easy and common way to fetch data from an RPLIDAR
 *
 * You may freely add your application code based on this template
 *
 * USAGE:
 * ---------------------------------
 * 1. Download this sketch code to your Arduino board
 * 2. Connect the RPLIDAR's serial port (RX/TX/GND) to your Arduino board (Pin 0 and Pin1)
 * 3. Connect the RPLIDAR's motor ctrl pin to the Arduino board pin 3
 */

/*
 * Copyright (c) 2014, RoboPeak
 * All rights reserved.
 * RoboPeak.com
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

// This sketch code is based on the RPLIDAR driver library provided by RoboPeak
#include <RPLidar.h>

// You need to create an driver instance
RPLidar lidar;

#define RPLIDAR_MOTOR 5 // The PWM pin for control the speed of RPLIDAR's motor.
                        // This pin should connected with the RPLIDAR's MOTOCTRL signal

#define BAUD_RATE 9600
#define WIDTH 450
#define LENGTH 350
#define Y_OFFSET 120
#define RECORD 0
#define PAUSE 1
#define RECORD_PAUSE_PIN 2
#define CLEAR_PIN 3
#define STOP_PIN 4


void setup() {
  // bind the RPLIDAR driver to the arduino hardware serial
  Serial.begin(BAUD_RATE);
  Serial2.begin(BAUD_RATE);
  lidar.begin(Serial1);

  // set pin modes
  pinMode(RPLIDAR_MOTOR, OUTPUT);
  pinMode(RECORD_PAUSE_PIN, INPUT);
  pinMode(CLEAR_PIN, INPUT);
  pinMode(STOP_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(CLEAR_PIN), clearBoard, RISING);
}

float minDistance = 100000;
float angleAtMinDist = 0;
volatile int state = RECORD;
volatile int boardClear = 0;
volatile int doneRecording = 0;

void loop() {
  if (IS_OK(lidar.waitPoint())) {
    float distance = lidar.getCurrentPoint().distance; //distance value in mm unit
    float angle    = lidar.getCurrentPoint().angle; //anglue value in degree

    // data processing here...
    if (lidar.getCurrentPoint().startBit) {
      float r = minDistance;
      float theta = (360 - angleAtMinDist) * PI / 180;
       // calculate cartesian coordinate here
       float x = r * cos(theta) + WIDTH/2;
       float y = r * sin(theta) * -1 - Y_OFFSET;
       if (digitalRead(STOP_PIN) == HIGH) {
         Serial2.print("D");
         Serial.println("done recording");
         return;
       }

       if (digitalRead(RECORD_PAUSE_PIN) == RECORD && 0 < x && x < WIDTH && 0 < y && y < LENGTH) {
         Serial.print("(");
         Serial.print(x);
         Serial.print(", ");
         Serial.print(y);
         Serial.print(")\n");
         // serial to edison
         Serial2.print("X");
         Serial2.println(x);
         Serial2.print("Y");
         Serial2.println(y);
       } else if (boardClear) {
         // board needs to be cleared
         Serial2.print("C");
         boardClear = 0;
       } else {
         // no point in the region OR paused recording
         Serial2.print("S");
       }
       // reset variables
       minDistance = 100000;
       angleAtMinDist = 0;
    } else {
       if (distance > 0 &&  distance < minDistance && angle < 180) {
          minDistance = distance;
          angleAtMinDist = angle;
       }
    }

  } else {
    analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor

    // try to detect RPLIDAR...
    rplidar_response_device_info_t info;
    if (IS_OK(lidar.getDeviceInfo(info, 100))) {
       // detected...
       lidar.startScan();

       // start motor rotating at max allowed speed
       analogWrite(RPLIDAR_MOTOR, 255);
       delay(1000);
    }
  }
}

void clearBoard() {
  Serial.println("clear pressed!");
  boardClear = 1;
}

void stopRecording() {
  Serial.println("stopped recording");
  doneRecording = 1;
}