#pragma once

#if CORE_MOCK
class AccelStepper
{
};
#else
#include <AccelStepper.h>
#endif

class Motor
{
public:

    using MotorHardware_t = AccelStepper;

    Motor (AccelStepper& stepper): stepper(stepper)
    {
        setSpeedMmPerSec(1);
        setAccelMmPerSecPerSec(0.01); //  <----- this is not yet configured by user
    }

    void setPhysical (int stepsPerRevolution, float mmPerRevolution)
    {
        this->stepsPerRevolution = stepsPerRevolution;
        this->mmPerRevolution = mmPerRevolution;
    }
    
    void setSpeedMmPerSec (float mmPerSec)
    {
        Serial.printf("motor: set speed: %g mm/s\n", mmPerSec);
        Serial.printf("motor: set speed: %g st/s\n", mmToStep(mmPerSec));
#if !CORE_MOCK
        stepper.setMaxSpeed(mmToStep(mmPerSec));
#endif
    }

    void setAccelMmPerSecPerSec (float mmPerSecPerSec)
    {
        Serial.printf("motor: set accel: %g mm/s/s\n", mmPerSecPerSec);
        Serial.printf("motor: set accel: %g st/s/s\n", mmToStep(mmPerSecPerSec));
        _retainAccel = mmPerSecPerSec;
#if !CORE_MOCK
        stepper.setAcceleration(mmToStep(mmPerSecPerSec));
#endif
    }
    
    float mmToStep (float mm)
    {
        return mm * stepsPerRevolution / mmPerRevolution;
    }

    int where ()
    {
#if CORE_MOCK
        return 0;
#else
        return stepper.currentPosition();
#endif
    }

    void moveToMm (float mm)
    {
        Serial.printf("motor: move to: %g mm\n", mm);
        moveToStep(mmToStep(mm));
    }
    
    void moveToStep (int step)
    {
        Serial.printf("motor: move to: %d step\n", step);
        Serial.printf("motor: currently at: %ld step\n", stepper.currentPosition());
#if !CORE_MOCK
        cli();
        setAccelMmPerSecPerSec(_retainAccel);
        stepper.moveTo(step);
        sei();
#endif
    }

    void stop ()
    {
#if !CORE_MOCK
        cli();
        stepper.stop();
        sei();
#endif
    }

	void reset()
	{
#if !CORE_MOCK
        stop();
        cli();
	    stepper.setCurrentPosition(0);
	    sei();
#endif
	}
	
	void stayHere ()
	{
#if !CORE_MOCK
        cli();
        stepper.setAcceleration(1e20);
	    stepper.moveTo(stepper.currentPosition()); // change target to here
        setAccelMmPerSecPerSec(_retainAccel);
	    sei();
#endif
	}

protected:

  	int stepsPerRevolution = 1;
   	float mmPerRevolution = 1;
   	float _retainAccel = -1;

    AccelStepper& stepper;
};