// #define EN_PIN 17   //  D1
// #define DIR_PIN 3   //  D2
// #define STEP_PIN 1  //  D3
// #define CS_PIN 16   // D8
// #define MOSI_PIN 23 //  D7
// #define MISO_PIN 19 //   D6
// #define SCK_PIN 18  //   D5

/*
 * You can control the rotation speed with
 * 0 Stop
 * 1 Resume
 * + Speed up
 * - Slow down
 */

#define MAX_SPEED 40 // In timer value
#define MIN_SPEED 1000

#define STALL_VALUE 0 // [-64..63]

// Note: You also have to connect GND, 5V and VM.
//       A connection diagram can be found in the schematics.
#define EN_PIN 17  //  D1
#define DIR_PIN 3  //  D2
#define STEP_PIN 1 //  D3
#define CS_PIN 16  // D8

#include <TMC2130Stepper.h>
#include <TMC2130Stepper_REGDEFS.h>
TMC2130Stepper driver = TMC2130Stepper(CS_PIN);

bool vsense;

uint16_t rms_current(uint8_t CS, float Rsense = 0.11)
{
    return (float)(CS + 1) / 32.0 * (vsense ? 0.180 : 0.325) / (Rsense + 0.02) / 1.41421 * 1000;
}

void setup()
{
    //init serial port
    {
        Serial.begin(250000); //init serial port and set baudrate
        while (!Serial)
            ; //wait for serial port to connect (needed for Leonardo only)
        Serial.println("\nStart...");
        pinMode(EN_PIN, OUTPUT);
        pinMode(DIR_PIN, OUTPUT);
        pinMode(STEP_PIN, OUTPUT);
        pinMode(CS_PIN, OUTPUT);
        digitalWrite(EN_PIN, HIGH); //deactivate driver (LOW active)
        digitalWrite(DIR_PIN, LOW); //LOW or HIGH
        digitalWrite(STEP_PIN, LOW);
        digitalWrite(CS_PIN, HIGH);
        SPI.begin();
        pinMode(MISO, INPUT_PULLUP);
    }

    //set TMC2130 config
    {
        driver.push();
        driver.toff(3);
        driver.tbl(1);
        driver.hysteresis_start(4);
        driver.hysteresis_end(-2);
        driver.rms_current(600); // mA
        driver.microsteps(16);
        driver.diag1_stall(1);
        driver.diag1_active_high(1);
        driver.coolstep_min_speed(0xFFFFF); // 20bit max
        driver.THIGH(0);
        driver.semin(5);
        driver.semax(2);
        driver.sedn(0b01);
        driver.sg_stall_value(STALL_VALUE);
    }

    // Set stepper interrupt
    {
        cli();       //stop interrupts
        TCCR1A = 0;  // set entire TCCR1A register to 0
        TCCR1B = 0;  // same for TCCR1B
        TCNT1 = 0;   //initialize counter value to 0
        OCR1A = 256; // = (16*10^6) / (1*1024) - 1 (must be <65536)
        // turn on CTC mode
        TCCR1B |= (1 << WGM12);
        // Set CS11 bits for 8 prescaler
        TCCR1B |= (1 << CS11); // | (1 << CS10);
        // enable timer compare interrupt
        TIMSK1 |= (1 << OCIE1A);
        sei(); //allow interrupts
    }

    //TMC2130 outputs on (LOW active)
    digitalWrite(EN_PIN, LOW);

    vsense = driver.vsense();
}

ISR(TIMER1_COMPA_vect)
{
    PORTF |= 1 << 0;
    PORTF &= ~(1 << 0);
}

