midi.cpp

#include <stdint.h>
#include <odrive_main.h>
#include <axis.hpp>
#include <algorithm>

#include "midi.hpp"

#define MAX_SONGS 10
const uint8_t* songs[MAX_SONGS] = {0};
size_t song_lengths[MAX_SONGS] = {0};

#define __MIDI_VAR_NAME(id)   midi_file_ ## id
#define __MIDI_VAR_NAME2(id)   dummy_ ## id
#define MIDI_VAR_NAME(id)   __MIDI_VAR_NAME(id)
#define MIDI_VAR_NAME2(id)   __MIDI_VAR_NAME2(id)

static int register_song(const uint8_t* song, size_t length, size_t id) {
    songs[id] = song;
    song_lengths[id] = length;
    return 0;
}

// Nyan Cat
// MIDI source: https://github.com/teslaworksumn/tesla-coil-midi/tree/master/Tesla%20Coil%20Music%201.8/MIDI/Themes
#define SONG_ID 1
#include <midi/nyan_cat.h>
#undef SONG_ID

// Bach Menuet
// MIDI source: ?
#define SONG_ID 2
#include <midi/menuet.h>
#undef SONG_ID

// Star Wars Imperial March
// MIDI source: https://github.com/teslaworksumn/tesla-coil-midi/tree/master/Tesla%20Coil%20Music%201.8/MIDI/Themes
#define SONG_ID 3
#include <midi/imperial_march.h>
#undef SONG_ID

// Bach Menuet
// MIDI source: Samuel Sadok
#define SONG_ID 4
#include <midi/bach_menuet.h>
#undef SONG_ID

// ACDC - Thunderstruck
// MIDI source: https://github.com/teslaworksumn/tesla-coil-midi/tree/master/Tesla%20Coil%20Music%201.8/MIDI/Songs
#define SONG_ID 5
#include <midi/thunderstruck.h>
#undef SONG_ID

// Tetris Theme
// MIDI source: derived from https://bitmidi.com/tetris-a-theme-mid
#define SONG_ID 6
#include <midi/tetris.h>
#undef SONG_ID

// Queen - Under Pressure
// MIDI source: Miguel Angel Quero Corrales
#define SONG_ID 7
#include <midi/under_pressure.h>
#undef SONG_ID

// Scorpions - Still loving you
// MIDI source: Daniel Kaufmann
#define SONG_ID 8
#include <midi/still_loving_you.h>
#undef SONG_ID

// Hava Nagila
// MIDI source: Samuel Sadok
#define SONG_ID 9
#include <midi/hava_nagila.h>
#undef SONG_ID

// Generate header from MIDI file:
// (echo "const uint8_t MIDI_VAR_NAME(SONG_ID)[] = {"; xxd -i < "my_file.mid" | sed 's/^/  /'; echo "};"; echo "const int MIDI_VAR_NAME2(SONG_ID) = register_song(MIDI_VAR_NAME(SONG_ID), sizeof(MIDI_VAR_NAME(SONG_ID)), SONG_ID);") > midi_file.h

// MIDI file reference:
// https://github.com/colxi/midi-parser-js/wiki/MIDI-File-Format-Specifications

float music_voltage = 250.0f;
size_t midi_file_id = 0xffffffff;

typedef struct {
    const uint8_t* buf = nullptr;
    size_t len = 0;
    size_t pos = 0;
    uint8_t last_cmd = 0;
    uint32_t tick_length_us = 1000;
    uint32_t next_event_us = 0;
} track_ctx_t;

typedef struct {
    Axis* axis = nullptr;
    uint8_t current_key = 0xff;
//    float normal_ctrl_freq = 8000;
} output_ctx_t;

 output_ctx_t outputs[] = {
    { &axes[1], 0xff },
    { &axes[0], 0xff },
 };
 const size_t n_outputs = sizeof(outputs) / sizeof(outputs[0]);

