/*
*******************************************************************************
Christian SIMON, 
FabLabSU
Sorbonne Universite
* Date: 2025/01/24
M5Stack Basic + Step Motor Module (tested with v1 leaving VERSION_1_0 commented)
XRF bench automation
Documentation :
https://wiki.fablab.sorbonne-universite.fr/BookStack/books/motorisation-banc-de-mesure
*******************************************************************************
Some code parts taken from various examples:
- StepmotorPulse_M039
- vl53l0x
- SerialEvent by Tom Igoe 
*/
#include "M5Unified.h"
#include "Module_Stepmotor.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "Adafruit_VL53L0X.h"


#if defined ( ARDUINO )
#include <Arduino.h>

#endif

//#define VERSION_1_0

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

String inputString  = "";
bool stringComplete = false;

static Module_Stepmotor driver;

int period_min = 25;
int period_max = 400;
int period = period_min;

/*
  SerialEvent occurs whenever a new data comes in the hardware serial RX. This
  routine is run between each time loop() runs, so using delay inside loop can
  delay response. Multiple bytes of data may be available.
*/

void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void setup() {
	M5.begin();
    Serial.begin(115200);
  	// reserve 200 bytes for the inputString:
  	inputString.reserve(200);


	M5.Lcd.setTextSize(2);

	Wire.begin(21, 22, 400000UL);
    driver.init(Wire);

    Serial.println("Adafruit VL53L0X test");
        if (!lox.begin()) {
        Serial.println(F("Failed to boot VL53L0X"));
        while(1);
    }
  // power 
  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 



#ifndef VERSION_1_0
    driver.resetMotor(0, 0);
    driver.resetMotor(1, 0);
    driver.resetMotor(2, 0);
#else
    driver.setMicrostepResolution(DIRECT_STEPMOTOR::kMicrosteps32);
#endif

    driver.enableMotor(1);
    Serial1.begin(115200, SERIAL_8N1, 35, 5);
    Serial2.begin(115200, SERIAL_8N1, 34, 26);
    Serial2.setTimeout(100);

    ledcAttach(16, 15000, 8); // Args: Channel, Freq, Resolution
    ledcAttach(12, 15000, 8);
    ledcAttach(15, 15000, 8);
    ledcWrite(16, 0);    // X axis
    ledcWrite(12, 0);    // "Y" = -X
    ledcWrite(15, 0);    // Z

    pinMode(17, OUTPUT); // X axis
    pinMode(13, OUTPUT); // "Y" = -X
    pinMode(0, OUTPUT);  // Z
 
    digitalWrite(17, 0);
    digitalWrite(13, 1);
    digitalWrite(0, 1);  // initialize in upward direction (Vz > 0)
}
 /*
 // syntax: 
 // change direction:    
digitalWrite(motor, step_dir); motor = 17, 13, 0 (X,1-X, Z respectively)
ledcWrite(motor, 0);    // disable
ledcWrite(motor, 1);    // enable
uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution); // for speed change 
//
*/



