Make the proper frequency formula for HMI model

1 files changed, 126 insertions, 0 deletions

diff --git a/src/adlmidi_midiplay.cpp b/src/adlmidi_midiplay.cpp
index 0776a05..870d629 100644
--- a/src/adlmidi_midiplay.cpp
+++ b/src/adlmidi_midiplay.cpp
@@ -313,6 +313,128 @@ static inline double s_9xFreq(double noteD, double bendD)
     return (double)freqpitched;
 }
 
+const size_t s_hmi_freqtable_size = 103;
+static uint_fast32_t s_hmi_freqtable[s_hmi_freqtable_size] =
+{
+    0x0157, 0x016B, 0x0181, 0x0198, 0x01B0, 0x01CA, 0x01E5, 0x0202, 0x0220, 0x0241, 0x0263, 0x0287,
+    0x0557, 0x056B, 0x0581, 0x0598, 0x05B0, 0x05CA, 0x05E5, 0x0602, 0x0620, 0x0641, 0x0663, 0x0687,
+    0x0957, 0x096B, 0x0981, 0x0998, 0x09B0, 0x09CA, 0x09E5, 0x0A02, 0x0A20, 0x0A41, 0x0A63, 0x0A87,
+    0x0D57, 0x0D6B, 0x0D81, 0x0D98, 0x0DB0, 0x0DCA, 0x0DE5, 0x0E02, 0x0E20, 0x0E41, 0x0E63, 0x0E87,
+    0x1157, 0x116B, 0x1181, 0x1198, 0x11B0, 0x11CA, 0x11E5, 0x1202, 0x1220, 0x1241, 0x1263, 0x1287,
+    0x1557, 0x156B, 0x1581, 0x1598, 0x15B0, 0x15CA, 0x15E5, 0x1602, 0x1620, 0x1641, 0x1663, 0x1687,
+    0x1957, 0x196B, 0x1981, 0x1998, 0x19B0, 0x19CA, 0x19E5, 0x1A02, 0x1A20, 0x1A41, 0x1A63, 0x1A87,
+    0x1D57, 0x1D6B, 0x1D81, 0x1D98, 0x1DB0, 0x1DCA, 0x1DE5, 0x1E02, 0x1E20, 0x1E41, 0x1E63, 0x1E87,
+    0x1EAE, 0x1EB7, 0x1F02, 0x1F30, 0x1F60, 0x1F94, 0x1FCA
+};
+
+const size_t s_hmi_bendtable_size = 12;
+static uint32_t s_hmi_bendtable[s_hmi_bendtable_size] =
+{
+    0x144, 0x132, 0x121, 0x110, 0x101, 0xf8, 0xe5, 0xd8, 0xcc, 0xc1, 0xb6, 0xac
+};
+
+#define hmi_range_assert(formula, maxVal) assert((formula) >= 0 && (formula) < maxVal)
+
+static uint32_t s_hmi_bend_calc(uint32_t bend, uint32_t note)
+{
+    const uint32_t midi_bend_range = 1;
+    uint32_t noteMod12, bendFactor, outFreq, fmOctave, fmFreq, newFreq;
+
+    note -= 12;
+//    while(doNote >= 12) // ugly way to MOD 12
+//        doNote -= 12;
+    noteMod12 = (note % 12);
+
+    outFreq = s_hmi_freqtable[note];
+
+    fmOctave = outFreq & 0x1c00;
+    fmFreq = outFreq & 0x3ff;
+
+    if(bend < 64)
+    {
+        bendFactor = ((63 - bend) * 1000) >> 6;
+
+        hmi_range_assert(note - midi_bend_range, s_hmi_freqtable_size);
+        hmi_range_assert(midi_bend_range - 1, s_hmi_bendtable_size);
+
+        newFreq = outFreq - s_hmi_freqtable[note - midi_bend_range];
+        if(newFreq > 719)
+        {
+            newFreq = fmFreq - s_hmi_bendtable[midi_bend_range - 1];
+            newFreq &= 0x3ff;
+        }
+        newFreq = (newFreq * bendFactor) / 1000;
+        outFreq -= newFreq;
+    }
+    else
+    {
+        bendFactor = ((bend - 64) * 1000) >> 6;
+
+        hmi_range_assert(note + midi_bend_range, s_hmi_freqtable_size);
+        hmi_range_assert(11 - noteMod12, s_hmi_bendtable_size);
+
+        newFreq = s_hmi_freqtable[note + midi_bend_range] - outFreq;
+        if(newFreq > 719)
+        {
+            fmFreq = s_hmi_bendtable[11 - noteMod12];
+            outFreq = (fmOctave + 1024) | fmFreq;
+            newFreq = s_hmi_freqtable[note + midi_bend_range] - outFreq;
+        }
+        newFreq = (newFreq * bendFactor) / 1000;
+        outFreq += newFreq;
+    }
+
+    return outFreq;
+}
+
+#undef hmi_range_assert
+
+static inline double s_hmiFreq(double noteD, double bendD)
+{
+    int_fast32_t note = (int_fast32_t)(noteD + 0.5);
+    double bendDec = bendD - (int)bendD; // 0.0 ± 1.0 - one halftone
+    int_fast32_t bend;
+    uint_fast32_t inFreq;
+    uint_fast32_t freq;
+    int_fast32_t octave;
+    int_fast32_t octaveOffset = 0;
+
+    note += (int)bendD;
+
+    bend = (int_fast32_t)(bendDec * 64.0) + 64;
+
+    while(note < 12)
+    {
+        octaveOffset--;
+        note += 12;
+    }
+    while(note > 114)
+    {
+        octaveOffset++;
+        note -= 12;
+    }
+
+    if(bend == 64)
+        inFreq = s_hmi_freqtable[note - 12];
+    else
+        inFreq = s_hmi_bend_calc(bend, note);
+
+    freq = inFreq & 0x3FF;
+    octave = (inFreq >> 10) & 0x07;
+
+    octave += octaveOffset;
+
+    while(octave > 0)
+    {
+        freq *= 2;
+        octave -= 1;
+    }
+
+    return freq;
+}
+
+
+
 
 enum { MasterVolumeDefault = 127 };
 
@@ -1606,6 +1728,10 @@ void MIDIplay::noteUpdate(size_t midCh,
                     finalFreq = s_9xFreq(currentTone, bend);
                     break;
 
+                case Synth::VOLUME_HMI:
+                    finalFreq = s_hmiFreq(currentTone, bend);
+                    break;
+
                 default:
                     finalFreq = s_commonFreq(currentTone, bend);
                 }