/*
*******************************************************************************
Christian SIMON, 
FabLabSU
Sorbonne Universite
* Date: 2024/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
*******************************************************************************
*/
#include "M5Unified.h"
#include "Module_Stepmotor.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

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

#endif

//#define VERSION_1_0

String inputString  = "";
bool stringComplete = false;

static Module_Stepmotor driver;

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

void setup() {
    M5.begin();
    Wire.begin(21, 22, 400000UL);
    driver.init(Wire);

#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, 0, 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, 0);
    digitalWrite(0, 1);  // initialize in upward direction (Vz > 0)
}
 /*
 // syntax: 
 // change direction:    
digitalWrite(motor, step_dir); motor = 17, 13, 0 (X,-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 uint8_t step_dir                                  = 0;
    static uint8_t reset_mtr                                 = 0;
    static Module_Stepmotor::MicrostepResolution_t micro_res = Module_Stepmotor::kMicrosteps32;
    Serial1.print("Y");
    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);
    }
    
    // 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);
    }
}