#include "floppy.h"
#include "floppy-interface.h"
#include "main.h"

#define FLOPPOTRON_TICK_SPEED (uint16_t)(72000000 / 3600)
#define FLOPPOTRON_SOUND_FREQ (FLOPPOTRON_TICK_SPEED / 2)

#define FLOPPOTRON_MAX_STEPS (320 * 2)

// класс, описывающий программный таймер для флоппи привода, но мы сделаем вместо него структуру
struct FloppyDrive {
public:
    uint16_t cnt = 0; // текущий счетчик таймера
    uint16_t top = 0; // верхнее значение таймера
    uint16_t curr_step = 0; // текущий шаг флоппи привода
    bool isStarted = false; // запущен ли таймер
    bool isInitialization = false; // режим инициализации, нужен чтобы отодвинуть флоппи привод на нулевую позицию

    GPIO_TypeDef* gpio{};
    const uint16_t gpioDirBit{}; // бит ноги GPIO для подключения к Floppy::DIR
    const uint16_t gpioStepBit{}; // бит ноги GPIO для подключения к Floppy::STEP
    const uint16_t gpioSelBit{}; // бит ноги GPIO для подключения к Floppy::DRVSB (Drive select B)

    void setFreq(uint16_t freq) {
        // freq = FLOPPOTRON_SOUND_FREQ / top
        top = FLOPPOTRON_SOUND_FREQ / freq;
    }

    void setInitializationMode() {
        isInitialization = true;
        isStarted = true;
    }

    void setEnable(bool en) {
        isStarted = en;
    }

    void tick() {
        if (isStarted) {
            if (isInitialization) {
                cnt++;
                if (cnt >= FLOPPOTRON_SOUND_FREQ / 500) {
                    // направление выставляем сразу
                    gpio->ODR |= gpioDirBit;

                    // и дергаем ногой
                    gpio->ODR ^= gpioStepBit;

                    cnt = 0;

                    curr_step++;

                    if (curr_step >= FLOPPOTRON_MAX_STEPS) {
                        // инициализация завершена
                        curr_step = 0;
                        isInitialization = false;
                        isStarted = false;
                    }
                }
            } else {
                cnt++;
                if (cnt >= top) {
                    cnt = 0;
                    curr_step++;
                    if (curr_step >= FLOPPOTRON_MAX_STEPS) {
                        curr_step = 0;
                    }
                    if (curr_step & 1) {
                        // нечетные шаги, это тик направления
                        if (curr_step < FLOPPOTRON_MAX_STEPS / 2) {
                            gpio->ODR &= ~gpioDirBit;
                        } else {
                            gpio->ODR |= gpioDirBit;
                        }
                    } else {
                        // четные шаги, это шаги мотора
                        gpio->ODR ^= gpioStepBit;
                    }
                }
            }
        } else {
            cnt = 0;
        }
    }
};

FloppyDrive drives[] = {
        {.gpio = DIR1_GPIO_Port, .gpioDirBit = DIR1_Pin, .gpioStepBit = STEP1_Pin},
        {.gpio = DIR2_GPIO_Port, .gpioDirBit = DIR2_Pin, .gpioStepBit = STEP2_Pin},
};


void FloppySoundTickHandler() {
    for (auto& drive:drives) {
        drive.tick();
    }
}

#define FT_CMD_NOTES_ON 0x1000
#define FT_CMD_NOTES_OFF 0x2000
#define FT_CMD_WAIT 0x3000

// составная команда - частота на
#define FT_CMD_SET_FRQ1 0x8000
#define FT_CMD_SET_FRQ2 0x9000

#define FT_CMD_STOP 0x0000
#define FT_CMD_MASK 0xF000


void FloppyDriveInitialize() {
    for (auto& d:drives) {
        d.setInitializationMode();
    }

    for (auto& d:drives) {
        // ожидание завершения инициализации
        while (d.isInitialization) {}
    }
}


// хандлер для проигрывания нот
void FloppyTrackTick() {
    static const uint16_t* pc = FloppyTrack;
    static uint16_t wait = 0;

    if (FloppyStatus == FP_READY) {
        pc = FloppyTrack;
        wait = 0;
    } else if (FloppyStatus == FP_PLAY) {
        // байткод для 100bpm, тестовая мелодия

        if (wait == 0) {
            while (wait == 0 && FloppyStatus == FP_PLAY) {
                uint16_t cmd = *(pc++);
                uint16_t value = cmd & (~FT_CMD_MASK);
                switch (cmd & FT_CMD_MASK) {
                    case FT_CMD_STOP:
                        pc = FloppyTrack;
                        drives[0].setEnable(false);
                        drives[1].setEnable(false);
                        FloppyStatus = FP_READY;
                        break;

                    case FT_CMD_NOTES_ON:
                        if (value & 1) {
                            drives[0].setEnable(true);
                        }
                        if (value & 2) {
                            drives[1].setEnable(true);
                        }
                        break;

                    case FT_CMD_NOTES_OFF:
                        if (value & 1) {
                            drives[0].setEnable(false);
                        }
                        if (value & 2) {
                            drives[1].setEnable(false);
                        }
                        break;

                    case FT_CMD_WAIT:
                        wait = value;
                        break;

                    case FT_CMD_SET_FRQ1:
                        drives[0].setFreq(value);
                        break;

                    case FT_CMD_SET_FRQ2:
                        drives[1].setFreq(value);
                        break;
                }
            }
        } else {
            wait--;
        }
    }
}