void tone_on(uint8_t key, uint8_t velocity) {
    // find first output that is not playing a tone
    size_t i = 0;
    while (outputs[i].current_key != 0xff)
        if (++i >= n_outputs)
            return;

    output_ctx_t* out = &outputs[i];
    if (out->current_key == 0xff) {
        out->current_key = key;
        //key -= 12; // avoid aliasing at high frequencies
        float freq = 440.0f * powf(2, static_cast<float>((int)key - 69) / 12.0f); // key 69 is 440Hz
        
        if (!out->axis)
            return;

        //channels[i].normal_ctrl_freq;
        //out->axis->motor_.config_.switching_frequency = new_ctrl_freq * out->axis->motor_.config_.control_frequency_divider / 2.0f;

        // Update switching frequency to be a multiple of the tone frequency to
        // prevent aliasing
        float switching_freq = out->axis->motor_.config_.max_switching_frequency;
        switching_freq = static_cast<float>(static_cast<uint32_t>(switching_freq / freq)) * freq;
        if (!((switching_freq >= out->axis->motor_.config_.min_switching_frequency)
           && (switching_freq <= out->axis->motor_.config_.max_switching_frequency))) {
            return;
        }
        out->axis->motor_.switching_frequency = switching_freq;
        out->axis->motor_.update_switching_frequency();

        //out->axis->motor_.update_switching_frequency();
        out->axis->motor_.current_control_.hf_tone_omega = 2.0f * M_PI * freq;
        float amplitude_coef = freq / out->axis->motor_.config_.max_switching_frequency;
        out->axis->motor_.current_control_.hf_tone_amplitude = music_voltage * amplitude_coef;
    }
}

void tone_off(uint8_t key) {
    // find first output that playing the specified key
    size_t i = 0;
    while (outputs[i].current_key != key)
        if (++i >= n_outputs)
            return;

    output_ctx_t* out = &outputs[i];
    if (key == out->current_key || key == 0xff) {
        out->current_key = 0xff;
        if (out->axis) {
            //out->axis->motor_.config_.switching_frequency = out->normal_ctrl_freq * out->axis->motor_.config_.control_frequency_divider / 2.0f;
            //out->axis->motor_.update_switching_frequency();
            out->axis->motor_.current_control_.hf_tone_amplitude = 0.0f;
        }
    }
}

void all_tones_off(void) {
    for (size_t i = 0; i < n_outputs; ++i)
        tone_off(outputs[i].current_key);
}

static uint32_t get_varint(const uint8_t* buf, size_t* pos) {
    uint32_t val = 0;
    do {
        val <<= 7;
        val |= (buf[*pos] & 0x7f);
    } while (buf[(*pos)++] & 0x80);
    return val;
}

static uint32_t parse_delta_us(track_ctx_t* track) {
    if (track->pos >= track->len)
        return UINT32_MAX;
    uint32_t ticks = get_varint(track->buf, &track->pos); // todo: read speed info from file
    return ticks * track->tick_length_us;
}

static bool parse_event(track_ctx_t* track) {
    uint8_t cmd = track->buf[track->pos++];

    // not sure if this is how it works but it works for my files
    if ((cmd & 0x80) == 0) {
        cmd = track->last_cmd;
        track->pos--;
    }

    if (cmd == 0xf0 || cmd == 0xf7) {
        track->pos += get_varint(track->buf, &track->pos);
    } else if (cmd == 0xff) {
        track->pos++; // type
        track->pos += get_varint(track->buf, &track->pos);
    } else if ((cmd & 0xf0) == 0xb0) {
        uint8_t channel = cmd & 0xf;
        uint8_t c = track->buf[track->pos++];
        (void) channel;
        if (c <= 0x77) {
            // footswitch / expression pedal / slider / ...
            track->pos++;
        } else {
            // MIDI mode messages
            switch (c) {
            case 0x78: all_tones_off(); break;
            case 0x79: all_tones_off(); break;
            case 0x7b: all_tones_off(); break;
            default: return true;
            }
            track->pos++;
        }
    } else if ((cmd & 0xf0) == 0xc0) { // "program change" message
        uint8_t channel = cmd & 0xf;
        uint8_t instrument = track->buf[track->pos++];
        (void) channel;
        (void) instrument; // ignore
    } else if ((cmd & 0xf0) == 0x80) { // "note off" message
        uint8_t channel = cmd & 0xf;
        uint8_t key = track->buf[track->pos++];
        uint8_t velocity = track->buf[track->pos++];
        (void) channel;
        (void) velocity;
        tone_off(key);
    } else if ((cmd & 0xf0) == 0x90) { // "note on" message
        uint8_t channel = cmd & 0xf;
        uint8_t key = track->buf[track->pos++];
        uint8_t velocity = track->buf[track->pos++];
        (void) channel;
        if (velocity > 0) {
            tone_on(key, velocity);
        } else {
            tone_off(key);
        }
    } else {
        return false;
    }
    track->last_cmd = cmd;
    return true;
}

