text: /*
   *  RPLIDAR
   *  Simple Data Grabber Demo App
   *
   *  Copyright (c) 2009 - 2014 RoboPeak Team
   *  http://www.robopeak.com
   *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
   *  http://www.slamtec.com
   *
   */
  /*
   * This program is free software: you can redistribute it and/or modify
   * it under the terms of the GNU General Public License as published by
   * the Free Software Foundation, either version 3 of the License, or
   * (at your option) any later version.
   *
   * This program is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   * GNU General Public License for more details.
   *
   * You should have received a copy of the GNU General Public License
   * along with this program.  If not, see <http://www.gnu.org/licenses/>.
   *
   */
  
  #include <stdio.h>
  #include <stdlib.h>
  
  #include "rplidar.h" //RPLIDAR standard sdk, all-in-one header
  
  
  #ifndef _countof
  #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
  #endif
  
  #ifdef _WIN32
  #include <Windows.h>
  #define delay(x)   ::Sleep(x)
  #else
  #include <unistd.h>
  static inline void delay(_word_size_t ms){
      while (ms>=1000){
          usleep(1000*1000);
          ms-=1000;
      };
      if (ms!=0)
          usleep(ms*1000);
  }
  #endif
  
  using namespace rp::standalone::rplidar;
  
  void print_usage(int argc, const char * argv[])
  {
      printf("Simple LIDAR data grabber for RPLIDAR.\n"
             "Version: " RPLIDAR_SDK_VERSION "\n"
             "Usage:\n"
             "%s <com port> [baudrate]\n"
             "The default baudrate is 115200(for A2) or 256000(for A3). Please refer to the datasheet for details.\n"
             , argv[0]);
  }
  
  
  void plot_histogram(rplidar_response_measurement_node_t * nodes, size_t count)
  {
      const int BARCOUNT =  75;
      const int MAXBARHEIGHT = 20;
      const float ANGLESCALE = 360.0f/BARCOUNT;
  
      float histogram[BARCOUNT];
      for (int pos = 0; pos < _countof(histogram); ++pos) {
          histogram[pos] = 0.0f;
      }
  
      float max_val = 0;
      for (int pos =0 ; pos < (int)count; ++pos) {
          int int_deg = (int)((nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f/ANGLESCALE);
          if (int_deg >= BARCOUNT) int_deg = 0;
          float cachedd = histogram[int_deg];
          if (cachedd == 0.0f ) {
              cachedd = nodes[pos].distance_q2/4.0f;
          } else {
              cachedd = (nodes[pos].distance_q2/4.0f + cachedd)/2.0f;
          }
  
          if (cachedd > max_val) max_val = cachedd;
          histogram[int_deg] = cachedd;
      }
  
      for (int height = 0; height < MAXBARHEIGHT; ++height) {
          float threshold_h = (MAXBARHEIGHT - height - 1) * (max_val/MAXBARHEIGHT);
          for (int xpos = 0; xpos < BARCOUNT; ++xpos) {
              if (histogram[xpos] >= threshold_h) {
                  putc('*', stdout);
              }else {
                  putc(' ', stdout);
              }
          }
          printf("\n");
      }
      for (int xpos = 0; xpos < BARCOUNT; ++xpos) {
          putc('-', stdout);
      }
      printf("\n");
  }
  
  u_result capture_and_display(RPlidarDriver * drv)
  {
  	//define the ans variable as a unsigned result
      u_result ans;
  
  	//generate 8192 steps
      rplidar_response_measurement_node_t nodes[8192];
      size_t   count = _countof(nodes);
  
      printf("waiting for data...\n");
  
      // fetech extactly one 0-360 degrees' scan
      ans = drv->grabScanData(nodes, count);
      if (IS_OK(ans) || ans == RESULT_OPERATION_TIMEOUT) {
          drv->ascendScanData(nodes, count);
          plot_histogram(nodes, count);
  
          printf("Do you want to see all the data? (y/n) ");
          int key = getchar();
          if (key == 'Y' || key == 'y') {
              for (int pos = 0; pos < (int)count ; ++pos) {
                  printf("%s theta: %03.2f Dist: %08.2f \n", 
                      (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":"  ", 
                      (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
                      nodes[pos].distance_q2/4.0f);
              }
          }
      } else {
          printf("error code: %x\n", ans);
      }
  
      return ans;
  }
  
  int main(int argc, const char * argv[]) {
  	//comport path
      const char * opt_com_path = NULL;
  	//define a 32 bit unsigned int of 115200 for the baudrate
      _u32         opt_com_baudrate = 115200;
  	//32bit text character?
  	// todo: what is typedef??
      u_result     op_result;
  
  	
      //if the argument is less than 2 show the usage.
      if (argc < 2) {
          print_usage(argc, argv);
          return -1;
      }
  	//define the comport
      opt_com_path = argv[1];
  
  	//the 3rd argument gives 115200
      if (argc>2) opt_com_baudrate = strtoul(argv[2], NULL, 10);
  
      // create the driver instance
  	//create drive to connect use the Serial port
      RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
  
      if (!drv) {
          fprintf(stderr, "insufficent memory, exit\n");
          exit(-2);
      }
  
      rplidar_response_device_health_t healthinfo;
      rplidar_response_device_info_t devinfo;
      do {
          // try to connect
          if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
              fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
                  , opt_com_path);
              break;
          }
  
          // retrieving the device info
          ////////////////////////////////////////
          op_result = drv->getDeviceInfo(devinfo);
  
          if (IS_FAIL(op_result)) {
              if (op_result == RESULT_OPERATION_TIMEOUT) {
                  // you can check the detailed failure reason
                  fprintf(stderr, "Error, operation time out.\n");
              } else {
                  fprintf(stderr, "Error, unexpected error, code: %x\n", op_result);
                  // other unexpected result
              }
              break;
          }
  
          // print out the device serial number, firmware and hardware version number..
          printf("RPLIDAR S/N: ");
          for (int pos = 0; pos < 16 ;++pos) {
              printf("%02X", devinfo.serialnum[pos]);
          }
  
          printf("\n"
                  "Version: "RPLIDAR_SDK_VERSION"\n"
                  "Firmware Ver: %d.%02d\n"
                  "Hardware Rev: %d\n"
                  , devinfo.firmware_version>>8
                  , devinfo.firmware_version & 0xFF
                  , (int)devinfo.hardware_version);
  
  
          // check the device health
          ////////////////////////////////////////
          op_result = drv->getHealth(healthinfo);
          if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
              printf("RPLidar health status : ");
              switch (healthinfo.status) {
              case RPLIDAR_STATUS_OK:
                  printf("OK.");
                  break;
              case RPLIDAR_STATUS_WARNING:
                  printf("Warning.");
                  break;
              case RPLIDAR_STATUS_ERROR:
                  printf("Error.");
                  break;
              }
              printf(" (errorcode: %d)\n", healthinfo.error_code);
  
          } else {
              fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result);
              break;
          }
  
  
          if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
              fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
              // enable the following code if you want rplidar to be reboot by software
              // drv->reset();
              break;
          }
  
      	//start the motor at the default RPM which is 660
          drv->startMotor();
  
          // take only one 360 deg scan and display the result as a histogram
          ////////////////////////////////////////////////////////////////////////////////
          if (IS_FAIL(drv->startScan( 0,1 ))) // you can force rplidar to perform scan operation regardless whether the motor is rotating
          {
              fprintf(stderr, "Error, cannot start the scan operation.\n");
              break;
          }
  
          if (IS_FAIL(capture_and_display(drv))) {
              fprintf(stderr, "Error, cannot grab scan data.\n");
              break;
  
          }
  
      } while(0);
  
      drv->stop();
      drv->stopMotor();
  
      RPlidarDriver::DisposeDriver(drv);
      return 0;
  }
  