3use crate::ym2612::debug::LfoState;
4use bincode::{Decode, Encode};
5use jgenesis_common::num::GetBit;

8const LFO_COUNTER_MASK: u8 = 0x7F;

10const LFO_DIVIDERS: [u8; 8] = [
11    108, // 3.85 Hz
12    77,  // 5.40 Hz
13    71,  // 5.86 Hz
14    67,  // 6.21 Hz
15    62,  // 6.71 Hz
16    44,  // 9.46 Hz
17    8,   // 52.02 Hz
18    5,   // 83.23 Hz
19];

23const FM_INCREMENT_TABLE: &[[u16; 8]; 8] = &[
24    [0, 0, 0, 0, 0, 0, 0, 0],
25    [0, 0, 0, 0, 4, 4, 4, 4],
26    [0, 0, 0, 4, 4, 4, 8, 8],
27    [0, 0, 4, 4, 8, 8, 12, 12],
28    [0, 0, 4, 8, 8, 8, 12, 16],
29    [0, 0, 8, 12, 16, 16, 20, 24],
30    [0, 0, 16, 24, 32, 32, 40, 48],
31    [0, 0, 32, 48, 64, 64, 80, 96],
32];

34#[derive(Debug, Clone, Encode, Decode)]
35pub struct LowFrequencyOscillator {
36    enabled: bool,
37    counter: u8,
38    divider: u8,
39    max_divider: u8,
40    frequency: u8,
41}
42
43impl LowFrequencyOscillator {
44    pub fn new() -> Self {
45        Self { enabled: false, counter: 0, divider: 0, max_divider: LFO_DIVIDERS[0], frequency: 0 }
46    }
47
48    pub fn set_enabled(&mut self, enabled: bool) {
49        self.enabled = enabled;
50        if !enabled {
51            self.counter = 0;
52        }
53    }
54
55    pub fn set_frequency(&mut self, frequency: u8) {
56        self.frequency = frequency;
57        self.max_divider = LFO_DIVIDERS[frequency as usize];
58    }
59
60    pub fn counter(&self) -> u8 {
61        self.counter
62    }
63
64    #[inline]
65    pub fn tick(&mut self) {
66        // TODO is this the correct way to handle LFO frequency changes?
67        self.divider += 1;
68        if self.divider >= self.max_divider {
69            self.divider = 0;
70
71            if self.enabled {
72                self.counter = (self.counter + 1) & LFO_COUNTER_MASK;
73            }
74        }
75    }
76
77    pub fn to_debug_state(&self) -> LfoState {
78        LfoState { enabled: self.enabled, frequency: self.frequency, divider: self.max_divider }
79    }
80}

83pub fn frequency_modulation(lfo_counter: u8, fm_sensitivity: u8, f_number: u16) -> u16 {
84    if fm_sensitivity == 0 {
85        return f_number << 1;
86    }
87
88    let fm_table_idx = if lfo_counter.bit(5) {
89        // Max to zero
90        (0x1F - (lfo_counter & 0x1F)) >> 2
91    } else {
92        // Zero to max
93        (lfo_counter & 0x1F) >> 2
94    };
95
96    // Compute total increment from the highest 7 bits of F-number; the lower 4 bits never add any
97    // increment
98    let raw_increment = FM_INCREMENT_TABLE[fm_sensitivity as usize][fm_table_idx as usize];
99    let fm_increment = (4..11)
100        .map(|i| {
101            let bit = (f_number >> i) & 1;
102            bit * (raw_increment >> (10 - i))
103        })
104        .sum::<u16>();
105
106    if lfo_counter.bit(6) {
107        // Negative half of wave
108        (f_number << 1).wrapping_sub(fm_increment) & 0xFFF
109    } else {
110        // Positive half of wave
111        (f_number << 1).wrapping_add(fm_increment) & 0xFFF
112    }
113}

116pub fn amplitude_modulation(lfo_counter: u8, am_sensitivity: u8) -> u16 {
117    let am_attenuation = if lfo_counter.bit(6) {
118        // Attenuation increases from 0dB to 11.8dB
119        lfo_counter & 0x3F
120    } else {
121        // Attenuation decreases from 11.8dB to 0dB
122        0x3F - lfo_counter
123    };
124
125    // Shift left 1 because LFO counter bits 5-0 correspond to bits 6-1 in envelope attenuation scale
126    // Max AM attenuation should be 11.8125dB
127    let am_attenuation: u16 = (am_attenuation << 1).into();
128
129    match am_sensitivity {
130        0 => 0,
131        1 => am_attenuation >> 3,
132        2 => am_attenuation >> 1,
133        3 => am_attenuation,
134        _ => panic!("invalid AM sensitivity value: {am_sensitivity}"),
135    }
136}

138#[cfg(test)]
139mod tests {
140    use super::*;
141
142    #[test]
143    fn lfo_dividers() {
144        for (freq, divider) in LFO_DIVIDERS.into_iter().enumerate() {
145            let mut lfo = LowFrequencyOscillator::new();
146            lfo.set_enabled(true);
147            lfo.set_frequency(freq as u8);
148
149            for i in 0..4 {
150                for tick in 0..divider - 1 {
151                    lfo.tick();
152                    assert_eq!(
153                        i,
154                        lfo.counter(),
155                        "LFO counter should be {i} after {} ticks with divider {divider}",
156                        tick + 1
157                    );
158                }
159
160                lfo.tick();
161                assert_eq!(
162                    i + 1,
163                    lfo.counter(),
164                    "LFO counter should be {} after {divider} ticks (frequency {freq})",
165                    i + 1
166                );
167            }
168        }
169    }
170}