bool play_midi() {
    size_t current_song = midi_file_id;
    if (current_song > MAX_SONGS)
        return false;

    const size_t max_tracks = 4;
    const uint8_t* buf = songs[current_song];
    if (!buf)
        return false;
    size_t len = song_lengths[current_song];
    size_t pos = 0;

    for (size_t i = 0; i < n_outputs; ++i) {
        if (outputs[i].axis) {
            outputs[i].axis->motor_.keep_timer_sync = false;
        }
    }
        

    track_ctx_t tracks[max_tracks];
    size_t n_tracks = 0;

    uint32_t start_us = get_ticks_us();
    uint32_t tick_length_us = 1000;

    bool result = false;

    while (pos + 8 < len) {
        uint32_t chunk_length = (buf[pos + 4] << 24) + (buf[pos + 5] << 16) + (buf[pos + 6] << 8) + (buf[pos + 7] << 0);
        if (strncmp((const char*)&buf[pos], "MThd", 4) == 0) {
            // header chunk - ignore for now
            pos += 8;
            uint16_t time_division = (buf[pos + 4] << 8) | buf[pos + 5];
            float fps;
            float ticks_per_frame;
            if (time_division & 0x8000) {
                // FPS format
                uint8_t fps_int = (time_division >> 8) & 0x7f;
                fps = fps_int == 29 ? 29.97f : (float)fps_int;
                ticks_per_frame = (float)(time_division & 0xff);
            } else {
                // ticks per beat format
                fps = 2.0f; // default is 120 beats per minute
                ticks_per_frame = (float)(time_division & 0x7fff);
            }
            tick_length_us = (uint32_t)(1000000.0f / fps / ticks_per_frame);
        } else if (strncmp((const char*)&buf[pos], "MTrk", 4) == 0) {
            // track chunk
            pos += 8;
            tracks[n_tracks].buf = &buf[pos];
            tracks[n_tracks].len = chunk_length;
            tracks[n_tracks].tick_length_us = tick_length_us;
            tracks[n_tracks].next_event_us = start_us + parse_delta_us(&tracks[n_tracks]);
            n_tracks++;
            if (n_tracks >= max_tracks)
                break;
        } else {
            goto done;
        }
        pos += chunk_length;
    }

    //tracks[0] = tracks[1];
    //n_tracks = 1;

    for (;;) {
        // abort if the song was switched
        if (current_song != midi_file_id)
            goto done;

        // find smallest time until next event across all tracks
        bool finished = true;
        uint32_t now_us = get_ticks_us();
        int32_t delta_us = INT32_MAX;
        for (size_t t = 0; t < n_tracks; ++t) {
            delta_us = std::min((int32_t)(tracks[t].next_event_us - now_us), delta_us);
            if (tracks[t].pos < tracks[t].len)
                finished = false;
        }

        // all tracks are finished
        if (finished)
            break;

        if (delta_us > 1000)
            osDelay((uint32_t)delta_us / 1000);
        
        // handle events on all tracks that are due
        now_us = get_ticks_us();
        for (size_t t = 0; t < n_tracks; ++t) {
            int32_t due_since = (now_us - tracks[t].next_event_us);
            if (due_since > -1500) { // accept deviations of up to 1.5ms
                if (!parse_event(&tracks[t])) {
                    break;
                }
                tracks[t].next_event_us += parse_delta_us(&tracks[t]);
            }
        }
    }

    result = true;

done:
    all_tones_off();

    for (size_t i = 0; i < n_outputs; ++i) {
        if (outputs[i].axis) {
            outputs[i].axis->motor_.keep_timer_sync = true;
            outputs[i].axis->motor_.switching_frequency = outputs[i].axis->motor_.config_.max_switching_frequency;
            outputs[i].axis->motor_.update_switching_frequency();
        }
    }
    return result;
}