#include <Arduino.h>
#include <AccelStepper.h>

#include "config.h"
#include "config_pins.h"
#include "steppers.h"

static AccelStepper stepper_x(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN, 0, 0, false);
static AccelStepper stepper_y(AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN, 0, 0, false);
static AccelStepper stepper_z(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN, 0, 0, false);
static AccelStepper stepper_e(AccelStepper::DRIVER, E0_STEP_PIN, E0_DIR_PIN, 0, 0, false);

enum stepper_states {
    step_disabled,
    step_not_homed,
    step_homing_x_fast,
    step_homing_x_back,
    step_homing_x_slow,
    step_homing_y_fast,
    step_homing_y_back,
    step_homing_y_slow,
    step_homing_z_fast,
    step_homing_z_back,
    step_homing_z_slow,
    step_homing_e_fast,
    step_homing_e_back,
    step_homing_e_slow,
    step_homed
};

static stepper_states state = step_disabled;

void steppers_init(void) {
    stepper_x.setEnablePin(X_ENABLE_PIN);
    stepper_x.setMaxSpeed(XY_MAX_SPEED * XY_STEPS_PER_MM);
    stepper_x.setAcceleration(XY_MAX_ACCEL * XY_STEPS_PER_MM);

    stepper_y.setEnablePin(Y_ENABLE_PIN);
    stepper_y.setMaxSpeed(XY_MAX_SPEED * XY_STEPS_PER_MM);
    stepper_y.setAcceleration(XY_MAX_ACCEL * XY_STEPS_PER_MM);

    stepper_z.setEnablePin(Z_ENABLE_PIN);
    stepper_z.setMaxSpeed(Z_MAX_SPEED * Z_STEPS_PER_MM);
    stepper_z.setAcceleration(Z_MAX_ACCEL * Z_STEPS_PER_MM);

    stepper_e.setEnablePin(E0_ENABLE_PIN);
    stepper_e.setMaxSpeed(E_MAX_SPEED * E_STEPS_PER_MM);
    stepper_e.setAcceleration(E_MAX_ACCEL * E_STEPS_PER_MM);
}

bool steppers_run(void) {
    if (state == step_homing_x_fast) {
    } else if (state == step_homing_x_back) {
    } else if (state == step_homing_x_slow) {
    } else if (state == step_homing_y_fast) {
    } else if (state == step_homing_y_back) {
    } else if (state == step_homing_y_slow) {
    } else if (state == step_homing_z_fast) {
    } else if (state == step_homing_z_back) {
    } else if (state == step_homing_z_slow) {
    } else if (state == step_homing_e_fast) {
    } else if (state == step_homing_e_back) {
    } else if (state == step_homing_e_slow) {
    }

    boolean x = stepper_x.run();
    boolean y = stepper_y.run();
    boolean z = stepper_z.run();
    boolean e = stepper_e.run();

    return x || y || z || e;
}

bool steppers_homed(void) {
    return (state == step_homed);
}

void steppers_start_homing(void) {
    state = step_homing_x_fast;

}

static int steppers_move_axis(AccelStepper &axis, long pos) {
    if (state == step_disabled) {
        Serial.println(F("Enabling stepper drivers"));

        stepper_x.enableOutputs();
        stepper_y.enableOutputs();
        stepper_z.enableOutputs();
        stepper_e.enableOutputs();
        state = step_not_homed;
    }

    axis.moveTo(pos);
}

int steppers_move_x(long pos) {
    Serial.print(F("Moving X to "));
    Serial.print(pos);
    Serial.print(F(" mm ("));
    Serial.print(pos * XY_STEPS_PER_MM);
    Serial.println(F(" steps)"));

    return steppers_move_axis(stepper_x, pos * XY_STEPS_PER_MM);
}

int steppers_move_y(long pos) {
    Serial.print(F("Moving Y to "));
    Serial.print(pos);
    Serial.print(F(" mm ("));
    Serial.print(pos * XY_STEPS_PER_MM);
    Serial.println(F(" steps)"));

    return steppers_move_axis(stepper_y, pos * XY_STEPS_PER_MM);
}

int steppers_move_z(long pos) {
    Serial.print(F("Moving Z to "));
    Serial.print(pos);
    Serial.print(F(" mm ("));
    Serial.print(pos * Z_STEPS_PER_MM);
    Serial.println(F(" steps)"));

    return steppers_move_axis(stepper_z, pos * Z_STEPS_PER_MM);
}

int steppers_move_e(long pos) {
    Serial.print(F("Moving E to "));
    Serial.print(pos);
    Serial.print(F(" mm ("));
    Serial.print(pos * E_STEPS_PER_MM);
    Serial.println(F(" steps)"));

    return steppers_move_axis(stepper_e, pos * E_STEPS_PER_MM);
}