// Moonlite-compatible stepper controller
//
// Uses AccelStepper (www.airspayce.com/mikem/arduino/AccelStepper/)

#include <TMC2130Stepper.h>
#include <AccelStepper.h>
#include <IRremote.h>

#define EnPin 0 // Nano v3
#define DirPin 1
#define StepPin 2
#define CsPin 7
#define TemperaturePin A4
#define TempAvgs 10


TMC2130Stepper driver = TMC2130Stepper(EnPin, DirPin, StepPin, CsPin);
AccelStepper stepper = AccelStepper(stepper.DRIVER, StepPin, DirPin);

const int RECV_PIN = 6;
IRrecv irrecv(RECV_PIN);
decode_results results;
unsigned long key_value = 0;

#define MaxSpeed 20000 //Maximum stepper speed in steps/s
#define MaxAcc MaxSpeed * 4 //Maximum stepper acceleration in steps/s/s
#define HoldTime 5
#define Direction false //Set the focuser direction using true or false
#define RunCurrent 500
#define HoldCurrent 15

//#define SPEEDMULT 3
#define MAXCOMMAND 8

char inChar;
char cmd[MAXCOMMAND];
char param[MAXCOMMAND];
char line[MAXCOMMAND];
long pos;
long Temperature = 100;
int isRunning = 0;
int Speed = 2000;
int eoc = 0;
int idx = 0;
int TempSum = 0;
long millisLastMove = 0;
float MeasVolt = 0;

void setup() {
Serial.begin(38400);
SPI.begin();

pinMode(CsPin, OUTPUT);
digitalWrite(CsPin, HIGH);
driver.begin(); // Initiate pins and registeries
driver.rms_current(RunCurrent); // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5);
driver.stealthChop(1); // Enable extremely quiet stepping
driver.stealth_autoscale(1);
driver.microsteps(2);

//stepper.setSpeed(Speed);
stepper.setMaxSpeed(Speed);
stepper.setAcceleration(MaxAcc);
stepper.setEnablePin(EnPin);
stepper.setPinsInverted(false, false, true);
stepper.enableOutputs();

irrecv.enableIRIn();

memset(line, 0, MAXCOMMAND);
millisLastMove = millis();
analogReadResolution(12);
}



void loop(){
// run the stepper if there's no pending command and if there are pending movements
if(!Serial.available()){
if(isRunning){
driver.rms_current(RunCurrent);
stepper.run();
millisLastMove = millis();
}
else{
if((millis() - millisLastMove) > HoldTime){
driver.rms_current(HoldCurrent);
//stepper.disableOutputs();
}
}

if(stepper.distanceToGo() == 0){
stepper.run();
isRunning = 0;
}
}
else{
// read the command until the terminating # character
while(Serial.available() && !eoc){
inChar = Serial.read();
if(inChar != '#' && inChar != ':'){
line[idx++] = inChar;
if(idx >= MAXCOMMAND){
idx = MAXCOMMAND - 1;
}
}
else{
if(inChar == '#'){
eoc = 1;
}
}
}
}

if (irrecv.decode(&results)){
if (results.value == 0XFFFFFFFF)
results.value = key_value;
switch(results.value){
case 0xFFA25D:
Speed -= 500;
if (Speed <= 0){
Speed = 100;
}
delay(100);
stepper.setMaxSpeed(Speed);
break;

case 0xFFE21D:
Speed += 500;
if (Speed > MaxSpeed){
Speed = MaxSpeed;
}
delay(100);
stepper.setMaxSpeed(Speed);
break;

case 0xFF22DD:
pos -= 100;
if (pos < 0){
pos = 0;
}
delay(100);
stepper.moveTo(pos);
isRunning = 1;
break;

case 0xFFC23D:
pos += 100;
if (pos > 100000){
pos = 100000;
}
delay(100);
stepper.moveTo(pos);
isRunning = 1;
break;
}
key_value = results.value;
irrecv.resume();
}

// process the command we got
if (eoc) {
memset(cmd, 0, MAXCOMMAND);
memset(param, 0, MAXCOMMAND);

int len = strlen(line);
if(len >= 2){
strncpy(cmd, line, 2);
}
if(len > 2){
strncpy(param, line + 2, len - 2);
}

memset(line, 0, MAXCOMMAND);
eoc = 0;
idx = 0;

// the stand-alone program sends :C# :GB# on startup
// :C# is a temperature conversion, doesn't require any response

// LED backlight value, always return "00"
if (!strcasecmp(cmd, "GB")) {
Serial.print("00#");
}

// home the motor, hard-coded, ignore parameters since we only have one motor
if (!strcasecmp(cmd, "PH")) {
stepper.setCurrentPosition(8000);
stepper.moveTo(0);
isRunning = 1;
}

// firmware value, always return "10"
if (!strcasecmp(cmd, "GV")) {
Serial.print("01#");
}

// get the current motor position
if (!strcasecmp(cmd, "GP")) {
pos = stepper.currentPosition();
char tempString[6];
sprintf(tempString, "%04X", pos);
Serial.print(tempString);
Serial.print("#");
}

// get the new motor position (target)
if (!strcasecmp(cmd, "GN")) {
pos = stepper.targetPosition();
char tempString[6];
sprintf(tempString, "%04X", pos);
Serial.print(tempString);
Serial.print("#");
}

// get the current temperature, hard-coded
if (!strcasecmp(cmd, "GT")) {
if(!isRunning){
TempSum = 0;
for(uint8_t i=0; i < TempAvgs; i++){
TempSum += analogRead(TemperaturePin);
delayMicroseconds(200);
}
MeasVolt = 3.3*TempSum / (4095 * TempAvgs);
Temperature = long((MeasVolt - 0.5) * 200);
if(Temperature < 0){
Temperature += 65536;
}
}
char tempString[6];
sprintf(tempString, "%04X", Temperature);
Serial.print(tempString);
Serial.print("#");
}


// get the temperature coefficient, hard-coded
if (!strcasecmp(cmd, "GC")) {
Serial.print("01#");
}

// get the current motor speed, only values of 02, 04, 08, 10, 20
if (!strcasecmp(cmd, "GD")) {
char tempString[4];
sprintf(tempString, "%02X", Speed);
Serial.print(tempString);
Serial.print("#");
}

// set speed, only acceptable values are 02, 04, 08, 10, 20
if (!strcasecmp(cmd, "SD")) {
Speed = hexstr2long(param);

stepper.setSpeed(Speed);
stepper.setMaxSpeed(Speed);
}

// whether half-step is enabled or not, always return "00"
if (!strcasecmp(cmd, "GH")) {
Serial.print("00#");
}

// motor is moving - 01 if moving, 00 otherwise
if (!strcasecmp(cmd, "GI")) {
if (abs(stepper.distanceToGo()) > 0) {
Serial.print("01#");
}
else {
Serial.print("00#");
}
}

// set current motor position
if (!strcasecmp(cmd, "SP")) {
pos = hexstr2long(param);
stepper.setCurrentPosition(pos);
}

// set new motor position
if (!strcasecmp(cmd, "SN")) {
pos = hexstr2long(param);
stepper.moveTo(pos);
}


// initiate a move
if (!strcasecmp(cmd, "FG")) {
driver.rms_current(RunCurrent);
//stepper.enableOutputs();
isRunning = 1;
}

// stop a move
if (!strcasecmp(cmd, "FQ")) {
stepper.moveTo(stepper.currentPosition());
stepper.run();
isRunning = 0;
}
}
}

long hexstr2long(char *line) {
long ret = 0;
ret = strtol(line, NULL, 16);
return (ret);
}