/**
 * A BLDC motor with step/dir interface to run a sherpa mini extruder
  * required: ESP32, simpleFOC-mini driver, 2805 140kV gimbal motor (MiToot)
 */

#include "Arduino.h"
#include "Wire.h"
#include "SPI.h"
#include <SimpleFOC.h>
#include "SimpleFOCDrivers.h"
#include "encoders/as5048a/MagneticSensorAS5048A.h"

#define SENSOR1_CS 2 // some digital pin that you're using as the nCS pin

// alternative pinout for esp32_SPI
#define VSPI_MISO 19
#define VSPI_MOSI 23
#define VSPI_SCLK 18

SPIClass SPI_1(VSPI);
MagneticSensorAS5048A sensor(SENSOR1_CS);

float start_angle;

BLDCMotor motor = BLDCMotor(7);
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver3PWM driver = BLDCDriver3PWM(13,12,14, 27);

// StepDirListener( step_pin, dir_pin, counter_to_value)
StepDirListener step_dir = StepDirListener(32, 33, 2.0f*_PI/200.0);
void onStep() { step_dir.handle(); }

void setup() {

  // start the newly defined spi communication
  SPI_1.begin(VSPI_SCLK, VSPI_MISO, VSPI_MOSI, SENSOR1_CS); //SCLK, MISO, MOSI, SS

  // initialise magnetic sensor hardware
  sensor.init(&SPI_1);
   // link the motor to the sensor
  motor.linkSensor(&sensor);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);  

  // set motion control loop to be used
  motor.controller = MotionControlType::angle;

  // controller configuration
  // default parameters in defaults.h
  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2f;
  motor.PID_velocity.I = 20;
  motor.PID_velocity.D = 0;
  //default voltage_power_supply
  motor.voltage_limit = 0.5* driver.voltage_power_supply;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;

  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01f;

  // angle P controller
  motor.P_angle.P = 10;
  //  maximal velocity of the position control
  motor.velocity_limit = 100;

  // use monitoring with serial
  //Serial.begin(115200);
  // comment out if not needed
  //motor.useMonitoring(Serial);
  //_delay(3000);
  // initialize motor
  motor.init();
  start_angle = sensor.getAngle(); 
  motor.target = start_angle;
  //Serial.println(start_angle);
    // align encoder and start FOC
  motor.initFOC(4.75, Direction::CW);	//	put your own zero_angle here. Find it with `motor.useMonitoring` enabled
  //motor.initFOC();

  // init step and dir pins
  step_dir.init();
  // enable interrupts
  step_dir.enableInterrupt(onStep);

  //Serial.println(F("Motor ready."));
  //Serial.println(F("Listening to step/dir commands!"));
  _delay(1000);
}

void loop() {

  // main FOC algorithm function
  motor.loopFOC();
  
  //motor.monitor();
  // Motion control function
  motor.move(start_angle + step_dir.getValue());
}