#pragma once
#include <Arduino.h>

class Motor {
    private: GPIO_TypeDef* gpio;
    private: int pinMask, lastStepTime;
    private: int sequence[4], currentStep;
    private: int maxSteps, stepCount;

    public: Motor(GPIO_TypeDef* gpio,
        int pinOffset, int maxSteps) {
        /* Initialize sequence and mask */
        sequence[0] = 0x0003 << pinOffset;
        sequence[1] = 0x0006 << pinOffset;
        sequence[2] = 0x000C << pinOffset;
        sequence[3] = 0x0009 << pinOffset;
        pinMask = 0x000F << pinOffset;

        /* Set initial values */
        lastStepTime = millis();
        this -> maxSteps = maxSteps;
        this -> gpio = gpio;
        currentStep = 0;
        stepCount = 0;
    }

    public: void begin() {
        /* A bad solution to a non-existing problem */
        if(gpio == GPIOA) RCC->AHBENR |= RCC_AHBENR_GPIOAEN;
        else if(gpio == GPIOB) RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
        else if(gpio == GPIOC) RCC->AHBENR |= RCC_AHBENR_GPIOCEN;
        else if(gpio == GPIOD) RCC->AHBENR |= RCC_AHBENR_GPIODEN;
        else if(gpio == GPIOF) RCC->AHBENR |= RCC_AHBENR_GPIOFEN;

        /* Set initial values */
        lastStepTime = millis();
        gpio -> MODER = 0x5555555;
        gpio -> ODR = 0;
        currentStep = 0;
        stepCount = 0;
    }

    private: void step(bool clockwise, int delay) {
        /* Progress sequence and thus turn motor */
        if(millis() - lastStepTime < delay) return;
        int next = (currentStep + 1) % 4, back = (currentStep + 3) % 4;
        gpio -> ODR = (gpio -> ODR & ~pinMask) | sequence[currentStep];
        currentStep = clockwise ? next : back;
        stepCount += clockwise ? 1 : -1;
        lastStepTime = millis();
    }

    public: int angle() {
        /* Calculate rotation angle */
        float count = (float) stepCount;
        return (count / maxSteps) * 360;
    }

    public: void update(int target, int delay) {
        /* Rotate motor towards given angle */
        if(angle() == target) return;
        step(angle() < target, delay);
    }
};