#include <Arduino.h>
#include <avr_functions.h>
#include <StepControl.h>

#define PIN_LED                 13
#define PIN_ENABLE              21
#define PIN_DIR                 22

#define MOTORS                  4
#define CONTROLLERS             4

#define LED_TOGGLE_INTERVAL     1000

Stepper mot0(2, PIN_DIR);
Stepper mot1(3, PIN_DIR);
Stepper mot2(4, PIN_DIR);
Stepper mot3(5, PIN_DIR);

StepControl<> motCtrl0;
StepControl<> motCtrl1;
StepControl<> motCtrl2;
StepControl<> motCtrl3;

Stepper* motors[] = {&mot0, &mot1, &mot2, &mot3};
StepControl<>* controllers[] = {&motCtrl0, &motCtrl1, &motCtrl2, &motCtrl3};

uint32_t lastLEDToggle = 0;


void setup() {
    Serial.begin(115200);

    pinMode(PIN_LED, OUTPUT);
    pinMode(PIN_ENABLE, OUTPUT);
    pinMode(PIN_DIR, OUTPUT);
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);
    pinMode(4, OUTPUT);
    pinMode(5, OUTPUT);
    
    digitalWriteFast(PIN_LED, 0);

    for (int i = 0; i < MOTORS; i++)
        mot0.setMaxSpeed(5000);

    while (! Serial.available()) {
        Serial.println("Press any key to start");
        delay(1000);
    }
    
    status();
    
    Serial.println("Synchronous...");
    
    for (int i = 0; i < MOTORS; i++)
        moveMotor(i);
    
    status();
    
    Serial.println("Asynchronous...");
    
    for (int i = 0; i < MOTORS; i++) {
        moveMotorAsync(i);
        delay(500);
    }
    
    status();
    
    Serial.println("Looping...");
}

void loop() {
    if ((millis() - lastLEDToggle) >= LED_TOGGLE_INTERVAL) {
        lastLEDToggle = millis();
        digitalWriteFast(PIN_LED, ! digitalReadFast(PIN_LED));
        status();
    }
}

void moveMotor(int i) {
    Serial.print("Motor ");
    Serial.println(i);
    motors[i]->setTargetRel(1000);
    controllers[i]->move(*motors[i]);
}
    
void moveMotorAsync(int i) {
    Serial.print("Motor ");
    Serial.println(i);
    motors[i]->setTargetRel(100000);
    controllers[i]->moveAsync(*motors[i]);
}
    
void status() {
    for (int i = 0; i < CONTROLLERS; i++) {
        Serial.print("controller ");
        Serial.print(i);
        Serial.print(' ');
        Serial.print(controllers[i]->isOk() ? "Ok" : "not Ok");
        Serial.print(' ');
        Serial.print(controllers[i]->isRunning() ? "running" : "not running");
        Serial.println();
    }
}