// LiDAR Sensor ESP8266 NodeMCU
// T2C LiDAR pin GPIO22 D22=SCL, GPIO21 D21=SDA on ESP32
// T2C LiDAR pin GPIO5 D1=SCL, GPIO4 D2=SDA  on ESP8266



#include <Arduino.h>
#include <Wire.h>        // Instantiate the Wire library 
#include <TFLI2C.h>      // TFLuna-I2C Library v.0.1.1
#include <AccelStepper.h>

TFLI2C tflI2C;   //**************************

//Stepper Motor Inputs
int Step = 14; //GPIO14---D5 of Nodemcu--Step of stepper motor driver
int Dire  = 2; //GPIO2---D4 of Nodemcu--Direction of stepper motor driver
int Sleep = 12; //GPIO12---D6 of Nodemcu-Control Sleep Mode on A4988
int n = 0; //Will be used to tell the motor how many steps to move
int MS1 = 13; //GPIO13---D7 of Nodemcu--MS1 for A4988 //********************
int MS2 = 16; //GPIO16---D0 of Nodemcu--MS2 for A4988 //*******************
int MS3 = 15; //GPIO15---D8 of Nodemcu--MS3 for A4988 //****************
#define motorInterfaceType 1
AccelStepper stepper = AccelStepper(motorInterfaceType, Step, Dire);

//Motor Specs
const float spr = 360; //Steps per revolution // 116.95 **************************
int Steps_per_sec = 1000; //Motor Speed in steps/sec  1500 ************************


//Lidar Sensor Inputs
int16_t  tfDist;    // distance in centimeters
int16_t  tfAddr = TFL_DEF_ADR;  // Use this default I2C address
const float Pi = 3.14159;
float angle = 0;
int pos2 = 0;

void setup() {
  Serial.begin(115200);
  //Set Pin Modes
  pinMode(Step, OUTPUT); //Step pin as output
  pinMode(Dire, OUTPUT); //Direcction pin as output
  pinMode(Sleep, OUTPUT); //Set Sleep OUTPUT Control button as output

  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT); //**********

  //Set Pin setting for some pins
  digitalWrite(Step, LOW); // Currently no stepper motor movement
  digitalWrite(Dire, LOW); // Set stepper motor rotation direction (set to HIGH for opposite direction)
  digitalWrite(MS1, LOW);  //Set MS1, MS2, and MS3 to LOW to enable full steps (no microstepping)
  digitalWrite(MS2, LOW);  //Set MS1, MS2, and MS3 to LOW to enable full steps (no microstepping)
  digitalWrite(MS3, LOW);  //Set MS1, MS2, and MS3 to LOW to enable full steps (no microstepping) ///********
  //Wire begin
  Wire.begin(); // Initalize Wire library
  //Stepper Motor Info
  stepper.setMaxSpeed(3000); //Max speed in steps/sec // 3000
  stepper.setCurrentPosition(0);
  stepper.setSpeed(Steps_per_sec);
}

void loop() {
    digitalWrite(Sleep, HIGH); //A logic high allows normal operation of the A4988 by removing from sleep
    n = pos2;
    if (n >= spr) {
      n = pos2 % int(spr);
    }  //% operator provides the remainder, so this will provide the remainder after dividing pos2 by the number of steps per revolution
    
    angle = 360*(float(n)/spr);  
    //Serial.println(angle);  //*************************
    stepper.runSpeed();
    pos2 = stepper.currentPosition();

    Serial.println(String(angle)+" angle / Lidar= "+String(tfDist)+" / counter= "+String(n));

      if(tflI2C.getData(tfDist, tfAddr)) {
        Serial.println(String(angle*Pi/180)+"p"+String(tfDist)+"p"+String(n));
      }    
    }





//////////////////////////////////////////////////////////////////////////////////////
///****************Example LiDAR Sensor.......................
/*

#include <Arduino.h>
#include <Wire.h>        // Instantiate the Wire library
#include <TFLI2C.h>      // TFLuna-I2C Library v.0.1.1

TFLI2C tflI2C;

int16_t  tfDist;    // distance in centimeters
int16_t  tfAddr = TFL_DEF_ADR;  // Use this default I2C address

void setup(){
    Serial.begin(115200);  // Initalize serial port
    Wire.begin();           // Initalize Wire library
}

void loop(){
  
    if(tflI2C.getData(tfDist, tfAddr)){
        Serial.println(String(tfDist)+" cm / " + String(tfDist/2.54)+" inches");
    }
    delay(50);
}
///////////////////////////////////////////////////////////////////////////////
*/




/*
/////////////////////

// Processing IDE Code   // LiDAR Sensor ESP8266 NodeMCU

////////////////////////////////////////////////


ArrayList <PVector> points = new ArrayList <PVector> ();

import processing.serial.*;
Serial myPort;  // The serial port
int lf = 10;    // Linefeed in ASCII
String myString = null;
float x,y,angle,r,n;
float f = 5;

void setup() {
  // List all the available serial ports
  printArray(Serial.list());
  // Open the port you are using at the rate you want:
  myPort = new Serial(this, Serial.list()[0], 115200);
    
  size(600,600);
  points.add(new PVector(0,0,0));
}

void draw() {
  while (myPort.available() > 0){
    myString = myPort.readStringUntil(10);
    if (myString != null) {
      String[] q = splitTokens(myString, "p");
      if (q.length>1){
        angle=float(q[0]);  // Converts and prints float
        r = float(q[1]);  // Converts and prints float
        n = float(q[2]);
        x = (f*sin(angle)*r + 300);
        y = (f*cos(angle)*r + 300);
      }
    }
        
    background(0);
    PVector p_last = points.get(points.size()-1);
    
    if (n != p_last.z){points.add(new PVector(x,y,n));}
    
    while (points.size() > 117) {
      points.remove(0);
    }
    
    for (int i=1; i<points.size(); i++) {
      PVector p1 = points.get(i-1);
      PVector p2 = points.get(i);
      stroke(0,255,0); // line color: no fill, green stroke
      strokeWeight(5);
      line(p1.x,p1.y, p2.x,p2.y);
    }
    PVector p3 = points.get(points.size()-1);
    stroke(255,0,0); // ellipse color: default white fill, red stroke
    strokeWeight(20);
    ellipse(p3.x,p3.y,15,15);
  }
}




*/