void loop() {


	static String commandString;
//    static uint8_t step_dir                                  = 0;
//    static uint8_t reset_mtr                                 = 0;
    static Module_Stepmotor::MicrostepResolution_t micro_res = Module_Stepmotor::kMicrosteps32;
    Serial1.print("Y"); // Qu'est-ce que c'est ?
    M5.update();

/*    
    if (Serial2.available()) {
        char inChar = (char)Serial2.read();
        if (inChar == 'Y') {
            M5.Lcd.fillRect(105, 180, 120, 20, TFT_GREEN);
        }
    } else {
        M5.Lcd.fillRect(105, 180, 120, 20, TFT_RED);
    }
*/  
    if (stringComplete) {
		Serial.println("received ");
		Serial.println(inputString);
		Serial.println("\n");
		M5.Lcd.drawString("received", 0, 10, 2);
		M5.Lcd.drawString(inputString, 0, 30, 2);
		// copy the received string for later analysis
		commandString = "" + inputString;
		commandString.trim();
    	// clear the string:
    	inputString = "";
    	stringComplete = false;
  	}

	if (commandString == "IZ" ) {
 		commandString = "";
        ledcWrite(16,0);
        ledcWrite(12,0);
        ledcWrite(15,0);
      	digitalWrite(0, 1);  // +Z direction
      	ledcWrite(15,1);
      	driver.getExtIOStatus();
      	while(driver.ext_io_status[1]) {
     		  driver.getExtIOStatus();
	  	}
		ledcWrite(15,0);
		digitalWrite(0, 0);  // -Z direction
	  	M5.Lcd.drawString("reached Z=0", 0, 40, 2);
		commandString = "D";
    }
  
  if (commandString == "+Z" ) {
      ledcWrite(16,0);
      ledcWrite(12,0);
      ledcWrite(15,0);
      commandString = "";
      digitalWrite(0, 1);
      ledcWrite(15,1);
	}

  if (commandString == "-Z" ) {
      ledcWrite(16,0);
      ledcWrite(12,0);
      ledcWrite(15,0);
      commandString = "";
      digitalWrite(0, 0);
      ledcWrite(15,1);
	}

  if (commandString == "+X" ) {
      ledcWrite(16,0);
      ledcWrite(12,0);
      ledcWrite(15,0);
      commandString = "";
      digitalWrite(17, 1);
      digitalWrite(13, 0);
      ledcWrite(16,1);
      ledcWrite(12,1);
	}
	
  if (commandString == "-X" ) {
      ledcWrite(16,0);
      ledcWrite(12,0);
      ledcWrite(15,0);
      commandString = "";
      digitalWrite(17, 0);
      digitalWrite(13, 1);
      ledcWrite(16,1);
      ledcWrite(12,1);
	}

  if (commandString == "S" ) {
      commandString = "";
      ledcWrite(16,0);
      ledcWrite(12,0);
      ledcWrite(15,0);
	
	}
	
    if (commandString == "D" ) {
    commandString = "";
	VL53L0X_RangingMeasurementData_t measure;
    
  	Serial.print("Reading a measurement... ");
	M5.Lcd.drawString("Reading...", 0, 0, 2);
  	lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

  	if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    	Serial.print("Distance (mm): "); 
		Serial.println(measure.RangeMilliMeter);
		M5.Lcd.drawString("D mm", 20, 20, 2);
		M5.Lcd.drawString(String(measure.RangeMilliMeter), 40, 40, 2);
  		} else {
    	Serial.println(" out of range ");
		 M5.Lcd.drawString("out of range", 260, 220, 2);
  		}
	}

    // test +X move
    if (M5.BtnA.wasPressed()) {
      digitalWrite(17, 1);
      digitalWrite(13, 0);
      ledcWrite(16,1);
      ledcWrite(12,1);
    }

    // test -X move
    if (M5.BtnB.wasPressed()) {
      digitalWrite(17, 0);
      digitalWrite(13, 1);
      ledcWrite(16,1);
      ledcWrite(12,1);
    }

    // stop
    if (M5.BtnC.wasPressed()) {
      ledcWrite(16,0);
      ledcWrite(12,0);
    }

    driver.getExtIOStatus();
    if (driver.ext_io_status[1]) {
        M5.Lcd.fillRect(300, 0, 20, 20, TFT_GREEN); // end-cours switch is free
    } else {
        M5.Lcd.fillRect(300, 0, 20, 20, TFT_RED); // end-course switch is pressed
    }
    if (driver.ext_io_status[2]) {
        M5.Lcd.fillRect(300, 21, 20, 20, TFT_GREEN); // end-cours switch is free
    } else {
        M5.Lcd.fillRect(300, 21, 20, 20, TFT_RED); // end-course switch is pressed
    }
}