void loop()
{
    static uint32_t last_time = 0;
    uint32_t ms = millis();

    while (Serial.available() > 0)
    {
        int8_t read_byte = Serial.read();
        if (read_byte == '0')
        {
            TIMSK1 &= ~(1 << OCIE1A);
            digitalWrite(EN_PIN, HIGH);
        }
        else if (read_byte == '1')
        {
            TIMSK1 |= (1 << OCIE1A);
            digitalWrite(EN_PIN, LOW);
        }
        else if (read_byte == '+')
            if (OCR1A > MAX_SPEED)
                OCR1A -= 20;
            else if (read_byte == '-')
                if (OCR1A < MIN_SPEED)
                    OCR1A += 20;
    }

    if ((ms - last_time) > 100) //run every 0.1s
    {
        last_time = ms;
        uint32_t drv_status = driver.DRV_STATUS();
        Serial.print("0 ");
        Serial.print((drv_status & SG_RESULT_bm) >> SG_RESULT_bp, DEC);
        Serial.print(" ");
        Serial.println(rms_current((drv_status & CS_ACTUAL_bm) >> CS_ACTUAL_bp), DEC);
    }
}

// bool dir = true;

// #include <TMC2130Stepper.h>
// TMC2130Stepper TMC2130 = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN);

// void setup() {
// 	Serial.begin(9600);
// 	TMC2130.begin(); // Initiate pins and registeries
// 	TMC2130.SilentStepStick2130(600); // Set stepper current to 600mA
// 	TMC2130.stealthChop(1); // Enable extremely quiet stepping

// 	digitalWrite(EN_PIN, LOW);
// }

// void loop() {
// 	digitalWrite(STEP_PIN, HIGH);
// 	delayMicroseconds(10);
// 	digitalWrite(STEP_PIN, LOW);
// 	delayMicroseconds(10);
// 	uint32_t ms = millis();
// 	static uint32_t last_time = 0;
// 	if ((ms - last_time) > 2000) {
// 		if (dir) {
// 			Serial.println("Dir -> 0");
// 			TMC2130.shaft_dir(0);
// 		} else {
// 			Serial.println("Dir -> 1");
// 			TMC2130.shaft_dir(1);
// 		}
// 		dir = !dir;
// 		Serial.println(TMC2130.GCONF(), BIN);
// 		last_time = ms;
// 	}
// }

// bool dir = true;
// #define TMC2130DEBUG
// #include <TMC2130Stepper.h>
// // Soft SPI
// TMC2130Stepper driver = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN, MOSI_PIN, MISO_PIN, SCK_PIN);
// // Hard SPI
// //TMC2130Stepper driver = TMC2130Stepper(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN);
// void setup()
// {
//     Serial.begin(9600);
//     while (!Serial)
//         ;
//     Serial.println("Start...");
//     driver.begin();           // Initiate pins and registeries
//     driver.rms_current(1200); // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5);
//     driver.microsteps(16);
//     driver.stealthChop(1); // Enable extremely quiet stepping

//     digitalWrite(EN_PIN, LOW);

//     Serial.print("DRV_STATUS=0b");
//     Serial.println(driver.DRV_STATUS(), BIN);
// }
// bool isKilled = false;

// void loop()
// {
//     if (driver.checkOT())
//     {
//         if (!isKilled)
//         {
//             Serial.print("Overheat flag triggered: ");
//             Serial.println(driver.getOTPW(), DEC);
//             driver.TPOWERDOWN();
//             isKilled = true;
//             Serial.println("Program stopped");
//         }
//         else
//         {
//             delay(1000);
//         }
//     }
//     else
//     {
//         digitalWrite(STEP_PIN, HIGH);
//         delayMicroseconds(10);
//         digitalWrite(STEP_PIN, LOW);
//         delayMicroseconds(10);

//         uint32_t ms = millis();
//         static uint32_t last_time = 0;
//         if ((ms - last_time) > 500)
//         {
//             if (dir)
//             {
//                 Serial.println("Dir -> 0");
//                 driver.shaft_dir(0);
//             }
//             else
//             {
//                 Serial.println("Dir -> 1");
//                 driver.shaft_dir(1);
//             }
//             Serial.print("OverTemperature: ");
//             Serial.print(driver.getOTPW(), DEC);
//             Serial.print(" MSTEP: ");
//             Serial.print(driver.microsteps(), DEC);
//             Serial.print(" DIR: ");
//             Serial.println(driver.dir(), DEC);
//             Serial.println("---");
//             //Serial.println(driver.checkStatus());
//             //driver.checkStatus();

//             dir = !dir;
//             last_time = ms;
//         }
//     }
// }