use midly::num::u7;
use crate::config::Config;
use crate::midi_shim::{
	MIDIShim, MIDI_DATA_ENTRY_MSB, MIDI_MODULATOR_WHEEL, MIDI_NRPN_LSB, MIDI_NRPN_MSB,
	MIDI_PAN, MIDI_VOLUME, NRPN_DRUM_PAN, NRPN_DRUM_PITCH_COARSE,
};
use crate::strict;
use crate::utils::FactoredState;
use crate::vgm2mid::CHN_DAC;
use crate::ym3812::YM3812;
use anyhow::{anyhow, Result};
use midly::num::u4;
#[allow(dead_code)]
const YM278_REG_LSI_TEST: u8 = 0x00;
#[allow(dead_code)]
const YM278_REG_MEMORY_ACCESS_MODE: u8 = 0x02;
#[allow(dead_code)]
const YM278_REG_MEMORY_TYPE: u8 = 0x02;
#[allow(dead_code)]
const YM278_REG_WAVE_TABLE_HEADER: u8 = 0x02;
struct Opl4Tone {
	instrument: u8,
	pitch: u16,
	#[allow(dead_code)]
	key_scl: u8,
}
const XG_MODE: bool = false;
static OPL4_TONES: Vec<Opl4Tone> = Vec::new();
fn note_ymf278(tone: u16, fnum: u32, oct: i16) -> f64 {
	if oct == -8 && fnum == 0 {
		return 0xFF.into();
	}
	// Note2FNum from Linux// OPL4 Driver
	//pitch = (note - 60) * voice->sound->key_scaling / 100 + 60;
	//pitch = pitch << 7;	-- 1 Halftone = 128 pitch --
	//pitch += voice->sound->pitch_offset;
	//octave = pitch / 0x600 - 8;
	//fnum = pitch2fnum(pitch % 0x600);
	let mut pitch = (1.0 + fnum as f64 / 1024.0).log2();
	let octave = oct + 8;
	//if OPL4Tones(Tone).Pitch = 0x1000 { Stop
	pitch -= OPL4_TONES[tone as usize].pitch as f64 / 0x600 as f64;
	// Formula from YMF278B Manual:			 1024 + F_NUMBER
	// F(c) = 1200 x (Octave - 1) + 1200 x log2 ---------------
	// 1 octave = 1200c							   1024
	//if OPL4Tones(Tone).KeyScl > 0 & OPL4Tones(Tone).KeyScl != 100 {
	//	Note = (Note - 60) * 100 / OPL4Tones(Tone).KeyScl + 60
	//}
	(octave as f64 * 12.0) + pitch * 12.0
}
fn channel_select(snum: u8, tone_ins: u8) -> (u4, bool) {
	let mut select_channel: u4;
	let drum_mode: bool;
	//FIXME: more nonsense constants
	if XG_MODE {
		select_channel = (snum & 0xF).into();
		drum_mode = false;
	} else if tone_ins & 0x80 == 0 {
 			select_channel = (snum & 0xF).into();
 			if select_channel == CHN_DAC {
 				select_channel = 0xF.into();
 			}
 			//if SNum <= 0x4 { SelChn = 0x8
 			drum_mode = false;
 		} else {
 			select_channel = CHN_DAC;
 			drum_mode = true;
 		}
	(select_channel, drum_mode)
}
pub(crate) struct YMF278State {
	tone_wave: [u16; 0x18],
	tone_ins: [u8; 0x18],
	oct: [i16; 0x18],
	fnum_o: [u32; 0x18],
	fnum_n: [u32; 0x18],
	note_o: [f64; 0x18],
	note_n: [f64; 0x18],
	note_on_o: [u8; 0x18],
	note_on_n: [u8; 0x18],
	slot_volume: [u8; 0x18],
	slot_pan: [u8; 0x18],
	damper_on_o: [u8; 0x18],
	damper_on_n: [u8; 0x18],
	vib: [u8; 0x18],
	drum_pan: [[u8; 0x80]; 0x10],
	drum_pitch: [[u8; 0x80]; 0x10],
	drum_nrpn_m: [u7; 0x10],
	drum_nrpn_l: [u8; 0x10],
	opl4_tones: Vec<Opl4Tone>,
	factored: FactoredState,
}
pub(crate) struct YMF278<'config> {
	pub state: YMF278State,
	config: &'config Config,
}
impl<'config> YMF278<'config> {
	pub(crate) fn new<'c: 'config>(
		config: &'c Config,
		opt_state: Option<YMF278State>,
	) -> YMF278 {
		YMF278 {
			state: opt_state.unwrap_or_default(),
			config,
		}
	}
	pub(crate) fn command_handle(
		&mut self,
		port: u8,
		register: u8,
		data: u8,
		ym3812: &mut YM3812,
		fsam_3812: f64,
		midi: &mut MIDIShim,
	) -> Result<()> {
		if port < 0x2 {
			ym3812.command_handle(
				(port as u16) << 8 | register as u16,
				data,
				fsam_3812,
				midi,
			)?;
			if port == 0x1 && register == 0x5 {
				self.state = Default::default();
			}
			return Ok(());
		}
		if (0x08..=0xF7).contains(&register) {
			let slot_number = (register - 0x8) % 24;
			let (channel, drum_mode) = channel_select(
				slot_number,
				self.state.tone_ins[slot_number as usize],
			);
			//FIXME: this is dumb, make this more sensible.
			match (register - 0x8) / 24 {
				0x0 => {
					wave_table_number(
						&mut self.state,
						slot_number,
						data,
						channel.into(),
						midi,
					)?;
				},
				0x1 => {
					f_number(&mut self.state, slot_number, data);
				},
				0x2 => {
					octave(
						&mut self.state,
						slot_number,
						data,
						channel.into(),
						midi,
					)?;
				},
				0x3 => {
					total_level(
						&mut self.state,
						slot_number,
						data,
						drum_mode,
						channel.into(),
						midi,
					);
				},
				0x4 => {
					key_on(
						data,
						&mut self.state,
						slot_number,
						drum_mode,
						channel.into(),
						midi,
					)?;
				},
				0x5 => {
					lfo(
						&mut self.state,
						slot_number,
						data,
						drum_mode,
						channel.into(),
						midi,
					);
				},
				0x6 => {
					ar(
						&mut self.state,
						slot_number,
						data,
						channel.into(),
						midi,
					)?;
				},
				0x7 => (), // Reg 0xB0 - 0xC7
				//slot.DL = dl_tab(Int(Data / 0x10))
				//slot.D2R = Data & 0xF
				0x8 => (), // Reg 0xC8 - 0xDF
				//slot.RC = Int(Data / 0x10)
				//slot.RR = Data & 0xF
				0x9 => (), // Reg 0xE0 - 0xF7
				//slot.AM = Data & 0x7
				_ => strict!(),
			}
		} else {
			// All non-slot registers
			match register {
				0x0 => (), // TEST
				0x1 => (),
				0x2 => (),
				//wavetblhdr = (Data >> 2) & 0x7
				//memmode = Data & 1
				0x3 => (),
				//memadr = (memadr & 0xFFFF&) | (Data * 0x10000)
				0x4 => (),
				//memadr = (memadr & 0xFF00FF) | (Data * 0x100&)
				0x5 => (),
				//memadr = (memadr & 0xFFFF00) | Data
				0x6 => (), // memory data
				//Call writeMem(memadr, Data)
				//memadr = (memadr + 1) & 0xFFFFFF
				0xF8 => (),
				//fm_l = Data & 0x7
				//fm_r = (Data >> 3) & 0x7
				0xF9 => (),
				//pcm_l = Data & 0x7
				//pcm_r = (Data >> 3) & 0x7
				_ => (),
			}
		}
		//regs [Register] = Data
		Ok(())
	}
	pub(crate) fn load_opl4_instrument_set(&mut self) -> Result<()> {
		const INSSET_SIG: &[u8] = b"INSSET";
		const COUNT_PTR: usize = INSSET_SIG.len(); //This should resolve to the subsequent byte.
		//NOTE: Consider adding this as a static resource?
		//There is at most 1 KiB of data. Even on an embedded device it would be reasonable to
		//just statically allocate this.
		let bytes = std::fs::read("yrw801.ins").map_err(|err| anyhow!(err))?;
		if bytes.len() <= INSSET_SIG.len() {
			strict!("OPL4 Instrument file is too short!");
		}
		if &bytes[0..INSSET_SIG.len()] != INSSET_SIG {
			strict!("Invalid OPL4 instrument set");
		}
		let tone_count = bytes[COUNT_PTR];
		for tone in 0..tone_count {
			let instrument = bytes[COUNT_PTR + 1 + (tone * 4) as usize];
			let pitch = u16::from_le_bytes([
				bytes[COUNT_PTR + 1 + (1 + tone * 4) as usize],
				bytes[COUNT_PTR + 1 + (2 + tone * 4) as usize],
			]);
			let key_scl = bytes[COUNT_PTR + 1 + (3 + tone * 4) as usize];
			self.state.opl4_tones.push(Opl4Tone {
				instrument,
				pitch,
				key_scl,
			});
		}
		Ok(())
	}
}
impl Default for YMF278State {
	fn default() -> Self {
		YMF278State {
			tone_wave: [0; 0x18],
			tone_ins: [0; 0x18],
			oct: [0; 0x18],
			fnum_o: [0; 0x18],
			fnum_n: [0; 0x18],
			note_o: [0xFF.into(); 0x18],
			note_n: [0xFF.into(); 0x18],
			note_on_o: [0; 0x18],
			note_on_n: [0; 0x18],
			slot_volume: [0; 0x18],
			slot_pan: [0; 0x18],
			damper_on_o: [0; 0x18],
			damper_on_n: [0; 0x18],
			vib: [0; 0x18],
			drum_pan: [[0xFF; 0x80]; 0x10],
			drum_pitch: [[0xFF; 0x80]; 0x10],
			drum_nrpn_m: [0xFF.into(); 0x10],
			drum_nrpn_l: [0xFF; 0x10],
			opl4_tones: Vec::new(),
			factored: Default::default(),
		}
	}
}
fn ar(
	state: &mut YMF278State,
	slot_number: u8,
	data: u8,
	channel: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	// Reg 0x98 - 0xAF
	//slot.AR = Int(Data / 0x10)
	//slot.D1R = Data & 0xF
	if state.tone_ins[slot_number as usize] == 0x77 {
		// Reverse Cymbal
		let temp_byte = data / 0x10;
		if temp_byte >= 0xE {
			// -> Crash Cymbal
			if state.note_on_o[slot_number as usize] != 0 {
				midi.do_note_on(
					state.note_o[slot_number as usize],
					0xFF.into(),
					channel.into(),
					&mut state.factored.midi_note[slot_number as usize],
					&mut state.factored.midi_wheel[slot_number as usize],
					Some(0xFF),
					Some(temp_byte.into()),
				)?;
				state.note_on_o[slot_number as usize] = 0;
			}
			state.tone_wave[slot_number as usize] = 0x180;
			state.tone_ins[slot_number as usize] = OPL4_TONES
				[state.tone_wave[slot_number as usize] as usize]
				.instrument;
			let (channel, _drum_mode) =
				channel_select(slot_number, state.tone_ins[slot_number as usize]);
			let channel_ptr = channel.as_int() as usize;
			if state.tone_ins[slot_number as usize] & 0x80 != 0 {
				let temp_byte = state.tone_ins[slot_number as usize] & 0x7F;
				let old_val = state.drum_pitch[channel_ptr][temp_byte as usize];
				state.drum_pitch[channel_ptr][temp_byte as usize] =
					state.note_n[slot_number as usize] as u8; //FIXME: This is impossible to understand;
				if state.drum_pitch[channel_ptr][temp_byte as usize] != old_val {
					if state.drum_nrpn_m[channel_ptr] != NRPN_DRUM_PITCH_COARSE
						|| state.drum_nrpn_l[channel_ptr] != temp_byte
					{
						state.drum_nrpn_m[channel_ptr] =
							NRPN_DRUM_PITCH_COARSE;
						state.drum_nrpn_l[channel_ptr] = temp_byte;
						midi.controller_write(
							channel,
							MIDI_NRPN_MSB,
							state.drum_nrpn_m[channel_ptr],
						);
						midi.controller_write(
							channel,
							MIDI_NRPN_LSB,
							state.drum_nrpn_l[channel_ptr].into(),
						);
					}
					midi.controller_write(
						channel,
						MIDI_DATA_ENTRY_MSB,
						state.drum_pitch[channel_ptr][temp_byte as usize]
							.into(),
					);
				}
				state.note_n[slot_number as usize] =
					(state.tone_ins[slot_number as usize] & 0x7F).into();
			}
		}
	}
	Ok(())
}
fn lfo(
	state: &mut YMF278State,
	slot_number: u8,
	data: u8,
	drum_mode: bool,
	channel: u8,
	midi: &mut MIDIShim,
) {
	// Reg 0x80 - 0x97
	let old_val = state.vib[slot_number as usize];
	state.vib[slot_number as usize] = (data & 0x7) * 0x10;
	if !drum_mode && state.vib[slot_number as usize] != old_val {
		midi.controller_write(
			channel.into(),
			MIDI_MODULATOR_WHEEL,
			state.vib[slot_number as usize].into(),
		);
	}
	//slot.set_lfo(Int(Data / 0x8) & 0x7)
}
fn key_on(
	data: u8,
	state: &mut YMF278State,
	slot_number: u8,
	drum_mode: bool,
	channel: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	// Reg 0x68 - 0x7F
	if data & 0x80 != 0 {
		let temp_byte = data & 0xF;
		state.slot_pan[slot_number as usize] = match temp_byte.cmp(&0x08) {
			std::cmp::Ordering::Less => 0x40 - temp_byte / 0x7 * 0x40,
			std::cmp::Ordering::Equal => 0x40,
			std::cmp::Ordering::Greater => {
				let temp = 0x40 + (0x10 - temp_byte) / 0x7 * 0x40;
				if temp == 0x80 {
					0x7F
				} else {
					temp
				}
			},
		};
		if !drum_mode {
			let old_val = state.factored.midi_pan[channel as usize];
			state.factored.midi_pan[channel as usize] =
				state.slot_pan[slot_number as usize].into();
			if state.factored.midi_pan[channel as usize] != old_val {
				midi.controller_write(
					channel.into(),
					MIDI_PAN,
					state.factored.midi_pan[channel as usize],
				);
			}
		} else {
			let temp_byte = state.tone_ins[slot_number as usize] & 0x7F;
			let old_val = state.drum_pan[channel as usize][temp_byte as usize];
			state.drum_pan[channel as usize][temp_byte as usize] =
				state.slot_pan[slot_number as usize];
			if state.drum_pan[channel as usize][temp_byte as usize] != old_val {
				if state.drum_nrpn_m[channel as usize] != NRPN_DRUM_PAN
					|| state.drum_nrpn_l[channel as usize] != temp_byte
				{
					state.drum_nrpn_m[channel as usize] = NRPN_DRUM_PAN;
					state.drum_nrpn_l[channel as usize] = temp_byte;
					midi.controller_write(
						channel.into(),
						MIDI_NRPN_MSB,
						state.drum_nrpn_m[channel as usize],
					);
					midi.controller_write(
						channel.into(),
						MIDI_NRPN_LSB,
						state.drum_nrpn_l[channel as usize].into(),
					);
				}
				midi.controller_write(
					channel.into(),
					MIDI_DATA_ENTRY_MSB,
					state.drum_pan[channel as usize][temp_byte as usize].into(),
				);
			}
		}
	}
	state.damper_on_n[slot_number as usize] = (data & 0x40) / 0x40;
	state.note_on_n[slot_number as usize] = (data & 0x80) / 0x80;
	if state.damper_on_n[slot_number as usize] != state.damper_on_o[slot_number as usize] {
		// Damping increases Decay rate
		//midi.event_write(MIDI_CONTROLLER_CHANGE, CH, MIDI_SOFT, DamperOn_N[SNum]) & 0x7F != 0;
		state.damper_on_o[slot_number as usize] = state.damper_on_n[slot_number as usize]
	}
	if state.note_on_n[slot_number as usize] != state.note_on_o[slot_number as usize] {
		let temp_byte = if !drum_mode {
			0x7F
		} else {
			state.slot_volume[slot_number as usize]
		};
		if state.note_on_n[slot_number as usize] != 0 {
			midi.do_note_on(
				state.note_o[slot_number as usize],
				state.note_n[slot_number as usize],
				channel.into(),
				&mut state.factored.midi_note[slot_number as usize],
				&mut state.factored.midi_wheel[slot_number as usize],
				Some(0xFF),
				Some(temp_byte.into()),
			)?;
		} else {
			midi.do_note_on(
				state.note_o[slot_number as usize],
				0xFF.into(),
				channel.into(),
				&mut state.factored.midi_note[slot_number as usize],
				&mut state.factored.midi_wheel[slot_number as usize],
				Some(0xFF),
				Some(temp_byte.into()),
			)?;
		}
		state.note_on_o[slot_number as usize] = state.note_on_n[slot_number as usize];
	}
	Ok(())
}
fn total_level(
	state: &mut YMF278State,
	slot_number: u8,
	data: u8,
	drum_mode: bool,
	channel: u8,
	midi: &mut MIDIShim,
) {
	// Reg 0x50 - 0x67
	let old_val = state.slot_volume[slot_number as usize];
	state.slot_volume[slot_number as usize] = 0x7F - data / 0x2;
	if state.slot_volume[slot_number as usize] == 0x0
		&& state.note_on_n[slot_number as usize] == 0
	{
		state.slot_volume[slot_number as usize] = old_val;
	}
	//LD = Data & 0x1
	//if LD {
	//	// directly change volume
	//} else {
	//	// interpolate volume
	//}
	if !drum_mode {
		let old_val = state.factored.midi_volume[channel as usize];
		state.factored.midi_volume[channel as usize] =
			state.slot_volume[slot_number as usize].into();
		if state.factored.midi_volume[channel as usize] != old_val {
			midi.controller_write(
				channel.into(),
				MIDI_VOLUME,
				state.factored.midi_volume[channel as usize],
			);
		}
	}
}
fn octave(
	state: &mut YMF278State,
	slot_number: u8,
	data: u8,
	channel: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	// Reg 0x38 - 0x4F
	//slot.FN = (slot.FN & 0x07F) | ((Data & 0x07) << 7)
	state.factored.fnum_msb[slot_number as usize] = data & 0x7;
	//slot.PRVB = ((data & 0x08) >> 3)
	//slot.OCT =  ((data & 0xF0) >> 4)
	state.oct[slot_number as usize] = ((data & 0xF0) / 0x10).into();
	//int oct = slot.OCT;
	if state.oct[slot_number as usize] & 0x8 != 0 {
		state.oct[slot_number as usize] |= -8;
	}
	state.fnum_o[slot_number as usize] = state.fnum_n[slot_number as usize];
	state.fnum_n[slot_number as usize] = ((state.factored.fnum_msb[slot_number as usize]
		* 0x80) | state.factored.fnum_lsb
		[slot_number as usize])
		.into();
	state.note_o[slot_number as usize] = state.note_n[slot_number as usize];
	state.note_n[slot_number as usize] = note_ymf278(
		state.tone_wave[slot_number as usize],
		state.fnum_n[slot_number as usize],
		state.oct[slot_number as usize],
	);
	if state.tone_ins[slot_number as usize] & 0x80 != 0 {
		let temp_byte = state.tone_ins[slot_number as usize] & 0x7F;
		let old_val = state.drum_pitch[channel as usize][temp_byte as usize];
		state.drum_pitch[channel as usize][temp_byte as usize] =
			state.note_n[slot_number as usize] as u8; //FIXME wtf is this shit.
		if state.drum_pitch[channel as usize][temp_byte as usize] != old_val {
			if state.drum_nrpn_m[channel as usize] != NRPN_DRUM_PITCH_COARSE
				|| state.drum_nrpn_l[channel as usize] != temp_byte
			{
				state.drum_nrpn_m[channel as usize] = NRPN_DRUM_PITCH_COARSE;
				state.drum_nrpn_l[channel as usize] = temp_byte;
				midi.controller_write(
					channel.into(),
					MIDI_NRPN_MSB,
					state.drum_nrpn_m[channel as usize],
				);
				midi.controller_write(
					channel.into(),
					MIDI_NRPN_LSB,
					state.drum_nrpn_l[channel as usize].into(),
				);
			}
			midi.controller_write(
				channel.into(),
				MIDI_DATA_ENTRY_MSB,
				state.drum_pitch[channel as usize][temp_byte as usize].into(),
			);
		}
		state.note_n[slot_number as usize] =
			(state.tone_ins[slot_number as usize] & 0x7F).into();
	}
	if state.note_on_n[slot_number as usize] != 0 {
		midi.do_note_on(
			state.note_o[slot_number as usize],
			state.note_n[slot_number as usize],
			channel.into(),
			&mut state.factored.midi_note[slot_number as usize],
			&mut state.factored.midi_wheel[slot_number as usize],
			None,
			None,
		)?;
	}
	Ok(())
}
fn f_number(state: &mut YMF278State, slot_number: u8, data: u8) {
	// Reg 0x20 - 0x37
	//slot.wave = (slot.wave & 0xFF) | ((data & 0x1) << 8);
	state.tone_wave[slot_number as usize] =
		(state.tone_wave[slot_number as usize] & 0xFF) | ((data as u16 & 0x1) * 0x100);
	//slot.FN = (slot.FN & 0x380) | (data >> 1);
	state.factored.fnum_lsb[slot_number as usize] = data / 0x2;
	// usually, FNum MSB is sent after FNum LSB
}
fn wave_table_number(
	state: &mut YMF278State,
	slot_number: u8,
	data: u8,
	channel: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	// Reg 0x8 - 0x1F
	//loadTime = time + LOAD_DELAY;
	//
	//slot.wave = (slot.wave & 0x100) | data;
	state.tone_wave[slot_number as usize] =
		(state.tone_wave[slot_number as usize] & 0x100) | data as u16;
	state.tone_ins[slot_number as usize] =
		OPL4_TONES[state.tone_wave[slot_number as usize] as usize].instrument;
	let old_val = channel;
	let (channel, _drum_mode) =
		channel_select(slot_number, state.tone_ins[slot_number as usize]);
	let channel_ptr = channel.as_int() as usize;
	if channel != old_val & state.note_on_o[slot_number as usize] {
		midi.do_note_on(
			state.note_o[slot_number as usize],
			0xFF.into(),
			channel,
			&mut state.factored.midi_note[slot_number as usize],
			&mut state.factored.midi_wheel[slot_number as usize],
			Some(0xFF),
			Some(0x00.into()), //FIXME: This was temp_byte, though it was never assigned...
		)?;
		state.note_on_o[slot_number as usize] = 0
	}
	let old_instrument = state.factored.midi_instrument[channel_ptr];
	state.factored.midi_instrument[channel_ptr] =
		if state.tone_ins[slot_number as usize] & 0x80 != 0 {
			0x80.into()
		} else {
			(state.tone_ins[slot_number as usize] & 0x7F).into()
		};
	if state.factored.midi_instrument[channel_ptr] != old_instrument {
		if XG_MODE {
			if (state.factored.midi_instrument[channel_ptr].as_int() & 0x80)
				!= (old_instrument.as_int() & 0x80)
			{
				let temp_byte = if state.factored.midi_instrument[channel_ptr]
					.as_int() & 0x80 != 0
				{
					0x7F
				} else {
					0x0
				};
				midi.controller_write(channel, 0x00.into(), temp_byte.into());
			}
			if (state.factored.midi_instrument[channel_ptr].as_int() & 0x80)
				& (old_instrument.as_int() & 0x80)
				== 0
			{
				midi.program_change_write(
					channel,
					state.factored.midi_instrument[channel_ptr],
				);
			}
		} else if state.factored.midi_instrument[channel_ptr].as_int() & 0x80 == 0 {
  				midi.program_change_write(
  					channel,
  					state.factored.midi_instrument[channel_ptr],
  				);
  			}
		//if CH < 8 & MIDIIns[channel_ptr] = 0x77 { Stop
	}
	//int base = (slot.wave < 384 | !wavetblhdr) ?
	//		   (slot.wave * 12) :
	//		   (wavetblhdr * 0x80000 + ((slot.wave - 384) * 12));
	//byte buf[12];
	//for (int i = 0; i < 12; ++i) {
	//	buf[i] = readMem(base + i);
	//}
	//slot.bits = (buf[0] & 0xC0) >> 6;
	//slot.set_lfo((buf[7] >> 3) & 7);
	//slot.vib  = buf[7] & 7;
	//slot.AR   = buf[8] >> 4;
	//slot.D1R  = buf[8] & 0xF;
	//slot.DL   = dl_tab[buf[9] >> 4];
	//slot.D2R  = buf[9] & 0xF;
	//slot.RC   = buf[10] >> 4;
	//slot.RR   = buf[10] & 0xF;
	//slot.AM   = buf[11] & 7;
	//slot.startaddr = buf[2] | (buf[1] << 8) |
	//				 ((buf[0] & 0x3F) << 16);
	//slot.loopaddr = buf[4] + (buf[3] << 8);
	//slot.endaddr  = (((buf[6] + (buf[5] << 8)) ^ 0xFFFF) + 1);
	//if ((regs[Register + 4] & 0x080)) {
	//	 keyOnHelper(slot);
	//}
	Ok(())
}

// OPL Module for YM3812, YM3526, Y8950 and YMF262, based on YM2413 (OPLL) Module
use crate::config::Config;
use crate::midi_shim::{
	db_to_midi_vol, MIDIShim, MIDI_PAN, MIDI_PAN_CENTER, MIDI_PAN_LEFT, MIDI_PAN_RIGHT,
	MIDI_VOLUME,
};
use crate::strict;
use crate::utils::{hz_to_note, FactoredState};
use crate::vgm2mid::{CHN_DAC, OPL_TYPE_YMF262};
use anyhow::Result;
use midly::num::{u4, u7};
// YM3812/YMF262 Register Constants
const YM3812_REG_TEST_WAVESEL_EN: u16 = 0x1;
const YM3812_REG_CSW_NOTESEL: u16 = 0x8;
const YM3812_REG_MOD_VIB_EG_KS_MULT: u16 = 0x20;
const YM3812_REG_MOD_VIB_EG_KS_MULT_END: u16 = YM3812_REG_MOD_VIB_EG_KS_MULT + 0x15;
const YM3812_REG_KSL_TL: u16 = 0x40;
const YM3812_REG_KSL_TL_END: u16 = YM3812_REG_KSL_TL + 0x17;
#[allow(dead_code)]
const YM3812_REG_AR_DR: u16 = 0x60;
#[allow(dead_code)]
const YM3812_REG_SL_RR: u16 = 0x80;
const YM3812_REG_FNUM_LSB: u16 = 0xA0;
const YM3812_REG_FNUM_LSB_END: u16 = YM3812_REG_FNUM_LSB + 0x8;
const YM3812_REG_KEY_BLOCK_FNUM_MSB: u16 = 0xB0;
const YM3812_REG_KEY_BLOCK_FNUM_MSB_END: u16 = YM3812_REG_KEY_BLOCK_FNUM_MSB + 0x8;
const YM3812_REG_RHYTHM: u16 = 0xBD;
const YM3812_FB_CONNECTION_PAN: u16 = 0xC0;
const YM3812_FB_CONNECTION_PAN_END: u16 = YM3812_FB_CONNECTION_PAN + 0x8;
#[allow(dead_code)]
const YM3812_WAVESEL: u16 = 0xE0;
const YMF262_4OP_EN: u16 = 0x104;
const YMF262_OPL3_EN: u16 = 0x105;
const YM3812_REG_KEY: u8 = 0x20;
const YM3812_REG_BLOCK: u8 = 0x1C;
const YM3812_REG_FNUM_MSB: u8 = 0x3;
#[allow(dead_code)]
const YM3812_REG_RHYTHM_MODE: u16 = 0x20;
const YM3812_REG_BD: u8 = 0x10;
const YM3812_REG_SD: u8 = 0x8;
const YM3812_REG_TOM: u8 = 0x4;
const YM3812_REG_TCT: u8 = 0x2;
const YM3812_REG_HH: u8 = 0x1;
pub(crate) fn hz_ym3812(fnum: f64, opt_block: Option<u8>, fsam_3812: f64) -> f64 {
	let block = opt_block.unwrap_or(0);
	if block == 0 && fnum == 0.0 {
		0.0
	} else {
		fnum * fsam_3812 * f64::powi(2.0, block as i32 - 20)
	}
}
pub(crate) struct YM3812State {
	key_on: [bool; 8],
	block: [u8; 8],
	volume: [u8; 8],
	bd_vol: u7,
	hh_vol: u7,
	sd_vol: u7,
	tom_vol: u7,
	tct_vol: u7,
	percussion_on: [bool; 5],
	opl3_mode: u8,
	pub opl_type: u8,
	factored: FactoredState,
}
pub(crate) struct YM3812<'config> {
	pub state: YM3812State,
	config: &'config Config, //FIXME: This should be able to be a reference. It's basically static anyway though...
}
impl<'config> YM3812<'config> {
	pub(crate) fn new<'c: 'config>(config: &'c Config, opt_state: Option<YM3812State>) -> Self {
		YM3812 {
			state: opt_state.unwrap_or_default(),
			config,
		}
	}
	pub(crate) fn command_handle(
		&mut self,
		register: u16,
		data: u8,
		fsam_3812: f64,
		midi: &mut MIDIShim,
	) -> Result<()> {
		match register {
			YM3812_REG_TEST_WAVESEL_EN => (),
			//WaveselEnable = (Data & 0x20) / 0x20
			YMF262_4OP_EN => (),
			YMF262_OPL3_EN => self.state.opl3_mode = data & 0x1,
			YM3812_REG_CSW_NOTESEL => (),
			YM3812_REG_MOD_VIB_EG_KS_MULT..=YM3812_REG_MOD_VIB_EG_KS_MULT_END => {
				process_mod_vib_eg_ks_mult(register, data, self.config, midi)
			},
			YM3812_REG_KSL_TL..=YM3812_REG_KSL_TL_END => {
				ksl_tl(self, register, data, midi)
			},
			YM3812_REG_FNUM_LSB..=YM3812_REG_FNUM_LSB_END
			| YM3812_REG_KEY_BLOCK_FNUM_MSB..=YM3812_REG_KEY_BLOCK_FNUM_MSB_END => {
				let channel = (register & 0xF) as u8;
				if channel > 0x8 {
					return Ok(());
				}
				if self.config.ym2413_ch_disabled[channel as usize] {
					return Ok(());
				}
				if (register & 0xF0) == YM3812_REG_FNUM_LSB {
					self.state.factored.fnum_lsb[channel as usize] = data;
				//Exit Sub
				} else if (register & 0xF0) == YM3812_REG_KEY_BLOCK_FNUM_MSB {
					self.state.key_on[channel as usize] =
						(data & YM3812_REG_KEY) / YM3812_REG_KEY != 0;
					self.state.block[channel as usize] =
						(data & YM3812_REG_BLOCK) / 0x4;
					self.state.factored.fnum_msb[channel as usize] =
						data & YM3812_REG_FNUM_MSB;
				}
				self.state.factored.fnum_1[channel as usize] =
					self.state.factored.fnum_2[channel as usize];
				self.state.factored.fnum_2[channel as usize] =
					(self.state.factored.fnum_msb[channel as usize] as u32
						* 256) + self.state.factored.fnum_lsb[channel as usize]
						as u32;
				self.state.factored.hz_1[channel as usize] =
					self.state.factored.hz_2[channel as usize];
				self.state.factored.hz_2[channel as usize] = hz_ym3812(
					self.state.factored.fnum_2[channel as usize] as f64,
					Some(self.state.block[channel as usize]),
					fsam_3812,
				);
				self.state.factored.note_1[channel as usize] =
					self.state.factored.note_2[channel as usize];
				self.state.factored.note_2[channel as usize] =
					hz_to_note(self.state.factored.hz_2[channel as usize]);
				self.state.factored.note_on_1[channel as usize] =
					self.state.factored.note_on_2[channel as usize];
				if (register & 0xF0) == YM3812_REG_KEY_BLOCK_FNUM_MSB {
					self.state.factored.note_on_2[channel as usize] =
						self.state.key_on[channel as usize]
				}
				if !self.state.key_on[channel as usize] {
					if self.state.factored.note_on_1[channel as usize]
						&& self.state.factored.note_1[channel as usize]
							!= 0.0
					{
						midi.note_off_write(
							channel.into(),
							self.state.factored.midi_note
								[channel as usize],
							0x00.into(),
						);
						self.state.factored.midi_note[channel as usize] =
							0xFF.into();
					}
				} else if self.state.key_on[channel as usize] {
					if self.state.factored.note_on_2[channel as usize]
						!= self.state.factored.note_on_1[channel as usize]
					{
						midi.do_note_on(
							self.state.factored.note_1
								[channel as usize],
							self.state.factored.note_2
								[channel as usize],
							channel.into(),
							&mut self.state.factored.midi_note
								[channel as usize],
							&mut self.state.factored.midi_wheel
								[channel as usize],
							Some(255),
							None,
						)?;
					} else {
						//if (Register & 0xF0) = YM3812_REG_KEY_BLOCK_FNum_MSB & _
						//self.state.factored.note_1[channel as usize] != self.state.factored.note_2[channel as usize] {
						midi.do_note_on(
							self.state.factored.note_1
								[channel as usize],
							self.state.factored.note_2
								[channel as usize],
							channel.into(),
							&mut self.state.factored.midi_note
								[channel as usize],
							&mut self.state.factored.midi_wheel
								[channel as usize],
							None,
							None,
						)?;
					}
				}
			},
			YM3812_REG_RHYTHM => {
				process_reg_rhythm(data, &mut self.state, self.config, midi)
			},
			YM3812_FB_CONNECTION_PAN..=YM3812_FB_CONNECTION_PAN_END => {
				process_fb_connection_pan(
					register,
					data,
					&mut self.state,
					self.config,
					midi,
				);
			},
			_ => {
				strict!("Error when processing ym3812: {}, {}", register, data);
			},
		}
		Ok(())
	}
}
impl Default for YM3812State {
	fn default() -> Self {
		YM3812State {
			key_on: [false; 8],
			block: [0; 8],
			volume: [0; 8],
			bd_vol: 0.into(),
			hh_vol: 0.into(),
			sd_vol: 0.into(),
			tom_vol: 0.into(),
			tct_vol: 0.into(),
			percussion_on: [false; 5],
			opl3_mode: 0,
			opl_type: 0,
			factored: Default::default(),
		}
	}
}
fn ksl_tl(me: &mut YM3812, register: u16, data: u8, midi: &mut MIDIShim) {
	//if (register & 0x7) > 0x5 {}
	let operation = (register & 0x7) / 0x3;
	let channel = (((register & 0x18) / 0x8 * 0x3) + ((register & 0x7) % 0x3)) as u8;
	if me.config.ym2413_ch_disabled[channel as usize] {
		return;
	}
	match (channel, operation) {
		(0x6, 0x1) =>
		//me.state.bd_vol = OPL_TL2Vol(Data & 0x3F)
		{
			me.state.bd_vol = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F))
		},
		(0x7, 0x0) =>
		//me.state.hh_vol = OPL_TL2Vol(Data & 0x3F)
		{
			me.state.hh_vol = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F))
		},
		(0x7, 0x1) =>
		//me.state.sd_vol = OPL_TL2Vol(Data & 0x3F)
		{
			me.state.sd_vol = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F))
		},
		(0x8, 0x0) =>
		//me.state.tom_vol = OPL_TL2Vol(Data & 0x3F)
		{
			me.state.tom_vol = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F))
		},
		(0x8, 0x1) =>
		//me.state.tct_vol = OPL_TL2Vol(Data & 0x3F)
		{
			me.state.tct_vol = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F))
		},
		(_, _) => (),
	}
	if me.config.ym2413_vol_disabled[channel as usize] || operation == 0x0 {
		return;
	}
	let temp_byte = db_to_midi_vol(ym3812_vol_to_db(data & 0x3F));
	//TempByt = OPL_TL2Vol(Data & 0x3F)
	if me.state.volume[channel as usize] != temp_byte {
		me.state.volume[channel as usize] = temp_byte.into();
		midi.controller_write(
			channel.into(),
			MIDI_VOLUME,
			me.state.volume[channel as usize].into(),
		);
	}
}
fn process_fb_connection_pan(
	register: u16,
	data: u8,
	state: &mut YM3812State,
	config: &Config,
	midi: &mut MIDIShim,
) {
	let channel = register as u8 & 0xF;
	if config.ym2413_ch_disabled[channel as usize] {
		return;
	}
	if config.ym2413_vol_disabled[channel as usize] {
		return;
	}
	if state.opl_type != OPL_TYPE_YMF262 {
		return;
	}
	let temp_pan = match (state.opl3_mode, (data & 0x30) / 0x10) {
		(_, 0x0) => MIDI_PAN_CENTER, // should actually be complete silence
		(_, 0x1) => MIDI_PAN_LEFT,
		(_, 0x2) => MIDI_PAN_CENTER,
		(_, 0x3) => MIDI_PAN_RIGHT,
		(0x0, _) => MIDI_PAN_CENTER,
		(_, _) => MIDI_PAN_CENTER, //FIXME: this is likely incorrect
	};
	state.factored.midi_pan[channel as usize] = temp_pan;
	midi.controller_write(
		channel.into(),
		MIDI_PAN,
		state.factored.midi_pan[channel as usize],
	);
}
fn process_reg_rhythm(data: u8, state: &mut YM3812State, config: &Config, midi: &mut MIDIShim) {
	if config.ym2413_percussion_disabled {
		return;
	}
	let channel = CHN_DAC;
	toggle_drum(
		data,
		YM3812_REG_BD,
		&mut state.percussion_on[0],
		midi,
		channel,
		0x23.into(), //FIXME: Magic numbers
		state.bd_vol,
	);
	toggle_drum(
		data,
		YM3812_REG_SD,
		&mut state.percussion_on[1],
		midi,
		channel,
		0x26.into(),
		state.sd_vol,
	);
	toggle_drum(
		data,
		YM3812_REG_TOM,
		&mut state.percussion_on[2],
		midi,
		channel,
		0x2D.into(),
		state.tom_vol,
	);
	toggle_drum(
		data,
		YM3812_REG_TCT,
		&mut state.percussion_on[3],
		midi,
		channel,
		0x33.into(),
		state.tct_vol,
	);
	toggle_drum(
		data,
		YM3812_REG_HH,
		&mut state.percussion_on[4],
		midi,
		channel,
		0x2A.into(),
		state.hh_vol,
	);
}
pub(crate) fn toggle_drum(
	data: u8,
	reg: u8,
	percussion_on: &mut bool,
	midi: &mut MIDIShim,
	channel: u4,
	key: u7,
	drum_vol: u7,
) {
	if (data & reg) == 0 {
		if *percussion_on {
			midi.note_off_write(channel, key, 0x00.into());
			*percussion_on = false;
		}
	} else if !*percussion_on {
 			//midi.event_write(Midi { channel: CH, message: NoteOff { key: 35, vel: 0x0 }});
 			midi.note_on_write(channel, key, drum_vol);
 			*percussion_on = true;
 		}
}
fn process_mod_vib_eg_ks_mult(register: u16, data: u8, config: &Config, midi: &mut MIDIShim) {
	if (register & 0x7) > 0x5 {
		return;
	}
	let operation = (register & 0x7) / 0x3;
	let channel = ((register & 0x18) / 0x8 * 0x3 + (register & 0x7) % 0x3) as u8;
	let temp_byte = (data & 0xFE) / 0x2;
	if config.ym2413_ch_disabled[channel as usize]
		|| config.ym2413_vol_disabled[channel as usize]
		|| operation == 0x00
	{
		return;
	}
	midi.program_change_write(channel.into(), temp_byte.into());
}
pub(crate) fn ym3812_vol_to_db(tl: u8) -> f64 {
	-f64::from(tl) * 4.0 / 3.0
}

use crate::config::Config;
use crate::midi_shim::{
	db_to_midi_vol, MIDIShim, MIDI_MODULATOR_WHEEL, MIDI_PAN, MIDI_PAN_CENTER, MIDI_PAN_LEFT,
	MIDI_PAN_RIGHT, MIDI_PITCHWHEEL_CENTER, MIDI_VOLUME,
};
use crate::strict;
use crate::utils::shift;
use crate::utils::{hz_to_note, FactoredState};
use crate::vgm2mid::{CHN_DAC, OPN_TYPE_YM2608, OPN_TYPE_YM2610, OPN_TYPE_YM2612};
use crate::ym3812::ym3812_vol_to_db;
use anyhow::{bail, Result};
// YM2612 Register Select Constants
#[allow(dead_code)]
pub(crate) const YM2610_DELTAT_ADPCM: u8 = 0x10; // DELTA-T/ADPCM
pub(crate) const YM2612_TEST: u8 = 0x21; // LSI Test Data
pub(crate) const YM2612_LFO_EN_LFO_FREQ: u8 = 0x22;
#[allow(dead_code)]
pub(crate) const YM2612_LFO_EN: u8 = 8;
#[allow(dead_code)]
pub(crate) const YM2612_LFO_FREQ: u8 = 7;
pub(crate) const YM2612_TIMER_A_MSB: u8 = 0x24;
pub(crate) const YM2612_TIMER_A_LSB: u8 = 0x25;
pub(crate) const YM2612_TIMER_B: u8 = 0x26;
#[allow(dead_code)]
pub(crate) const YM2612_CH3_MODE_RESET_ENABLE_LOAD: u8 = 0x27;
#[allow(dead_code)]
pub(crate) const YM2612_CH3_MODE: u8 = 192;
#[allow(dead_code)]
pub(crate) const YM2612_TIMER_RESET: u8 = 48;
#[allow(dead_code)]
pub(crate) const YM2612_TIMER_ENABLE: u8 = 12;
#[allow(dead_code)]
pub(crate) const YM2612_TIMER_LOAD: u8 = 3;
pub(crate) const YM2612_SLOT_CH: u8 = 0x28; // Key On/Off
pub(crate) const YM2612_SLOT: u8 = 240;
pub(crate) const YM2612_CH: u8 = 7;
pub(crate) const YM2612_DAC: u8 = 0x2A; // DAC Data
#[allow(dead_code)]
pub(crate) const YM2612_DAC_DATA: u8 = 255;
pub(crate) const YM2612_DAC_EN: u8 = 0x2B; // DAC Data
#[allow(dead_code)]
pub(crate) const YM2612_DAC_EN_BIT: u8 = 127;
#[allow(dead_code)]
pub(crate) const YM2612_DT_MULTI: u8 = 0x30; // Detune/Multiple
#[allow(dead_code)]
pub(crate) const YM2612_DT: u8 = 112;
#[allow(dead_code)]
pub(crate) const YM2612_MULTI: u8 = 15;
pub(crate) const YM2612_TL: u8 = 0x40; // Total Level
pub(crate) const YM2612_TL_END: u8 = YM2612_TL + 0xF;
pub(crate) const YM2612_KS_AR: u8 = 0x50; // Key Scale/Attack Rate
#[allow(dead_code)]
pub(crate) const YM2612_KS: u8 = 192;
#[allow(dead_code)]
pub(crate) const YM2612_AR: u8 = 31;
pub(crate) const YM2612_DR: u8 = 0x60; // Decay Rate
pub(crate) const YM2612_SR: u8 = 0x70; // Sustain Rate
pub(crate) const YM2612_SL_RR: u8 = 0x80; // Sustain Level/Release Rate
#[allow(dead_code)]
pub(crate) const YM2612_SL: u8 = 240;
#[allow(dead_code)]
pub(crate) const YM2612_RR: u8 = 15;
pub(crate) const YM2612_SSG_EG: u8 = 0x90; // SSG-Type Envelope Control
#[allow(dead_code)]
pub(crate) const YM2612_FNUM_LSB: u8 = 0xA0;
#[allow(dead_code)]
pub(crate) const YM2612_BLOCK_FNUM_MSB: u8 = 0xA4;
pub(crate) const YM2612_BLOCK: u8 = 56;
#[allow(dead_code)]
pub(crate) const YM2612_FNUM_MSB: u8 = 7;
pub(crate) const YM2612_CH3_SPECIAL_FNUM_MSB: u8 = 0xA8;
pub(crate) const YM2612_CH3_SPECIAL_FNUM_MSB_END: u8 = YM2612_CH3_SPECIAL_FNUM_MSB + 2;
pub(crate) const YM2612_CH3_SPECIAL_BLOCK_FNUM_LSB: u8 = 0xAC;
pub(crate) const YM2612_CH3_SPECIAL_BLOCK_FNUM_LSB_END: u8 = YM2612_CH3_SPECIAL_BLOCK_FNUM_LSB + 2;
#[allow(dead_code)]
pub(crate) const YM2612_CH3_SPECIAL_BLOCK: u8 = 56;
#[allow(dead_code)]
pub(crate) const YM2612_CH3_SPECIAL_FNUM_LSB: u8 = 7;
pub(crate) const YM2612_FB_CONNECTION: u8 = 0xB0; // Self-Feedback/Connection
pub(crate) const YM2612_FB_CONNECTION_END: u8 = YM2612_FB_CONNECTION + 2;
#[allow(dead_code)]
pub(crate) const YM2612_FB: u8 = 56;
pub(crate) const YM2612_CONNECTION: u8 = 7;
pub(crate) const YM2612_L_R: u8 = 0xB4; // Pan Select
pub(crate) const YM2612_L_R_END: u8 = YM2612_L_R + 2;
pub(crate) const YM2612_L: u8 = 0x80;
pub(crate) const YM2612_R: u8 = 0x40;
pub(crate) const YM2612_BOTH_L_R: u8 = 192;
#[test]
fn test_hz() {
	let res = hz_ym2612(20, None, 3579545.0 / 72.0);
	assert!((res - 0.23706389798058403).abs() < 0.000001);
}
pub(crate) fn hz_ym2612(fnum: u32, opt_block: Option<u8>, fsam_2612: f64) -> f64 {
	let block = opt_block.unwrap_or(0);
	fnum as f64 * fsam_2612 * f64::powf(2.0, block as f64 - 22.0)
}
pub(crate) struct YM2612State {
	last_dac_note: u8,
	dac_en: bool,
	slot: [u8; 6],
	//	DT(5, 3) As Byte, MULTI(5, 3) As Byte
	tl: [[u8; 4]; 6],
	volume: [u8; 6],
	//KF As Byte
	//	KS(5, 3) As Byte, AR(5, 3) As Byte
	//	DR: [u8; 6, 3],
	//	SR: [u8; 6, 3],
	//	SL(5, 3) As Byte, RR(5, 3) As Byte
	block: [u8; 6],
	connection: [u8; 6],
	fmopn_adpcm_state: FMOPNADPCMState,
	ym_adpcm_state: YMADPCMState,
	pub opn_type: u8,
	factored: FactoredState,
}
impl Default for YM2612State {
	fn default() -> Self {
		YM2612State {
			last_dac_note: 0,
			dac_en: false,
			slot: [0; 6],
			tl: [[0; 4]; 6],
			volume: [0xFF; 6],
			block: [0; 6],
			connection: [0; 6],
			fmopn_adpcm_state: Default::default(),
			ym_adpcm_state: Default::default(),
			opn_type: 0,
			factored: Default::default(),
		}
	}
}
pub(crate) struct YM2612<'config> {
	pub(crate) state: YM2612State,
	config: &'config Config,
}
impl<'config> YM2612<'config> {
	pub(crate) fn new<'c: 'config>(config: &'c Config, opt_state: Option<YM2612State>) -> Self {
		YM2612 {
			state: opt_state.unwrap_or_default(),
			config,
		}
	}
	pub(crate) fn command_handle(
		&mut self,
		port: u8,
		register: u8,
		data: u8,
		fsam_2612: f64,
		midi: &mut MIDIShim,
	) -> Result<()> {
		match register {
			0x0..=0x1F => {
				self.opn_pcm_write(port, register, data, midi)?;
			},
			YM2612_TEST => (),
			YM2612_LFO_EN_LFO_FREQ => (),
			YM2612_TIMER_A_MSB => (),
			YM2612_TIMER_A_LSB => (),
			YM2612_TIMER_B => (),
			//  YM2612_CH3_MODE_RESET_ENABLE_LOAD => {
			//		CH3_Mode = (Data & YM2612_CH3_MODE) / 96
			//  },
			YM2612_SLOT_CH => self.process_slot(data, midi)?,
			YM2612_DAC => self.process_dac(data, midi),
			YM2612_DAC_EN => {
				self.state.dac_en = (register & YM2612_DAC_EN) == YM2612_DAC_EN;
				//	} YM2612_DT_MULTI => {
			},
			YM2612_TL..=YM2612_TL_END => {
				self.process_total_level(register, port, data, midi)?
			},
			YM2612_KS_AR => (),
			YM2612_DR => (),
			YM2612_SR => (),
			YM2612_SL_RR => (),
			YM2612_SSG_EG => (),
			0xA4..=0xA6 => {
				let channel = (register ^ 0xA4) + port;
				if self.config.ym2612_ch_disabled[channel as usize] {
					return Ok(());
				}
				self.state.block[channel as usize] = (data & YM2612_BLOCK) / 8;
				self.state.factored.fnum_msb[channel as usize] = data & 7;
			},
			0xA0..=0xA2 => {
				let channel = (register ^ 0xA0) + port;
				if self.config.ym2612_ch_disabled[channel as usize] {
					return Ok(());
				}
				self.state.factored.fnum_lsb[channel as usize] = data;
				self.state.factored.fnum_1[channel as usize] =
					self.state.factored.fnum_2[channel as usize];
				self.state.factored.fnum_2[channel as usize] =
					((self.state.factored.fnum_msb[channel as usize] as u32)
						<< 8) + self.state.factored.fnum_lsb[channel as usize]
						as u32;
				self.state.factored.hz_1[channel as usize] =
					self.state.factored.hz_2[channel as usize];
				self.state.factored.hz_2[channel as usize] = hz_ym2612(
					self.state.factored.fnum_2[channel as usize],
					Some(self.state.block[channel as usize]),
					fsam_2612,
				);
				self.state.factored.note_1[channel as usize] = shift(
					&mut self.state.factored.note_2[channel as usize],
					hz_to_note(self.state.factored.hz_2[channel as usize]),
				);
				if self.state.factored.note_on_2[channel as usize]
					&& self.state.factored.note_1[channel as usize]
						!= self.state.factored.note_2[channel as usize]
				{
					midi.do_note_on(
						self.state.factored.note_1[channel as usize],
						self.state.factored.note_2[channel as usize],
						channel.into(),
						&mut self.state.factored.midi_note
							[channel as usize],
						&mut self.state.factored.midi_wheel
							[channel as usize],
						None,
						None,
					)?;
				}
			},
			YM2612_CH3_SPECIAL_FNUM_MSB..=YM2612_CH3_SPECIAL_FNUM_MSB_END => (),
			YM2612_CH3_SPECIAL_BLOCK_FNUM_LSB
				..=YM2612_CH3_SPECIAL_BLOCK_FNUM_LSB_END => (),
			YM2612_FB_CONNECTION..=YM2612_FB_CONNECTION_END => {
				let channel = (register ^ YM2612_FB_CONNECTION) + port;
				self.state.connection[channel as usize] = data & YM2612_CONNECTION;
				if self.config.ym2612_ch_disabled[channel as usize] {
					return Ok(());
				}
				if self.config.ym2612_prog_disabled[channel as usize] {
					return Ok(());
				}
				midi.program_change_write(channel.into(), (data & 0x7F).into());
				//midi.event_write(MIDI_PROGRAM_CHANGE, CH, (Data & 0xF8) / 0x8);
			},
			YM2612_L_R..=YM2612_L_R_END => {
				let channel = (register ^ YM2612_L_R) + port;
				if self.config.ym2612_ch_disabled[channel as usize] {
					return Ok(());
				}
				if self.config.ym2612_pan_disabled[channel as usize] {
					return Ok(());
				}
				if channel == 0x5 && self.state.dac_en {
					self.state.factored.midi_pan[channel as usize] = 0xFF.into()
				}
				let temp_pan = get_pan_from_data(data)?;
				if self.state.factored.midi_pan[channel as usize] != temp_pan {
					self.state.factored.midi_pan[channel as usize] = temp_pan;
					midi.controller_write(
						channel.into(),
						MIDI_PAN,
						self.state.factored.midi_pan[channel as usize],
					);
				}
				let temp_double = (data & 0x07) as f64 / 0x07 as f64;
				let temp = (temp_double * temp_double * 0x7F as f64).round() as u8;
				if self.state.factored.midi_mod[channel as usize] != temp {
					self.state.factored.midi_mod[channel as usize] =
						temp.into();
					midi.controller_write(
						channel.into(),
						MIDI_MODULATOR_WHEEL,
						self.state.factored.midi_mod[channel as usize],
					);
				}
				if self.config.ym2612_dac_disabled {
					return Ok(());
				}
				//if CH = 5 & DAC_EN = 1 {
				//	midi.event_write(Midi { channel: CHN_DAC, message: Controller { controller: MIDI_PAN, value: self.state.factored.midi_pan[channel as usize] }});
				//	midi.event_write(Midi { channel: CHN_DAC, message: NoteOff { key: 35, vel: 0 }});
				//	midi.event_write(Midi { channel: CHN_DAC, message: NoteOn { key: 35, vel: MIDI_VOLUME_MAX * 0.8 }});
				//}
			},
			_ => {
				/*Exit Sub
				Debug.Print "Register " & Hex$(Register) & ": ";
				match Register
				} YM2612_TEST => {
					Debug.Print "TEST - LSI Test Data"
				} YM2612_LFO_EN_LFO_FREQ => {
					Debug.Print "LFO_EN_LFO_FREQ"
				} YM2612_TIMER_A_MSB => {
					Debug.Print "TIMER_A_MSB"
				} YM2612_TIMER_A_LSB => {
					Debug.Print "TIMER_A_LSB"
				} YM2612_TIMER_B => {
					Debug.Print "TIMER_B"
				} YM2612_CH3_MODE_RESET_ENABLE_LOAD => {
					Debug.Print "CH3_MODE_RESET_ENABLE_LOAD"
				} YM2612_SLOT_CH => {
					Debug.Print "SLOT_CH - Key On/Off"
				} YM2612_DAC => {
					Debug.Print "DAC - DAC Data"
				} YM2612_DAC_DATA => {
					Debug.Print "DAC_DATA"
				} YM2612_DAC_EN => {
					Debug.Print "DAC_EN - DAC Data"
				} YM2612_DT_MULTI => {
					Debug.Print "DT_MULTI - Detune/Multiple"
				} YM2612_TL => {
					Debug.Print "TL - Total Level"
				} YM2612_KS_AR => {
					Debug.Print "KS_AR - Key Scale/Attack Rate"
				} YM2612_DR => {
					Debug.Print "DR - Decay Rate"
				} YM2612_SR => {
					Debug.Print "SR - Sustain Rate"
				} YM2612_SL_RR => {
					Debug.Print "SL_RR - Sustain Level/Release Rate"
				} YM2612_SSG_EG => {
					Debug.Print "SSG_EG - SSG-Type Envelope Control"
				} YM2612_FNum_LSB => {
					Debug.Print "FNum_LSB"
				} YM2612_Block_FNum_MSB => {
					Debug.Print "Block_FNum_MSB"
				} YM2612_CH3_SPECIAL_FNum_MSB => {
					Debug.Print "CH3_SPECIAL_FNum_MSB"
				} YM2612_CH3_SPECIAL_BLOCK_FNum_LSB => {
					Debug.Print "CH3_SPECIAL_BLOCK_FNum_LSB"
				} YM2612_CH3_SPECIAL_FNum_LSB => {
					Debug.Print "CH3_SPECIAL_FNum_LSB"
				} YM2612_FB_CONNECTION => {
					Debug.Print "FB_CONNECTION - Self-Feedback/Connection"
				} YM2612_L_R => {
					Debug.Print "L_R - Pan Select"
				_ => {
					Debug.Print "Unknown"
				},*/
			}, //Stop
		}
		Ok(())
	}
	fn process_slot(&mut self, data: u8, midi: &mut MIDIShim) -> Result<()> {
		let mut channel = data & YM2612_CH;
		channel -= if channel > 3 { 1 } else { 0 };
		channel = channel.clamp(0, 5);
		if self.config.ym2612_ch_disabled[channel as usize] {
			return Ok(());
		}
		self.state.slot[channel as usize] = (data & YM2612_SLOT) / 8;
		if self.state.slot[channel as usize] == 0
			&& self.state.factored.note_on_2[channel as usize]
		{
			self.state.factored.midi_wheel[channel as usize] = MIDI_PITCHWHEEL_CENTER;
			//				midi.event_write(MIDI_PITCHWHEEL, CH, self.state.factored.midi_wheel[channel as usize]);
			midi.note_off_write(
				channel.into(),
				self.state.factored.midi_note[channel as usize],
				0x00.into(),
			);
			self.state.factored.midi_note[channel as usize] = 0xFF.into();
			self.state.factored.note_on_1[channel as usize] =
				shift(&mut self.state.factored.note_on_2[channel as usize], false);
		} else if self.state.slot[channel as usize] != 0
			&& !self.state.factored.note_on_2[channel as usize]
		{
			midi.do_note_on(
				self.state.factored.note_1[channel as usize],
				self.state.factored.note_2[channel as usize],
				channel.into(),
				&mut self.state.factored.midi_note[channel as usize],
				&mut self.state.factored.midi_wheel[channel as usize],
				Some(255),
				None,
			)?;
			self.state.factored.note_on_1[channel as usize] =
				self.state.factored.note_on_2[channel as usize];
			self.state.factored.note_on_2[channel as usize] = true;
		}
		Ok(())
	}
	fn opn_pcm_write(
		&mut self,
		port: u8,
		register: u8,
		data: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		match (port, self.state.opn_type) {
			(0x00, OPN_TYPE_YM2608) if (register & 0x10) != 0 => fmopn_adpcm_write(
				register & 0xF,
				data,
				&mut self.state.fmopn_adpcm_state,
				midi,
			)?,
			(0x00, OPN_TYPE_YM2610) if (register & 0x10) == 0 => ym_adpcm_write(
				register & 0xF,
				data,
				&mut self.state.ym_adpcm_state,
				midi,
			)?,
			(0x01, OPN_TYPE_YM2608) if (register & 0x10) == 0 => ym_adpcm_write(
				register & 0xF,
				data,
				&mut self.state.ym_adpcm_state,
				midi,
			)?,
			(0x01, OPN_TYPE_YM2610) if register > 0x30 => fmopn_adpcm_write(
				register & 0xF,
				data,
				&mut self.state.fmopn_adpcm_state,
				midi,
			)?,
			_ => strict!(),
		}
		Ok(())
	}
	fn process_total_level(
		&mut self,
		register: u8,
		port: u8,
		data: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		let mut channel = ((register ^ YM2612_TL) % 4) + port;
		if self.config.ym2612_ch_disabled[channel as usize] {
			return Ok(());
		}
		if self.config.ym2612_vol_disabled[channel as usize] {
			return Ok(());
		}
		let operation = (register ^ YM2612_TL) / 4;
		channel = if channel > 5 { 5 } else { channel };
		self.state.tl[channel as usize][operation as usize] = data & 0x7F;
		if operation < 3 {
			return Ok(());
		}
		let temp_byte = self.state.factored.midi_volume[channel as usize];
		self.state.volume[channel as usize] = self.volume_from_connection(channel)?;
		self.state.factored.midi_volume[channel as usize] =
			db_to_midi_vol(ym3812_vol_to_db(self.state.volume[channel as usize]) / 4.0);
		//Const PI = 3.14159265358979
		////Const Phase = PI / (MIDI_VOLUME_MAX * 2)
		////self.state.factored.midi_volume[channel as usize] = MIDI_VOLUME_MAX * (1 - (Sin(self.state.factored.midi_volume[channel as usize] * Phase)))
		//self.state.factored.midi_volume[channel as usize] = 0x80 * (1# - Sin(self.state.volume[channel as usize] / 0x80 * (PI / 2)))
		//if self.state.factored.midi_volume[channel as usize] = 0x80 {
		//	self.state.factored.midi_volume[channel as usize] = 0x7F
		//}
		if temp_byte != self.state.factored.midi_volume[channel as usize] {
			midi.controller_write(
				channel.into(),
				MIDI_VOLUME,
				self.state.factored.midi_volume[channel as usize],
			);
		}
		Ok(())
	}
	fn volume_from_connection(&mut self, channel: u8) -> Result<u8> {
		Ok((match self.state.connection[channel as usize] {
			0..=3 => self.state.tl[channel as usize][3] as u16,
			4 => {
				(self.state.tl[channel as usize][1] as u16
					+ self.state.tl[channel as usize][3] as u16)
					/ 2
			},
			5 | 6 => {
				(self.state.tl[channel as usize][1] as u16
					+ self.state.tl[channel as usize][2] as u16
					+ self.state.tl[channel as usize][3] as u16)
					/ 3
			},
			7 => {
				(self.state.tl[channel as usize][0] as u16
					+ self.state.tl[channel as usize][1] as u16
					+ self.state.tl[channel as usize][2] as u16
					+ self.state.tl[channel as usize][3] as u16)
					/ 4
			},
			_ => bail!("Invalid connection state"),
		}) as u8)
	}
	fn process_dac(&mut self, data: u8, midi: &mut MIDIShim) {
		if self.config.ym2612_dac_disabled {
			return;
		}
		if self.state.opn_type != OPN_TYPE_YM2612 {
			return;
		}
		//if Data = 0xF {
		//	Data = 0x14	// Sonic 3K Bass Drum
		//} else if Data = 0x10 {
		//	Data = 0x16	// Sonic 3K Snare Drum
		//} else {
		//	Data = 0xFF
		//}
		//if Data = 0x22 {
		//	Data = 0x23	 // Sonic Crackers Bass Drum
		//} else if Data = 0x23 {
		//	Data = 0x26	 // Sonic Crackers Snare Drum
		//} else if Data = 0xC4 {
		//	Data = 0x2D	 // Sonic Crackers Tom Tom
		//} else {
		//	Data = 0xFF
		//}
		//if Data = 0x1A {
		//	Data = 0x23	 // Sonic 1 Bass Drum
		//} else if Data = 0xB {
		//	Data = 0x26	 // Sonic 1 Snare Drum
		//} else {
		//	Data = 0xFF
		//}
		let should_write = data < 0x20;
		if data != 0xFF && should_write {
			midi.note_off_write(CHN_DAC, self.state.last_dac_note.into(), 0x00.into());
			//midi.event_write(Midi { channel: CHN_DAC, message: NoteOn { key: 35, vel: Data }});
			midi.note_on_write(CHN_DAC, data.into(), 0x7F.into());
			self.state.last_dac_note = data
		}
	}
}
fn get_pan_from_data(data: u8) -> Result<midly::num::u7> {
	Ok(match data & YM2612_BOTH_L_R {
		YM2612_L => MIDI_PAN_LEFT,
		YM2612_R => MIDI_PAN_RIGHT,
		YM2612_BOTH_L_R | 0 => MIDI_PAN_CENTER,
		_ => {
			strict!();
			MIDI_PAN_CENTER
		},
	})
}
struct FMOPNADPCMState {
	tl: u8,
	il: [u8; 5],
	ins_vol: [u8; 5],
	note_on: [u8; 5],
	vol_max: u8,
	adpcm_drum_notes: [u8; 6],
}
impl Default for FMOPNADPCMState {
	fn default() -> Self {
		FMOPNADPCMState {
			tl: 0,
			il: [0; 5],
			ins_vol: [0; 5],
			note_on: [0; 5],
			vol_max: 0,
			adpcm_drum_notes: [
				0x23, // Bass Drum
				0x26, // Snare Drum
				0x33, // Top Cymbal
				0x2A, // High Hat
				0x2D, // Tom Tom
				0x25, // Rim Shot
			],
		}
	}
}
fn fmopn_adpcm_write(
	register: u8,
	data: u8,
	state: &mut FMOPNADPCMState,
	midi: &mut MIDIShim,
) -> Result<()> {
	let channel: u8;
	let mut volume: u8;
	let vol_mul: u8;
	let vol_shift: u8;
	match register {
		0x0 => {
			fmopn_dm(data, state, midi);
		},
		0x1 => {
			fnopn_tl(state, data);
		},
		_ => {
			channel = register & 0x7;
			if channel >= 0x6 {
				return Ok(());
			}
			match register & 0x38 {
				0x8 => {
					// Bit 7 = L, Bit 6 = R, Bit 4-0 = Instrument Level
					state.il[channel as usize] = (!data) & 0x3F;
					volume = state.tl + state.il[channel as usize];
					if volume < state.vol_max {
						state.vol_max = volume
					}
					if volume >= 0x3F {
						vol_mul = 0x00;
						vol_shift = 0x00;
					} else {
						vol_mul = 0x0F - (volume & 0x7);
						vol_shift = 0x01 + ((volume & 0x38) / 0x08);
					}
					let _temp_byte = get_pan_from_data(register);
					//midi.event_write(Midi { channel: channel, message: Controller { controller: MIDI_PAN, value: state.factored.midi_pan[channel as usize] }});
					//Volume = (AdPcm_Acc * VolMul) / 2 ^ VolShift
					let mut temp_volume =
						((vol_mul as u32) << 8) >> vol_shift as u32; //FIXME: this is messy
					temp_volume = if temp_volume == 0x80 {
						0x7F
					} else {
						temp_volume
					};
					volume = if temp_volume >= 0x100 {
						temp_volume as u8 | 0x80
					} else {
						temp_volume as u8
					};
					volume = if volume == 0x0 { 0x1 } else { volume };
					state.ins_vol[channel as usize] = volume;
					//midi.event_write(MIDI_CONTROLLER_channelANGE, channel, MIDI_VOLUME, Volume);
				},
				0x10 | 0x18 => {
					// Start Address
					if (register & 0x38) == 0x18 {
						//Debug.Print Hex(Data)
						//Stop
					}
				},
				0x20 | 0x28 => (), // End Address
				_ => strict!(),
			}
		},
	}
	Ok(())
}
fn fnopn_tl(state: &mut FMOPNADPCMState, data: u8) {
	// Bit 0-5 = Total Level
	state.tl = (!data) & 0x3F;
	if state.vol_max == 0 {
		state.vol_max = 0xFF
	}
	//TempByt = (!Data) & 0x3F
	//for channel in 0x0..=0x5 {
	//	Volume = TL + IL[channel as usize]
	//	if Volume >= 0x3F {
	//		VolMul = 0x0
	//		VolShift = 0x0
	//	} else {
	//		VolMul = 0xF - (Volume & 0x7)
	//		VolShift = 0x1 + ((Volume & 0x38) / 0x8)
	//	}
	//	//Volume = (AdPcm_Acc * VolMul) / 1 << VolShift
	//	Volume = 0x100 * VolMul / 1 << VolShift
	//}
}
fn fmopn_dm(data: u8, state: &mut FMOPNADPCMState, midi: &mut MIDIShim) {
	// DM, --, C5, C4, C3, C2, C1, C0
	if (data & 0x80) == 0x0 {
		// Key On
		for channel in 0x0..=0x5 {
			if data & (1 << channel) != 0 {
				if channel != 0 {
					midi.note_off_write(
						0x09.into(),
						state.adpcm_drum_notes[channel as usize].into(),
						0x00.into(),
					);
				}
				midi.note_on_write(
					0x09.into(),
					state.adpcm_drum_notes[channel as usize].into(),
					state.ins_vol[channel as usize].into(),
				);
				state.note_on[channel as usize] = 1
			} else {
				if state.note_on[channel as usize] != 0 {
					midi.note_off_write(
						0x09.into(),
						state.adpcm_drum_notes[channel as usize].into(),
						0x00.into(),
					);
				}
				state.note_on[channel as usize] = 0
			}
		}
	} else {
		// All Keys Off
		for channel in 0x0..=0x5 {
			if state.note_on[channel as usize] != 0 {
				midi.note_off_write(
					0x09.into(),
					state.adpcm_drum_notes[channel as usize].into(),
					0x00.into(),
				);
			}
			state.note_on[channel as usize] = 0
		}
	}
}
#[derive(Default)]
struct YMADPCMState {
	ins_vol: u8,
}
fn ym_adpcm_write(
	register: u8,
	data: u8,
	state: &mut YMADPCMState,
	midi: &mut MIDIShim,
) -> Result<()> {
	match register & 0x0F {
		0x00 => {
			// Control 1: Start, Rec, MemMode, Repeat, SpOff, --, --, Reset => {
			if data & 0x80 != 0 {
				midi.note_off_write(0x09.into(), 0x7F.into(), 0x00.into());
				midi.note_on_write(0x09.into(), 0x7F.into(), state.ins_vol.into());
			} else {
				midi.note_off_write(0x09.into(), 0x7F.into(), 0x00.into());
			}
		},
		0x01 => {
			// Control 2: L, R, -, -, Sample, DA/AD, RAMTYPE, ROM => {
			match (data & 0xC0) / 0x40 {
				0x1 => (),       //state.factored.midi_pan[channel as usize] = MIDI_PAN_RIGHT
				0x2 => (),       //state.factored.midi_pan[channel as usize] = MIDI_PAN_LEFT
				0x3 | 0x0 => (), //state.factored.midi_pan[channel as usize] = MIDI_PAN_CENTER
				_ => strict!(),
			}
		},
		0x02 => (),                         // Start Address Low
		0x03 => (),                         // Start Address High
		0x04 => (),                         // Stop Address Low
		0x05 => (),                         // Stop Address High
		0x06 => (),                         // Prescale Low
		0x07 => (),                         // Prescale High
		0x08 => (),                         // ADPCM Data
		0x09 => (),                         // Delta-N Low
		0x0A => (),                         // Delta-N High
		0x0B => state.ins_vol = data / 0x2, // Volume
		0x0C => (),                         // Flag Control: Extend Status Clear/Mask
		_ => (),
	}
	Ok(())
}

use crate::config::Config;
use crate::midi_shim::{db_to_midi_vol, MIDIShim, MIDI_VOLUME};
use crate::strict;
use crate::utils::hz_to_note;
use crate::utils::FactoredState;
use crate::vgm2mid::CHN_DAC;
use crate::ym3812::toggle_drum;
use anyhow::Result;
use midly::num::u7;
// YM2413 Register Constants
#[allow(dead_code)]
pub(crate) const YM2413_REG_USER: u8 = 0x00;
pub(crate) const YM2413_REG_RHYTHM: u8 = 0x0E;
pub(crate) const YM2413_REG_RHYTHM_MODE: u8 = 0x20;
pub(crate) const YM2413_REG_BD: u8 = 0x10;
pub(crate) const YM2413_REG_SD: u8 = 0x08;
pub(crate) const YM2413_REG_TOM: u8 = 0x04;
pub(crate) const YM2413_REG_TCT: u8 = 0x02;
pub(crate) const YM2413_REG_HH: u8 = 0x01;
#[allow(dead_code)]
pub(crate) const YM2413_REG_TEST: u8 = 0x0F;
pub(crate) const YM2413_REG_FNUM_LSB: u8 = 0x10;
pub(crate) const YM2413_REG_FNUM_LSB_END: u8 = YM2413_REG_FNUM_LSB + 8;
pub(crate) const YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB: u8 = 0x20;
pub(crate) const YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB_END: u8 = YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB + 8;
pub(crate) const YM2413_REG_SUS: u8 = 0x20;
pub(crate) const YM2413_REG_KEY: u8 = 0x10;
pub(crate) const YM2413_REG_BLOCK: u8 = 0x0E;
pub(crate) const YM2413_REG_FNUM_MSB: u8 = 0x01;
pub(crate) const YM2413_REG_INST_VOL: u8 = 0x30;
pub(crate) const YM2413_REG_INST_VOL_5: u8 = YM2413_REG_INST_VOL + 5;
pub(crate) const YM2413_REG_INST_VOL_6: u8 = YM2413_REG_INST_VOL + 6;
pub(crate) const YM2413_REG_INST_VOL_8: u8 = YM2413_REG_INST_VOL + 8;
pub(crate) const YM2413_REG_INST: u8 = 0xF0;
pub(crate) const YM2413_REG_VOL: u8 = 0x0F;
// Tone Data (INST) Constants
pub(crate) const INST_ORIGINAL: u8 = 0x00;
pub(crate) const INST_VIOLIN: u8 = 0x01;
pub(crate) const INST_GUITAR: u8 = 0x02;
pub(crate) const INST_PIANO: u8 = 0x03;
pub(crate) const INST_FLUTE: u8 = 0x04;
pub(crate) const INST_CLARINET: u8 = 0x05;
pub(crate) const INST_OBOE: u8 = 0x06;
pub(crate) const INST_TRUMPET: u8 = 0x07;
pub(crate) const INST_ORGAN: u8 = 0x08;
pub(crate) const INST_HORN: u8 = 0x09;
pub(crate) const INST_SYNTHESIZER: u8 = 0x0A;
pub(crate) const INST_HARPSICHORD: u8 = 0x0B;
pub(crate) const INST_VIBRAPHONE: u8 = 0x0C;
pub(crate) const INST_BASS_SYNTH: u8 = 0x0D;
pub(crate) const INST_BASS_ACOUSTIC: u8 = 0x0E;
pub(crate) const INST_GUITAR_ELECTRIC: u8 = 0x0F;
pub(crate) fn hz_ym2413(fnum: f64, opt_block: Option<u8>, clock: u32) -> f64 {
	let block = opt_block.unwrap_or(0);
	if block == 0 && fnum == 0.0 {
		0.0
	} else {
		fnum * (f64::from(clock) / 72.0) * 2.0_f64.powi(block as i32 - 19)
	}
}
pub(crate) struct YM2413State {
	sus_on: [bool; 8],
	key_on: [bool; 8],
	block: [u8; 8],
	ins_vol: [u8; 8],
	instrument: [u8; 8],
	volume: [u8; 8],
	bd_vol: u7,
	hh_vol: u7,
	sd_vol: u7,
	tom_vol: u7,
	tct_vol: u7,
	percussion_on: [bool; 5],
	factored: FactoredState,
}
impl Default for YM2413State {
	fn default() -> Self {
		YM2413State {
			sus_on: [false; 8],
			key_on: [false; 8],
			block: [0; 8],
			ins_vol: [0; 8],
			instrument: [0xFF; 8],
			volume: [0xFF; 8],
			bd_vol: 0.into(),
			hh_vol: 0.into(),
			sd_vol: 0.into(),
			tom_vol: 0.into(),
			tct_vol: 0.into(),
			percussion_on: [false; 5],
			factored: Default::default(),
		}
	}
}
pub(crate) struct YM2413<'config> {
	state: YM2413State,
	config: &'config Config,
}
impl<'config> YM2413<'config> {
	pub(crate) fn new<'c: 'config>(config: &'c Config, opt_state: Option<YM2413State>) -> Self {
		YM2413 {
			state: opt_state.unwrap_or_default(),
			config,
		}
	}
	pub(crate) fn command_handle(
		&mut self,
		register: u8,
		data: u8,
		clock: u32,
		midi: &mut MIDIShim,
	) -> Result<()> {
		/*if Variables_Clear_YM2413 = 1 {
			CH = 0
			Erase state.FNum_MSB
			Erase state.SUS_ON: Erase state.KEY_ON:  Erase block: Erase state.FNum_LSB: Erase state.FNum_1: Erase state.FNum_2: Erase state.Hz_1: Erase state.Hz_2: Erase state.Note_1: Erase state.Note_2
			Erase state.InsVol: Erase state.Instrument: Erase state.Volume: state.BD_VOL = 0x7F: state.HH_VOL = 0x7F: state.SD_VOL = 0x7F: state.TOM_VOL = 0x7F: state.TCT_VOL = 0x7F
			Erase state.MIDINote: Erase state.MIDIWheel: Erase state.factored.midi_volume[0]: Erase state.NoteOn_1: Erase state.NoteOn_2: Erase state.Percussion_on
			For CH = 0x0..=0x8
				state.InsVol[CH as usize] = 0x0
				state.Instrument[CH as usize] = 0xFF
				state.Volume[CH as usize] = 0xFF
				state.MIDINote[CH as usize] = 0xFF
				state.MIDIWheel[CH as usize] = 0x8000
				state.factored.midi_volume[CH as usize] = 0xFF
			Next CH
			Variables_Clear_YM2413 = 0
		}*/
		match register {
			//	Case YM2413_REG_USER..=(YM2413_REG_USER + 7)
			YM2413_REG_RHYTHM => {
				self.rhythm(data, midi);
			},
			YM2413_REG_FNUM_LSB..=YM2413_REG_FNUM_LSB_END
			| YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB
				..=YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB_END => {
				self.fnum(register, data, clock, midi)?;
			},
			YM2413_REG_INST_VOL..=YM2413_REG_INST_VOL_5 => {
				self.instrument_volume_1_to_6(register, data, midi)?;
			},
			YM2413_REG_INST_VOL_6..=YM2413_REG_INST_VOL_8 => {
				self.instrument_volume_6_to_8(register, data)?;
			},
			_ => strict!(),
		}
		Ok(())
	}
	fn instrument_volume_6_to_8(&mut self, register: u8, data: u8) -> Result<()> {
		if self.config.ym2413_percussion_disabled {
			return Ok(());
		}
		match register - YM2413_REG_INST_VOL {
			6 => {
				////self.state.BD_VOL = (MIDI_VOLUME_MAX - ((Data & 15) * 8)) * 0.8
				//self.state.BD_VOL = (MIDI_VOLUME_MAX - ((Data & 0xF) * 0x8)) + 1
				//if self.state.BD_VOL = 0x80 { self.state.BD_VOL = 0x7F
				self.state.bd_vol = db_to_midi_vol(ym2413_vol_to_db(data & 0xF));
			},
			7 => {
				//self.state.HH_VOL = MIDI_VOLUME_MAX - (((Data & 0xF0) / 0x10) * 8) + 1
				//if self.state.HH_VOL = 0x80 { self.state.HH_VOL = 0x7F
				self.state.hh_vol =
					db_to_midi_vol(ym2413_vol_to_db((data & 0xF0) / 0x10));
				////self.state.SD_VOL = (MIDI_VOLUME_MAX - ((Data & 15) * 8)) * 0.8
				//self.state.SD_VOL = (MIDI_VOLUME_MAX - ((Data & 0xF) * 0x8)) + 1
				//if self.state.SD_VOL = 0x80 { self.state.SD_VOL = 0x7F
				self.state.sd_vol = db_to_midi_vol(ym2413_vol_to_db(data & 0xF));
			},
			8 => {
				////self.state.TOM_VOL = (MIDI_VOLUME_MAX - (((Data & 240) / 16) * 8)) * 0.6
				//self.state.TOM_VOL = (MIDI_VOLUME_MAX - (((Data & 0xF0) / 0x10) * 8)) + 1
				//if self.state.TOM_VOL = 0x80 { self.state.TOM_VOL = 0x7F
				self.state.tom_vol =
					db_to_midi_vol(ym2413_vol_to_db((data & 0xF0) / 0x10));
				//self.state.TCT_VOL = MIDI_VOLUME_MAX - ((Data & 0xF) * 0x8) + 1
				//if self.state.TCT_VOL = 0x80 { self.state.TCT_VOL = 0x7F
				self.state.tct_vol = db_to_midi_vol(ym2413_vol_to_db(data & 0xF));
			},
			_ => strict!(),
		}
		Ok(())
	}
	fn instrument_volume_1_to_6(
		&mut self,
		register: u8,
		data: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		let channel = register - YM2413_REG_INST_VOL;
		let mut temp_byte = self.state.instrument[channel as usize];
		self.state.instrument[channel as usize] = (data & YM2413_REG_INST) / 16;
		if !self.config.ym2413_prog_disabled[channel as usize]
			&& (self.state.ins_vol[channel as usize] == data
				|| temp_byte != self.state.instrument[channel as usize])
		{
			match self.state.instrument[channel as usize] {
				INST_ORIGINAL => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[0].into(),
					);
				},
				INST_VIOLIN => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[1].into(),
					);
				},
				INST_GUITAR => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[2].into(),
					);
				},
				INST_PIANO => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[3].into(),
					);
				},
				INST_FLUTE => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[4].into(),
					);
				},
				INST_CLARINET => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[5].into(),
					);
				},
				INST_OBOE => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[6].into(),
					);
				},
				INST_TRUMPET => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[7].into(),
					);
				},
				INST_ORGAN => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[8].into(),
					);
				},
				INST_HORN => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[9].into(),
					);
				},
				INST_SYNTHESIZER => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[10].into(),
					);
				},
				INST_HARPSICHORD => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[11].into(),
					);
				},
				INST_VIBRAPHONE => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[12].into(),
					);
				},
				INST_BASS_SYNTH => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[13].into(),
					);
				},
				INST_BASS_ACOUSTIC => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[14].into(),
					);
				},
				INST_GUITAR_ELECTRIC => {
					midi.program_change_write(
						channel.into(),
						self.config.ym2413_midi_patch[15].into(),
					);
				},
				_ => strict!(),
			}
		}
		temp_byte = self.state.volume[channel as usize];
		self.state.volume[channel as usize] = data & YM2413_REG_VOL;
		if self.config.ym2413_vol_disabled[channel as usize]
			&& (self.state.ins_vol[channel as usize] == data
				|| temp_byte != self.state.volume[channel as usize])
		{
			//self.state.Volume[CH as usize] = MIDI_VOLUME_MAX - ((Data & YM2413_REG_VOL) * 8) + 1
			//if self.state.Volume[CH as usize] = 0x80 { self.state.Volume[CH as usize] = 0x7F
			self.state.factored.midi_volume[channel as usize] = db_to_midi_vol(
				ym2413_vol_to_db(self.state.volume[channel as usize]),
			);
			midi.controller_write(
				channel.into(),
				MIDI_VOLUME,
				self.state.factored.midi_volume[channel as usize],
			);
		}
		self.state.ins_vol[channel as usize] = data;
		Ok(())
	}
	fn fnum(&mut self, register: u8, data: u8, clock: u32, midi: &mut MIDIShim) -> Result<()> {
		let channel = register & 0xF;
		if self.config.ym2413_ch_disabled[channel as usize] {
			return Ok(());
		}
		if (register & 0xF0) == YM2413_REG_FNUM_LSB {
			self.state.factored.fnum_lsb[channel as usize] = data;
			if !self.config.ym2413_optimized_vgms {
				return Ok(());
			}
		} else if (register & 0xF0) == YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB {
			self.state.sus_on[channel as usize] = data & YM2413_REG_SUS == 0;
			self.state.key_on[channel as usize] = data & YM2413_REG_KEY == 0;
			self.state.block[channel as usize] = (data & YM2413_REG_BLOCK) / 2;
			self.state.factored.fnum_msb[channel as usize] = data & YM2413_REG_FNUM_MSB;
		}
		self.state.factored.fnum_1[channel as usize] =
			self.state.factored.fnum_2[channel as usize];
		self.state.factored.fnum_2[channel as usize] =
			(self.state.factored.fnum_msb[channel as usize] as u32 * 256)
				+ self.state.factored.fnum_lsb[channel as usize] as u32;
		self.state.factored.hz_1[channel as usize] =
			self.state.factored.hz_2[channel as usize];
		self.state.factored.hz_2[channel as usize] = hz_ym2413(
			self.state.factored.fnum_2[channel as usize] as f64,
			Some(self.state.block[channel as usize]),
			clock,
		);
		self.state.factored.note_1[channel as usize] =
			self.state.factored.note_2[channel as usize];
		self.state.factored.note_2[channel as usize] =
			hz_to_note(self.state.factored.hz_2[channel as usize]);
		self.state.factored.note_on_1[channel as usize] =
			self.state.factored.note_on_2[channel as usize];
		if (register & 0xF0) == YM2413_REG_SUS_KEY_BLOCK_FNUM_MSB {
			self.state.factored.note_on_2[channel as usize] =
				self.state.key_on[channel as usize];
		}
		//CH = Register - YM2413_REG_SUS_KEY_BLOCK_self.state.FNum_MSB
		if !self.state.key_on[channel as usize] {
			if self.state.factored.note_on_1[channel as usize]
				&& self.state.factored.note_1[channel as usize] != 0.0
			{
				midi.note_off_write(
					channel.into(),
					self.state.factored.midi_note[channel as usize],
					0x00.into(),
				);
				self.state.factored.midi_note[channel as usize] = 0xFF.into();
			}
		} else if self.state.key_on[channel as usize] {
			if self.state.factored.note_on_2[channel as usize]
				!= self.state.factored.note_on_1[channel as usize]
			{
				midi.do_note_on(
					self.state.factored.note_1[channel as usize],
					self.state.factored.note_2[channel as usize],
					channel.into(),
					&mut self.state.factored.midi_note[channel as usize],
					&mut self.state.factored.midi_wheel[channel as usize],
					Some(255),
					None,
				)?;
			} else {
				//if (Register & 0xF0) = YM2413_REG_SUS_KEY_BLOCK_self.state.FNum_MSB & _
				//self.state.Note_1[CH as usize] != self.state.Note_2[CH as usize] {
				midi.do_note_on(
					self.state.factored.note_1[channel as usize],
					self.state.factored.note_2[channel as usize],
					channel.into(),
					&mut self.state.factored.midi_note[channel as usize],
					&mut self.state.factored.midi_wheel[channel as usize],
					None,
					None,
				)?;
			}
		}
		Ok(())
	}
	fn rhythm(&mut self, mut data: u8, midi: &mut MIDIShim) {
		if self.config.ym2413_percussion_disabled {
			return;
		}
		let channel = CHN_DAC;
		if (data & YM2413_REG_RHYTHM_MODE) == 0 {
			data = 0x0;
		}
		//TODO: Move this into utils or something. Also, fix magic key numbers
		toggle_drum(
			data,
			YM2413_REG_BD,
			&mut self.state.percussion_on[0],
			midi,
			channel,
			0x23.into(),
			self.state.bd_vol,
		);
		toggle_drum(
			data,
			YM2413_REG_SD,
			&mut self.state.percussion_on[1],
			midi,
			channel,
			0x26.into(),
			self.state.sd_vol,
		);
		toggle_drum(
			data,
			YM2413_REG_TOM,
			&mut self.state.percussion_on[2],
			midi,
			channel,
			0x2D.into(),
			self.state.tom_vol,
		);
		toggle_drum(
			data,
			YM2413_REG_TCT,
			&mut self.state.percussion_on[3],
			midi,
			channel,
			0x2E.into(),
			self.state.tct_vol,
		);
		toggle_drum(
			data,
			YM2413_REG_HH,
			&mut self.state.percussion_on[4],
			midi,
			channel,
			0x2A.into(),
			self.state.hh_vol,
		);
		// Note Value 46 (Open HiHat) replaced with 51 (Ride Cymbal 1),
		// because REG_TCT is often used with REG_HH at the same time and
		// prevents the Cymbal from playing
		//	Case YM2413_REG_TEST
		//Case YM2413_REG_self.state.FNum_LSB..=(YM2413_REG_self.state.FNum_LSB + 8)
		//CH = Register - YM2413_REG_self.state.FNum_LSB
		//
		//if self.config.ym2413_ch_disabled[CH as usize] = 1 { return }
		//
		//self.state.FNum_LSB[CH as usize] = Data
		//Case YM2413_REG_SUS_KEY_BLOCK_self.state.FNum_MSB..=(YM2413_REG_SUS_KEY_BLOCK_self.state.FNum_MSB + 8)
	}
}
pub(crate) fn ym2413_vol_to_db(tl: u8) -> f64 {
	-f64::from(tl) * 3.0
}

use crate::config::Config;
use crate::midi_shim::{
	db_to_midi_vol, MIDIShim, MIDI_MODULATOR_WHEEL, MIDI_PAN, MIDI_PAN_CENTER, MIDI_PAN_LEFT,
	MIDI_PAN_RIGHT, MIDI_VOLUME,
};
use crate::strict;
use crate::utils::shift;
use crate::utils::FactoredState;
use crate::ym3812::ym3812_vol_to_db;
use anyhow::{bail, Result};
use midly::num::u7;
// YM2151 Register Constants
#[allow(dead_code)]
const YM2151_TEST: u8 = 0x01;
const YM2151_KEY_ON: u8 = 0x08;
const YM2151_NOISE: u8 = 0x0F;
const YM2151_CLK_A_MSB: u8 = 0x10;
const YM2151_CLK_A_LSB: u8 = 0x11;
const YM2151_CLK_B: u8 = 0x12;
const YM2151_CSM_IRQ_RESET_EN_TIMER: u8 = 0x14;
const YM2151_LFO_FREQ: u8 = 0x18;
const YM2151_PMD_AMD: u8 = 0x19;
const YM2151_CT1_CT2_WAVEFORM: u8 = 0x1B;
const YM2151_PAN_FB_CONNECTION: u8 = 0x20;
const YM2151_PAN_FB_CONNECTION_END: u8 = YM2151_PAN_FB_CONNECTION + 0x07;
const YM2151_BLK_FNUM: u8 = 0x28;
const YM2151_BLK_FNUM_END: u8 = YM2151_BLK_FNUM + 0x07;
const YM2151_KF: u8 = 0x30;
const YM2151_KF_END: u8 = YM2151_KF + 0x07;
const YM2151_PMS_AMS: u8 = 0x38;
const YM2151_PMS_AMS_END: u8 = YM2151_PMS_AMS + 0x07;
const YM2151_DT1_MUL: u8 = 0x40;
const YM2151_DT1_MUL_END: u8 = YM2151_DT1_MUL + 0x1F;
const YM2151_TL: u8 = 0x60;
const YM2151_TL_END: u8 = YM2151_TL + 0x1F;
#[allow(dead_code)]
const YM2151_KS_AR: u8 = 0x80;
#[allow(dead_code)]
const YM2151_LFO_AM_EN_D1R: u8 = 0xA0;
#[allow(dead_code)]
const YM2151_DT2_D2R: u8 = 0xC0;
#[allow(dead_code)]
const YM2151_D1L_RR: u8 = 0xE0;
fn ym2151_fnum_to_midi(mut fnum: u8, mut block: u8) -> Result<u7> {
	let mut note_val: u8;
	if fnum > 0x0F {
		strict!("Invalid fnum {}", fnum);
	}
	block = block.clamp(0, 0x07);
	fnum &= 0x0F;
	if (fnum & 0x03) != 0x03 {
		note_val = (fnum & 0xC) >> 2;
		note_val = note_val * 3 + (fnum & 0x3);
		note_val += 61;
	} else {
		note_val = 0xFF
	}
	if note_val == 0xFF {
		Ok(0xFF.into())
	} else {
		Ok((note_val + (block - 0x4) * 12).into())
		//if YM2151FNumToMidi = 0x0 { Stop
	}
}
#[derive(Default)]
pub(crate) struct YM2151State {
	slot: [u8; 8],
	//	DT(5, 3) As Byte, MULTI(5, 3) As Byte
	tl: [[u8; 4]; 8],
	//KF As Byte
	//	KS(5, 3) As Byte, AR(5, 3) As Byte
	//	DR: [u8; 5, 3],
	//	SR: [u8; 5, 3],
	//	SL(5, 3) As Byte, RR(5, 3) As Byte
	block: [u8; 8],
	feedback: [u8; 8],
	connection: [u8; 8],
	factored: FactoredState,
}
pub(crate) struct YM2151<'config> {
	state: YM2151State,
	config: &'config Config,
}
impl<'config> YM2151<'config> {
	pub(crate) fn new<'c: 'config>(config: &'c Config, opt_state: Option<YM2151State>) -> Self {
		YM2151 {
			state: opt_state.unwrap_or_default(),
			config,
		}
	}
	//FIXME: This subroutine used a lot of static state. It needs to be re-written to something more sane.
	pub(crate) fn command_handle(
		&mut self,
		register: u8,
		data: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		match register {
			YM2151_KEY_ON => {
				process_key_on(data, self.config, &mut self.state, midi)?;
			},
			YM2151_NOISE => (), // NOISE: NE, NFRQ
			//nen = if(Data & 0x80, 1, 0)
			//nfrq = Data & 0x31
			YM2151_CLK_A_MSB => (),
			YM2151_CLK_A_LSB => (),
			YM2151_CLK_B => (),
			YM2151_CSM_IRQ_RESET_EN_TIMER => (),
			YM2151_LFO_FREQ => (),
			YM2151_PMD_AMD => (),
			//depth = Data & 0x7F
			//ispm = if(Data & 0x80, 1, 0)
			YM2151_CT1_CT2_WAVEFORM => (),
			//w = Data & 0x3	// W: 0 - ramp, 1 - sq, 2 - tri, 3 - noise
			YM2151_PAN_FB_CONNECTION..=YM2151_PAN_FB_CONNECTION_END => {
				process_pan_feedback(
					register,
					self.config,
					data,
					&mut self.state,
					midi,
				)?;
			},
			YM2151_BLK_FNUM..=YM2151_BLK_FNUM_END => {
				process_fnum(register, self.config, &mut self.state, data, midi)?;
			},
			YM2151_KF..=YM2151_KF_END => {
				process_kf(register, self.config, &mut self.state, data, midi)?;
			},
			YM2151_PMS_AMS..=YM2151_PMS_AMS_END => {
				process_modulation(register, data, midi);
			},
			YM2151_DT1_MUL..=YM2151_DT1_MUL_END => {
				//let channel = (register & 0x3F) / 0x8;
				//DT1 = Fix(Data / 0x10) & 0x7
				//MUl = Data & 0xF
			},
			YM2151_TL..=YM2151_TL_END => {
				process_total_level(
					register,
					&mut self.state,
					data,
					self.config,
					midi,
				);
			},
			/*YM2151_KS_AR..=YM2151_KS_AR + 0x1F
				channel = Fix((register & 0x3F) / 0x8)
				//KS = Fix(Data / 0x40) & 0x3
				//AR = Data & 0x1F
			YM2151_LFO_AM_EN_D1R..=YM2151_LFO_AM_EN_D1R + 0x1F
				channel = Fix((register & 0x3F) / 0x8)
				//AMS_EN = if(Data & 0x80, 1, 0)
				//D1R = Data & 0x1F
			YM2151_DT2_D2R..=YM2151_DT2_D2R + 0x1F
				channel = Fix((register & 0x3F) / 0x8)
				//DT2 = Fix(Data / 0x40) & 0x3
				//D2R = Data & 0x1F
			YM2151_D1L_RR..=YM2151_D1L_RR + 0x1F
				channel = Fix((register & 0x1F) / 0x4)
				//D1L = Fix(Data / 0x10) & 0xF
				//RR = Data & 0xF
				//Abort("YM2151 ADSR - SL " & D1L & " RR " & RR);*/
			_ => strict!(),
		}
		Ok(())
	}
}
fn process_total_level(
	register: u8,
	state: &mut YM2151State,
	data: u8,
	config: &Config,
	midi: &mut MIDIShim,
) {
	let channel = register & 0x7;
	let op = (register & 0x1F) / 0x8;
	state.tl[channel as usize][op as usize] = data & 0x7F;
	if op < 3 {
		return;
	}
	if config.ym2151_vol_disabled[channel as usize] {
		return;
	}
	let temp_vol = db_to_midi_vol(ym3812_vol_to_db(state.tl[channel as usize][3]) / 4.0);
	//match state.connection[channel as usize]
	//0, 1, 2, 3
	//4
	//	TempLng = (CLng(state.tl[channel as usize][1]) + state.tl[channel as usize][3]) / 2
	//5, 6
	//	TempLng = (CLng(state.tl[channel as usize][1]) + state.tl[channel as usize][2] + state.tl[channel as usize][3]) / 3
	//7
	//	TempLng = (CLng(state.tl[channel as usize][0]) + state.tl[channel as usize][1] + state.tl[channel as usize][2] + state.tl[channel as usize][3]) / 4
	//}
	if temp_vol != state.factored.midi_volume[channel as usize] {
		state.factored.midi_volume[channel as usize] = temp_vol;
		midi.controller_write(
			channel.into(),
			MIDI_VOLUME,
			state.factored.midi_volume[channel as usize],
		);
	}
}
fn process_modulation(register: u8, data: u8, midi: &mut MIDIShim) {
	let channel = register & 0x7;
	//PMS = Fix(Data / 0x10) & 0x7
	//AMS = Data & 0x3
	let temp_double = ((data & 0x70) as f32) / (0x70 as f32);
	let temp = (temp_double * temp_double * (0x7F as f32)) as u8;
	midi.controller_write(channel.into(), MIDI_MODULATOR_WHEEL, temp.into());
}
fn process_kf(
	register: u8,
	config: &Config,
	state: &mut YM2151State,
	data: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	let channel = register & 0x7;
	if config.ym2151_ch_disabled[channel as usize] {
		return Ok(());
	}
	state.factored.fnum_lsb[channel as usize] = (data >> 2) & 0x3F;
	state.factored.note_1[channel as usize] = state.factored.note_2[channel as usize];
	state.factored.note_2[channel as usize] =
		ym2151_fnum_to_midi(
			state.factored.fnum_2[channel as usize] as u8,
			state.block[channel as usize],
		)?
		.as_int() as f64 + (state.factored.fnum_lsb[channel as usize] as f64 / 0x40 as f64);
	// Key Fraction goes from 0x00 to 0xFC
	// 0x00 is no change, 0x100 would be a full semitone
	//TempLng = MIDI_PITCHWHEEL_CENTER + state.factored.fnum_lsb[channel as usize] * 0x40	 //(0x1000 / 0x40)
	//if TempLng != state.factored.midi_wheel[channel as usize] {
	//	state.factored.midi_wheel[channel as usize] = TempLng
	//	midi.event_write(Midi { channel: channel, message: PitchBend { bend: state.factored.midi_wheel[channel as usize] }});
	//}
	if state.factored.note_on_2[channel as usize] {
		midi.do_note_on(
			state.factored.note_1[channel as usize],
			state.factored.note_2[channel as usize],
			channel.into(),
			&mut state.factored.midi_note[channel as usize],
			&mut state.factored.midi_wheel[channel as usize],
			None,
			None,
		)?;
	}
	Ok(())
}
fn process_fnum(
	register: u8,
	config: &Config,
	state: &mut YM2151State,
	data: u8,
	midi: &mut MIDIShim,
) -> Result<()> {
	let channel = register & 0x7;
	if config.ym2151_ch_disabled[channel as usize] {
		return Ok(());
	}
	state.block[channel as usize] = (data / 0x10) & 0x7;
	state.factored.fnum_msb[channel as usize] = data & 0xF;
	state.factored.fnum_1[channel as usize] = shift(
		&mut state.factored.fnum_2[channel as usize],
		state.factored.fnum_msb[channel as usize] as u32,
	);
	state.factored.note_1[channel as usize] = state.factored.note_2[channel as usize];
	state.factored.note_2[channel as usize] =
		((ym2151_fnum_to_midi(
			state.factored.fnum_2[channel as usize] as u8,
			state.block[channel as usize],
		)? + state.factored.fnum_lsb[channel as usize].into())
		.as_int() as f64) / (0x40 as f64);
	//FIXME: fnum_2 really shouldn't be a long
	//if state.factored.note_on_2[channel as usize] = 1 | NOTE_ON_MODE & 0x2 != 0 {
	//	if state.factored.note_2[channel as usize] != state.factored.note_1[channel as usize] | (NOTE_ON_MODE & 0x2) = 0x2 {
	//		midi.event_write(Midi { channel: channel, message: NoteOff { key: state.factored.note_1[channel as usize], vel: 0x0 }});
	//		midi.event_write(Midi { channel: channel, message: NoteOn { key: state.factored.note_2[channel as usize], vel: 0x7F }});
	//	}
	//}
	if state.factored.note_on_2[channel as usize] {
		midi.do_note_on(
			state.factored.note_1[channel as usize],
			state.factored.note_2[channel as usize],
			channel.into(),
			&mut state.factored.midi_note[channel as usize],
			&mut state.factored.midi_wheel[channel as usize],
			None,
			None,
		)?;
	}
	Ok(())
}
fn process_pan_feedback(
	register: u8,
	config: &Config,
	data: u8,
	state: &mut YM2151State,
	midi: &mut MIDIShim,
) -> Result<()> {
	let channel = register & 0x7;
	if config.ym2151_ch_disabled[channel as usize] {
		return Ok(());
	}
	if !config.ym2151_pan_disabled[channel as usize] {
		state.factored.midi_pan[channel as usize] = match (data / 0x40) & 0x3 {
			0x1 => MIDI_PAN_LEFT,
			0x2 => MIDI_PAN_RIGHT,
			0x3 | 0x0 => MIDI_PAN_CENTER,
			_ => bail!("Invalid pan data"),
		};
		midi.controller_write(
			channel.into(),
			MIDI_PAN,
			state.factored.midi_pan[channel as usize],
		);
	}
	state.feedback[channel as usize] = (data / 0x8) & 0x7;
	state.connection[channel as usize] = data & 0x7;
	if !config.ym2151_prog_disabled[channel as usize] {
		state.factored.midi_instrument[channel as usize] = (data & 0x3F).into();
		midi.program_change_write(
			channel.into(),
			state.factored.midi_instrument[channel as usize],
		);
	}
	Ok(())
}
fn process_key_on(
	data: u8,
	config: &Config,
	state: &mut YM2151State,
	midi: &mut MIDIShim,
) -> Result<()> {
	let channel = data & 0x7;
	if config.ym2151_ch_disabled[channel as usize] {
		return Ok(());
	}
	state.slot[channel as usize] = (data / 0x8) & 0xF;
	state.factored.note_on_1[channel as usize] = state.factored.note_on_2[channel as usize];
	state.factored.note_on_2[channel as usize] = state.slot[channel as usize] > 0;
	if
	/*NOTE_ON_MODE & 0x1*/
	true {
		//FIXME: wtf?
		// This is a const. wtf?
		if (state.factored.note_on_2[channel as usize]
			!= state.factored.note_on_1[channel as usize])
			&& (state.factored.note_2[channel as usize] != 0.0)
		{
			if !state.factored.note_on_2[channel as usize] {
				//midi.event_write(Midi { channel: channel, message: NoteOff { key: state.factored.note_2[channel as usize], vel: 0x0 }});
				midi.do_note_on(
					state.factored.note_1[channel as usize],
					0xFF as f64,
					channel.into(),
					&mut state.factored.midi_note[channel as usize],
					&mut state.factored.midi_wheel[channel as usize],
					Some(255),
					None,
				)?;
			} else {
				//midi.event_write(Midi { channel: channel, message: NoteOn { key: state.factored.note_2[channel as usize], vel: 0x7F }});
				midi.do_note_on(
					state.factored.note_1[channel as usize],
					state.factored.note_2[channel as usize],
					channel.into(),
					&mut state.factored.midi_note[channel as usize],
					&mut state.factored.midi_wheel[channel as usize],
					Some(255),
					None,
				)?;
			}
		}
	}
	Ok(())
}

#[allow(unused_imports)]
use crate::midi_shim::{
	MIDIShim, MIDI_PAN, MIDI_PAN_CENTER, MIDI_PAN_LEFT, MIDI_PAN_RIGHT, MIDI_PATCH_GUNSHOT,
	MIDI_PATCH_LEAD_1_SQUARE, MIDI_PITCHWHEEL_CENTER, MIDI_PITCHWHEEL_MAX, MIDI_PITCHWHEEL_MIN,
	MIDI_RPN_LSB, MIDI_RPN_MSB, MIDI_VOLUME, RPN_PITCH_BEND_RANGE_L, RPN_PITCH_BEND_RANGE_M,
};
use midly::num::{u14, u7, u4};
pub(crate) const CHN_DAC: u4 = u4::new(9);
#[allow(dead_code)]
pub(crate) const MIDI_NOTE_STEPS: u16 = 12;
//const PITCHWHEEL_SENSITIVITY = 64 //+/- n Semitones
//const PITCHWHEEL_STEPS_REAL = 8192 / PITCHWHEEL_SENSITIVITY
//const PITCHWHEEL_STEPS_DEFAULT = 4096 //16384 / 4
pub(crate) const PITCHWHEEL_SENSITIVITY_DEFAULT: u7 = u7::new(0x02); //+/- n Semitones
pub(crate) const PITCHWHEEL_STEPS_DEFAULT: u14 = u14::new(4096); //16384 / 4
//const MIDI_NOTE_CURVE = 2 ^ (1 / MIDI_NOTE_STEPS)
//const PITCHWHEEL_CURVE = MIDI_NOTE_CURVE ^ (1 / PITCHWHEEL_STEPS)
// Timing
//FIXME: Ensure that floating point numbers are the correct representation.
#[allow(dead_code)]
pub(crate) const OSC1: f64 = 53693100.0;
#[allow(dead_code)]
pub(crate) const FM_YM2413_SN76489: f64 = OSC1 / 15.0;
#[allow(dead_code)]
pub(crate) const FSAM_YM2413_SN76489: f64 = FM_YM2413_SN76489 / 72.0;
#[allow(dead_code)]
pub(crate) const FM_YM2612: f64 = OSC1 / 14.0;
#[allow(dead_code)]
pub(crate) const FSAM_YM2612: f64 = FM_YM2612 / 72.0;
pub(crate) const OPL_TYPE_YM3526: u8 = 0x1;
pub(crate) const OPL_TYPE_YM3812: u8 = 0x2;
pub(crate) const OPL_TYPE_YMF262: u8 = 0x3;
pub(crate) const OPL_TYPE_Y8950: u8 = 0x8;
pub(crate) const OPN_TYPE_YM2203: u8 = 0x1;
pub(crate) const OPN_TYPE_YM2608: u8 = 0x2;
pub(crate) const OPN_TYPE_YM2610: u8 = 0x3;
pub(crate) const OPN_TYPE_YM2612: u8 = 0x4;

use crate::ay8910::AY8910;
use crate::config::Config;
use crate::gameboy::GameBoy;
use crate::gd3::{get_gd3_header, Gd3Header};
use crate::midi_shim::{MIDIShim, TEXT_LOOP_END, TEXT_LOOP_START};
use crate::nesapu::NESAPU;
use crate::pcm::SegaPCM;
use crate::sn76489::SN76489;
use crate::vgm2mid::{
	OPL_TYPE_Y8950, OPL_TYPE_YM3526, OPL_TYPE_YM3812, OPL_TYPE_YMF262, OPN_TYPE_YM2203,
	OPN_TYPE_YM2608, OPN_TYPE_YM2610, OPN_TYPE_YM2612,
};
use crate::ym2151::YM2151;
use crate::ym2413::YM2413;
use crate::ym2612::{YM2612, YM2612_DAC};
use crate::ym3812::YM3812;
use crate::ymf278::YMF278;
use crate::{strict, verbose};
use anyhow::{bail, Result};
use midly::MetaMessage::Marker;
use midly::TrackEventKind::Meta;
#[derive(Debug)]
#[allow(dead_code)]
pub(crate) struct VgmHeader {
	str_vgm: [u8; 4], // 0x00
	eof_offset: u32,
	version: u32,
	hz_sn76489: u32,
	hz_ym2413: u32, // 0x10
	gd3_offset: u32,
	total_samples: u32,
	loop_offset: u32,
	loop_samples: u32, // 0x20
	rate: u32,
	sn_feedback: u16,
	sn_stwidth: u8,
	sn_flags: u8,
	hz_ym2612: u32,
	hz_ym2151: u32, // 0x30
	data_offset: u32,
	hz_spcm: u32,
	spcm_intf: u32,
	hz_rf5c68: u32, // 0x40
	hz_ym2203: u32,
	hz_ym2608: u32,
	hz_ym2610b: u32,
	hz_ym3812: u32, // 0x50
	hz_ym3526: u32,
	hz_y8950: u32,
	hz_ymf262: u32,
	hz_ymf278b: u32, // 0x60
	hz_ymf271: u32,
	hz_ymz280b: u32,
	hz_rf5c164: u32,
	hz_pwm: u32, // 0x70
	hz_ay8910: u32,
	ay_type: u8,
	ay_flags: u8,
	ay_flags2203: u8,
	ay_flags2608: u8,
	volume_modifier: u8,
	reserved_1: u8,
	loop_base: u8,
	loop_modifier: u8,
	hz_gb_dmg: u32, // 0x80
	hz_nes_apu: u32,
	hz_multipcm: u32,
	hz_upd7759: u32,
	hz_okim6258: u32, // 0x90
	okim_flags: u8,
	k0_flags: u8,
	c140_type: u8,
	reserved_2: u8,
	hz_okim5295: u32,
	hz_k051649: u32,
	hz_k054539: u32, // 0xA0
	hz_huc6280: u32,
	hz_c140: u32,
	k053260: u32,
	hz_pokey: u32, // 0xB0
	hz_qsound: u32,
	hz_scsp: u32,
	extra_header_offset: u32,
}
struct Vgm {
	file_header: VgmHeader,
	data_bank: Vec<u8>,
	data_block_count: u32,
	data_block_pos: [usize; 0xFF],
	gt_gd3_tag: Gd3Header,
	conversion_status_current: u32,
	conversion_status_total: u32,
	clock_sn76489: u32,
	clock_ym2151: u32,
	clock_ym2413: u32,
	clock_ym2612: u32,
	clock_ay8910: u32,
}
// VGM Command Constants
const GG_STEREO: u8 = 0x4F;
const SN76489: u8 = 0x50;
const YM2413: u8 = 0x51;
const YM2612_P0: u8 = 0x52;
const YM2612_P1: u8 = 0x53;
const YM2151: u8 = 0x54;
const YM2203: u8 = 0x55;
const YM2608_P0: u8 = 0x56;
const YM2608_P1: u8 = 0x57;
const YM2610_P0: u8 = 0x58;
const YM2610_P1: u8 = 0x59;
const YM3812: u8 = 0x5A;
const YM3526: u8 = 0x5B;
const Y8950: u8 = 0x5C;
const YMF262_P0: u8 = 0x5E;
const YMF262_P1: u8 = 0x5F;
const RF5C68_REG: u8 = 0xB0;
const SPCM_MEM: u8 = 0xC0;
const RF5C68_MEM: u8 = 0xC1;
const DATABNK_SEEK: u8 = 0xE0;
const AY8910: u8 = 0xA0;
const WAIT_N_SAMPLES: u8 = 0x61;
const WAIT_735_SAMPLES: u8 = 0x62;
const WAIT_882_SAMPLES: u8 = 0x63;
const END_OF_SOUND_DATA: u8 = 0x66;
const DATA_BLOCK: u8 = 0x67;
fn correct_header_for_version(mut header: VgmHeader) -> VgmHeader {
	// correct the header for old files and calculate absolute offsets
	header.eof_offset += 0x4;
	if header.gd3_offset > 0x0 {
		header.gd3_offset += 0x14;
	}
	if header.loop_offset > 0x0 {
		header.loop_offset += 0x1C;
	}
	if header.version < 0x101 {
		header.rate = 0x0;
	}
	if header.version < 0x110 {
		header.sn_feedback = 0x9;
		header.sn_stwidth = 16;
		header.hz_ym2612 = header.hz_ym2413;
		header.hz_ym2612 = header.hz_ym2413;
	}
	if header.version < 0x150 || header.data_offset == 0x0 {
		header.data_offset = 0xC;
	}
	if header.version < 0x151 || header.data_offset <= 0xC {
		header.hz_spcm = 0x00;
		header.spcm_intf = 0x00;
		header.hz_rf5c68 = 0x00;
		header.hz_ym2203 = 0x00;
		header.hz_ym2608 = 0x00;
		header.hz_ym2610b = 0x00;
		header.hz_ym3812 = 0x00;
		header.hz_ym3526 = 0x00;
		header.hz_y8950 = 0x00;
		header.hz_ymf262 = 0x00;
		header.hz_ymf278b = 0x00;
		header.hz_ymf271 = 0x00;
		header.hz_ymz280b = 0x00;
		header.hz_rf5c164 = 0x00;
		header.hz_pwm = 0x00;
		header.hz_ay8910 = 0x00;
		header.ay_type = 0x00;
		header.ay_flags = 0x00;
		header.ay_flags2203 = 0x00;
		header.ay_flags2608 = 0x00;
	}
	header.data_offset += 0x34;
	header
}
fn parse_vgm_header(data: &[u8]) -> Result<VgmHeader> {
	let mut header = VgmHeader {
		str_vgm: [0; 4],
		eof_offset: 0,
		version: 0,
		hz_sn76489: 0,
		hz_ym2413: 0,
		gd3_offset: 0,
		total_samples: 0,
		loop_offset: 0,
		loop_samples: 0,
		rate: 0,
		sn_feedback: 0,
		sn_stwidth: 0,
		sn_flags: 0,
		hz_ym2612: 0,
		hz_ym2151: 0,
		data_offset: 0,
		hz_spcm: 0,
		spcm_intf: 0,
		hz_rf5c68: 0,
		hz_ym2203: 0,
		hz_ym2608: 0,
		hz_ym2610b: 0,
		hz_ym3812: 0,
		hz_ym3526: 0,
		hz_y8950: 0,
		hz_ymf262: 0,
		hz_ymf278b: 0,
		hz_ymf271: 0,
		hz_ymz280b: 0,
		hz_rf5c164: 0,
		hz_pwm: 0,
		hz_ay8910: 0,
		ay_type: 0,
		ay_flags: 0,
		ay_flags2203: 0,
		ay_flags2608: 0,
		volume_modifier: 0,
		reserved_1: 0,
		loop_base: 0,
		loop_modifier: 0,
		hz_gb_dmg: 0,
		hz_nes_apu: 0,
		hz_multipcm: 0,
		hz_upd7759: 0,
		hz_okim6258: 0,
		okim_flags: 0,
		k0_flags: 0,
		c140_type: 0,
		reserved_2: 0,
		hz_okim5295: 0,
		hz_k051649: 0,
		hz_k054539: 0,
		hz_huc6280: 0,
		hz_c140: 0,
		k053260: 0,
		hz_pokey: 0,
		hz_qsound: 0,
		hz_scsp: 0,
		extra_header_offset: 0,
	};
	let mut pos = 0;
	header.str_vgm.clone_from_slice(&data[0..=3]);
	if header.str_vgm != "Vgm ".as_bytes() {
		verbose!("VGM Header is incorrect.");
		strict!();
	}
	pos += 4;
	header.eof_offset = extract_u32(data, &mut pos);
	if data.len() < header.eof_offset as usize {
		verbose!("VGM EOF is incorrect.");
		strict!();
	}
	header.version = extract_u32(data, &mut pos);
	header.hz_sn76489 = extract_u32(data, &mut pos);
	header.hz_ym2413 = extract_u32(data, &mut pos);
	header.gd3_offset = extract_u32(data, &mut pos);
	header.total_samples = extract_u32(data, &mut pos);
	header.loop_offset = extract_u32(data, &mut pos);
	header.loop_samples = extract_u32(data, &mut pos);
	header.rate = extract_u32(data, &mut pos);
	pos += 4; //FIXME: extract others
	header.hz_ym2612 = extract_u32(data, &mut pos);
	header.hz_ym2151 = extract_u32(data, &mut pos);
	header.data_offset = extract_u32(data, &mut pos);
	header.hz_spcm = extract_u32(data, &mut pos);
	header.spcm_intf = extract_u32(data, &mut pos);
	header.hz_rf5c68 = extract_u32(data, &mut pos);
	header.hz_ym2203 = extract_u32(data, &mut pos);
	header.hz_ym2608 = extract_u32(data, &mut pos);
	header.hz_ym2610b = extract_u32(data, &mut pos);
	header.hz_ym3812 = extract_u32(data, &mut pos);
	header.hz_ym3526 = extract_u32(data, &mut pos);
	header.hz_y8950 = extract_u32(data, &mut pos);
	header.hz_ymf262 = extract_u32(data, &mut pos);
	header.hz_ymf278b = extract_u32(data, &mut pos);
	header = correct_header_for_version(header);
	Ok(header)
}
fn extract_u32(data: &[u8], pos: &mut usize) -> u32 {
	let mut dst = [0u8; 4];
	dst.clone_from_slice(&data[*pos..=*pos + 3]);
	*pos += 4;
	u32::from_le_bytes(dst)
}
fn parse_vgm_data(data: &[u8]) -> Result<Vgm> {
	Ok(Vgm {
		file_header: parse_vgm_header(data)?,
		data_bank: Vec::new(),
		data_block_count: 0,
		data_block_pos: [0; 0xFF],
		gt_gd3_tag: get_gd3_header(None)?,
		conversion_status_current: 0,
		conversion_status_total: 0,
		clock_sn76489: 0,
		clock_ym2151: 0,
		clock_ym2413: 0,
		clock_ym2612: 0,
		clock_ay8910: 0,
	})
}
pub(crate) fn convert_vgm_to_mid(file_data: &[u8], config: &Config, midi: &mut MIDIShim) -> Result<()> {
	let mut vgm = parse_vgm_data(file_data)?;
	dbg!(&vgm.file_header);
	let mut file_pos: usize;
	let (mut command, mut port, mut register, mut data): (u8, u8, u8, u8);
	let _detimer: f32;
	let mut last_data_note: u8;
	let mut data_bank_pos: usize = 0;
	let temp_long: u32 = 0;
	let mut last_sn76489_cmd: [u8; 2] = [0; 2];
	let mut stop_vgm: bool = false;
	let mut loop_pos: usize = 0;
	let mut dac_wrt: bool;
	let mut cur_loop: u16 = 0;
	vgm.conversion_status_current = 0x0;
	vgm.conversion_status_total = vgm.file_header.eof_offset - vgm.file_header.data_offset;
	let clock_sn76489 = vgm.file_header.hz_sn76489 & 0x3FFFFFFF;
	vgm.clock_ym2413 = vgm.file_header.hz_ym2413 & 0x3FFFFFFF;
	vgm.clock_ym2612 = vgm.file_header.hz_ym2612 & 0x3FFFFFFF;
	vgm.clock_ym2151 = vgm.file_header.hz_ym2151 & 0x3FFFFFFF;
	let mut fsam_2612 = f64::from(vgm.clock_ym2612) / 72.0;
	let mut fsam_3812 = 0.0;
	vgm.clock_ay8910 = vgm.file_header.hz_ay8910;
	let t6w28_sn76489 = (vgm.file_header.hz_sn76489 & 0xC0000000) == 0xC0000000;
	let mut ay8910 = AY8910::new(config, None);
	let mut sn76489 = SN76489::new(t6w28_sn76489, clock_sn76489, config, None);
	let mut segapcm = SegaPCM::new(None);
	let mut gameboy = GameBoy::new(config, None);
	let mut nes_apu = NESAPU::new(config, None);
	let mut ym2151 = YM2151::new(config, None);
	let mut ym2413 = YM2413::new(config, None);
	let mut ym2612 = YM2612::new(config, None);
	let mut ym3812 = YM3812::new(config, None);
	let mut ymf278 = YMF278::new(config, None);
	gameboy.init(midi);
	if vgm.file_header.hz_ymf278b != 0 {
		ymf278.load_opl4_instrument_set()?;
	}
	let loop_available = vgm.file_header.loop_offset > 0;
	if loop_available { loop_pos = vgm.file_header.loop_offset as usize }
	//detimer = Timer;
	//Call DAC_WriteOpen
	last_sn76489_cmd[0x0] = 0x0;
	last_sn76489_cmd[0x1] = 0x0;
	dac_wrt = false;
	let mut _dac_data_byte = 0x80;
	last_data_note = 0xFF;
	//FIXME: Erase MID_Trackdata;
	//FIXME: ReDim MID_Trackdata(0): u8;
	//WriteMidiTag();
	midi.data_init(
		vgm.clock_sn76489 != 0 || vgm.clock_ay8910 != 0,
		t6w28_sn76489,
	);
	file_pos = vgm.file_header.data_offset as usize;
	loop {
		let mut wait = 0;
		let mut cur_chip = 0x00u8;
		command = file_data[file_pos];
		if config.dualchips {
			match command {
				0x30 => {
					cur_chip = 0x1;
					command += 0x20;
				},
				0x3F => {
					cur_chip = 0x1;
					command += 0x10;
				},
				0xA1..=0xAC => {
					cur_chip = 0x1;
					command -= 0x50;
				},
				_ => verbose!("Invalid dual-chip command: {}", command),
			}
		}
		match command {
			GG_STEREO => {
				data = file_data[file_pos + 1];
				sn76489.gg_stereo_handle(data, cur_chip, midi)?;
				file_pos += 2;
			},
			SN76489 => {
				data = file_data[file_pos + 1];
				sn76489.command_handle(data, cur_chip, midi)?;
				file_pos += 2
			},
			YM2413 => {
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				ym2413.command_handle(
					register,
					data,
					vgm.clock_ym2413,
					midi,
				)?;
				file_pos += 3;
			},
			YM2612_P0 | YM2612_P1 | YM2203..=YM2610_P1 => {
				//if command = YM2612_P0 {
				//	port = 0
				//} else if command = YM2612_P1 {
				//	port = 3
				//}
				(port, fsam_2612) = match command {
					YM2612_P0 | YM2612_P1 => {
						ym2612.state.opn_type = OPN_TYPE_YM2612;
						(command & 0x1,
						f64::from(
							vgm.file_header.hz_ym2612 & 0x3FFFFFFF,
						) / 72.0)
					},
					YM2203 => {
						ym2612.state.opn_type = OPN_TYPE_YM2203;
						(0x0,
						f64::from(
							vgm.file_header.hz_ym2203 & 0x3FFFFFFF,
						) / 72.0)
					},
					YM2608_P0 | YM2608_P1 => {
						ym2612.state.opn_type = OPN_TYPE_YM2608;
						(command & 0x1,
						f64::from(
							vgm.file_header.hz_ym2608 & 0x3FFFFFFF,
						) / 72.0)
					},
					YM2610_P0 | YM2610_P1 => {
						ym2612.state.opn_type = OPN_TYPE_YM2610;
						(command & 0x1,
						f64::from(
							vgm.file_header.hz_ym2610b & 0x3FFFFFFF,
						) / 72.0)
					},
					_ => bail!("Invalid register"),
				};
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				ym2612.command_handle(
					port * 0x3,
					register,
					data,
					fsam_2612,
					midi,
				)?;
				if register == YM2612_DAC {
					_dac_data_byte = data;
					dac_wrt = true;
				}
				file_pos += 3;
			},
			YM2151 => {
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				ym2151.command_handle(
					register,
					data,
					midi,
				)?;
				file_pos += 3;
			},
			YM3812..=Y8950 => {
				match command {
					YM3812 => {
						ym3812.state.opl_type = OPL_TYPE_YM3812;
						fsam_3812 = f64::from(
							vgm.file_header.hz_ym3812 & 0x3FFFFFFF,
						) / 72.0;
					},
					YM3526 => {
						ym3812.state.opl_type = OPL_TYPE_YM3526;
						fsam_3812 = f64::from(
							vgm.file_header.hz_ym3526 & 0x3FFFFFFF,
						) / 72.0;
					},
					Y8950 => {
						ym3812.state.opl_type = OPL_TYPE_Y8950;
						fsam_3812 = f64::from(
							vgm.file_header.hz_y8950 & 0x3FFFFFFF,
						) / 72.0;
					},
					_ => (),
				}
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				ym3812.command_handle(register as u16, data, fsam_3812, midi)?;
				file_pos += 3;
			},
			YMF262_P0..=YMF262_P1 => {
				ym3812.state.opl_type = OPL_TYPE_YMF262;
				fsam_3812 =
					f64::from(vgm.file_header.hz_ymf262 & 0x3FFFFFFF) / 288.0;
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				ym3812.command_handle(
					(command as u16 & 0x1) << 8 | register as u16,
					data,
					fsam_3812,
					midi,
				)?;
				file_pos += 3;
			},
			0xD0 => {
				port = file_data[file_pos + 1];
				register = file_data[file_pos + 2];
				data = file_data[file_pos + 3];
				ymf278.command_handle(
					port,
					register,
					data,
					&mut ym3812,
					fsam_3812,
					midi,
				)?;
				file_pos += 4;
			},
			0xB3 => {
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				gameboy.command_handle(
					register,
					data,
					midi,
				)?;
				file_pos += 3;
			},
			0xB4 => {
				if vgm.clock_sn76489 == 0 {
					vgm.clock_sn76489 = 1
				}
				register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				nes_apu.command_handle(
					register,
					data,
					midi,
				)?;
				file_pos += 3;
			},
			WAIT_N_SAMPLES => {
				wait = (file_data[file_pos + 2] as u32) << 8;
				wait |= file_data[file_pos + 1] as u32;
				file_pos += 3
			},
			WAIT_735_SAMPLES => {
				wait = 735;
				file_pos += 1
			},
			WAIT_882_SAMPLES => {
				wait = 882;
				file_pos += 1;
			},
			END_OF_SOUND_DATA => stop_vgm = true,
			DATA_BLOCK => {
				//FIXME Why is this an unused assignment?
				//register = file_data[file_pos + 1];
				data = file_data[file_pos + 2];
				wait = file_data[file_pos + 3] as u32
					| (file_data[file_pos + 4] as u32) << 0x08
					| (file_data[file_pos + 5] as u32) << 0x10
					| (file_data[file_pos + 6] as u32) << 0x18;
				file_pos += 7;
				if data == 0x00 {
					let previous_length = vgm.data_bank.len();
					vgm.data_bank
						.resize(vgm.data_bank.len() + wait as usize, 0);
					for offset in 0x0..wait {
						vgm.data_bank[previous_length + offset as usize] =
							file_data[file_pos + offset as usize]
					}
				}
				file_pos += TryInto::<usize>::try_into(wait).unwrap();
				wait = 0;
			},
			0x70..=0x7F => {
				wait = ((command & 0x0F) + 1).into();
				file_pos += 1;
			},
			0x80..=0x8F => {
				wait = (command & 0x0F).into();
				port = 0x00;
				register = YM2612_DAC;
				data = vgm.data_bank[data_bank_pos];
				ym2612.command_handle(port, register, data, fsam_2612, midi)?;
				_dac_data_byte = data;
				dac_wrt = true;
				data_bank_pos += 1;
				file_pos += 1;
			},
			AY8910 => {
				port = file_data[file_pos + 1] / 0x80;
				register = file_data[file_pos + 1] & 0xF;
				data = file_data[file_pos + 2];
				ay8910.command_handle(
					port,
					register,
					data,
					vgm.clock_ay8910,
					midi,
				)?;
				file_pos += 3;
			},
			RF5C68_REG =>
			// RF5C68 Register Write
			{
				file_pos += 3
			},
			SPCM_MEM => {
				//FIXME: wtf?
				// SegaPCM Memory Write
				wait = (file_data[file_pos + 2] as u32) << 8
					| file_data[file_pos + 1] as u32;
				data = file_data[file_pos + 3];
				segapcm.mem_write(
					wait.try_into().unwrap(),
					data,
					midi,
				)?;
				wait = 0;
				file_pos += 4;
			},
			RF5C68_MEM =>
			// RF5C68 Memory Write
			{
				file_pos += 4
			},
			DATABNK_SEEK => {
				data_bank_pos = file_data[file_pos + 1] as usize
					| (file_data[file_pos + 2] as usize) << 0x08
					| (file_data[file_pos + 3] as usize) << 0x10
					| (file_data[file_pos + 4] as usize) << 0x18;
				for block in 0x0..vgm.data_block_count {
					if vgm.data_block_pos[block as usize] == data_bank_pos {
						break;
					}
				}
				if temp_long >= vgm.data_block_count {
					vgm.data_block_pos[vgm.data_block_count as usize] =
						data_bank_pos;
					vgm.data_block_count += 0x01;
				}
				file_pos += 5;
			},
			0x30..=0x4E => file_pos += 2,
			0x55..=0x5F => file_pos += 3,
			0xA0..=0xBF => file_pos += 3,
			0xC0..=0xDF => file_pos += 4,
			0xE1..=0xFF => file_pos += 5,
			0x90 =>
			// DAC Stream Setup
			{
				file_pos += 5
			},
			0x91 =>
			// Set Stream Data
			{
				file_pos += 5
			},
			0x92 =>
			// Set Stream Frequency
			{
				file_pos += 6
			},
			0x93 => {
				// Start Stream
				data_bank_pos = file_data[file_pos + 2] as usize
					| (file_data[file_pos + 3] as usize) << 8
					| (file_data[file_pos + 4] as usize) << 16
					| (file_data[file_pos + 5] as usize) << 24;
				for data_block_pointer in 0x0..(vgm.data_block_count as usize)
				{
					if vgm.data_block_pos[data_block_pointer] == data_bank_pos {
						break;
					}
				}
				if temp_long >= vgm.data_block_count {
					vgm.data_block_pos[vgm.data_block_count as usize] =
						data_bank_pos;
					vgm.data_block_count += 0x1;
				}
				dac_wrt = true;
				file_pos += 11;
			},
			0x94 => {
				// Stop Stream
				if last_data_note < 0x80 {
					midi.note_off_write(0x0F.into(), last_data_note.into(), 0x00.into());
				}
				last_data_note = 0xFF;
				file_pos += 2
			},
			0x95 => {
				// Start Stream (fast call)
				let temp = file_data[file_pos + 2] & 0x7F;
				if last_data_note < 0x80 {
					midi.note_off_write(0x0F.into(), last_data_note.into(), 0x00.into());
				}
				midi.note_on_write(0x0F.into(), temp.into(), 0x7F.into());
				last_data_note = temp;
				file_pos += 5;
			},
			_ => {
				file_pos += 1;
				verbose!("Unrecognized command {} at {}", command, file_pos);
				//FIXME: unrecoverable? Stop
			},
		}
		midi.delta_time += wait;
		if loop_available {
			if stop_vgm {
				cur_loop += 0x01;
				if cur_loop < config.vgm_loops {
					midi.event_write(Meta(Marker(TEXT_LOOP_END)));
					stop_vgm = false;
					file_pos = loop_pos;
				}
			}
			//FIXME: If this is not aligned with an instruction, it won't loop.
			if file_pos == loop_pos {
				midi.event_write(Meta(Marker(TEXT_LOOP_START)));
				//Call SegaPCM_Dump
			}
		}
		if vgm.clock_sn76489 != 0 {
			for counter in 0..sn76489.state.note_delay.len() {
				sn76489.state.note_delay[counter] += wait;
			}
		}
		//if config.ym2612_dac_disabled = 1 { DACWrt = false
		//if vgm.clock_2612 {
		//for TempLng = 0x0..=wait - 1
		//	Put #2, , DAC_DATA_BYTE
		//}
		//}
		if dac_wrt {
			for counter in 0x0..vgm.data_block_count {
				if vgm.data_block_pos[counter as usize] == data_bank_pos - 1 {
					if last_data_note < 0x80 {
						midi.note_off_write(0x0F.into(), last_data_note.into(), 0x00.into());
					}
					midi.note_on_write(0x0F.into(), (counter as u8).into(), 0x7F.into());
					last_data_note = counter.try_into().unwrap(); //FIXME: This is something fucky. Check if VB allows overflow or not.
				}
			}
			dac_wrt = false
		}
		/* FIXME: if Timer >= DETimer {
			// Calling DoEvents too often causes huge slowdowns
			Call frmMain.tmrConversionStatus_Timer
			Conversion_Status_Current = file_pos - vhFileHeader.lngDataOffset
			DoEvents
			DETimer = Timer + 0.2
		}*/
		if file_pos >= vgm.file_header.eof_offset as usize || stop_vgm {
			break;
		}
	}
	if loop_available {
		midi.event_write(Meta(Marker(TEXT_LOOP_END)));
	}
	//Call SegaPCM_Dump
	//Call DAC_WriteClose
	if vgm.clock_sn76489 != 0 {
		port = if vgm.file_header.hz_sn76489 & 0x40000000 != 0 {
			0x1
		} else {
			0x0
		};
		for chip_number in 0x0..=port {
			for command in 0x0..=0x3 {
				data = (1 << 7) | (command << 5);
				sn76489.command_handle(
					data,
					chip_number,
					midi,
				)?;
				sn76489.command_handle(
					0x00,
					chip_number,
					midi,
				)?;
			}
		}
	}
	/*if vgm.clock_ym2413 != 0 && false {
		// TODO ...
		for _command in 0x0..=0x7 {
			register = 0x0;
			data = 0x0;
			ym2413.command_handle(
				register,
				data,
				vgm.clock_ym2413,
				midi,
			)?;
		}
	}
	if vgm.clock_ym2612 != 0 && false {
		// TODO ...
		for command in 0x0..=0x5 {
			port = command / 0x3;
			register = command % 0x3;
			data = 0x0;
			ym2612.command_handle(port, register, data, fsam_2612, midi)?;
		}
	}*/
	if vgm.clock_ym2151 != 0 {
		for command in 0x0..=0x7 {
			register = 0x8;
			data = command;
			ym2151.command_handle(register, data, midi)?;
		}
	}
	Ok(())
}

use midly::num::u14;
use midly::num::u7;
pub(crate) struct FactoredState {
	pub(crate) fnum_msb: [u8; 0x10],
	pub(crate) fnum_lsb: [u8; 0x10],
	pub(crate) fnum_1: [u32; 0x10],
	pub(crate) fnum_2: [u32; 0x10],
	pub(crate) hz_1: [f64; 0x10],
	pub(crate) hz_2: [f64; 0x10],
	pub(crate) note_1: [f64; 0x10],
	pub(crate) note_2: [f64; 0x10],
	pub(crate) midi_instrument: [u7; 0x10],
	pub(crate) midi_note: [u7; 0x10],
	pub(crate) midi_wheel: [u14; 0x10],
	pub(crate) midi_volume: [u7; 0x10],
	pub(crate) midi_pan: [u7; 0x10],
	pub(crate) midi_mod: [u7; 0x10],
	pub(crate) note_on_1: [bool; 0x10],
	pub(crate) note_on_2: [bool; 0x10],
}
impl Default for FactoredState {
	fn default() -> Self {
		FactoredState {
			fnum_msb: [0; 0x10],
			fnum_lsb: [0; 0x10],
			fnum_1: [0; 0x10],
			fnum_2: [0; 0x10],
			hz_1: [0.0; 0x10],
			hz_2: [0.0; 0x10],
			note_1: [0xFF.into(); 0x10],
			note_2: [0xFF.into(); 0x10],
			midi_instrument: [0xFF.into(); 0x10],
			midi_note: [0xFF.into(); 0x10],
			midi_wheel: [0x8000.into(); 0x10],
			midi_volume: [0xFF.into(); 0x10],
			midi_pan: [0.into(); 0x10],
			midi_mod: [0xFF.into(); 0x10],
			note_on_1: [false; 0x10],
			note_on_2: [false; 0x10],
		}
	}
}
pub(crate) fn shift<T: Copy>(buf: &mut T, new: T) -> T {
	let old = *buf;
	*buf = new;
	old
}
pub(crate) fn midi_from_note(note: f64) -> u7 {
	(note.round() as u8).into()
}
#[test]
fn test_hz_to_note() {
	assert!((hz_to_note(110.0) - 45.0).abs() < 0.01);
	assert!((hz_to_note(220.0) - 57.0).abs() < 0.01);
	assert!((hz_to_note(261.63) - 60.0).abs() < 0.01);
	assert!((hz_to_note(440.0) - 69.0).abs() < 0.01);
	assert!((hz_to_note(523.25) - 72.0).abs() < 0.01);
	assert!((hz_to_note(880.0) - 81.0).abs() < 0.01);
	assert!((hz_to_note(1760.0) - 93.0).abs() < 0.01);
}
//FIXME: Frequencies close to zero will not behave well.
pub(crate) fn hz_to_note(freq: f64) -> f64 {
	if freq == 0.into() {
		0xFF.into()
	} else {
		f64::log(freq / 440.0, f64::powf(2.0, 1.0 / 12.0)) + 69.0
	}
}

use crate::config::Config;
use crate::midi_shim::{
	db_to_midi_vol, MIDIShim, MIDI_PAN, MIDI_PAN_CENTER, MIDI_PAN_LEFT, MIDI_PAN_RIGHT,
	MIDI_VOLUME, MIDI_VOLUME_MAX,
};
use crate::strict;
use crate::utils::{hz_to_note, midi_from_note, shift, FactoredState};
use anyhow::{bail, Result};
use midly::num::u4;
pub(crate) const SN76489_LATCH: u8 = 0x80; //128
pub(crate) const SN76489_CHANNEL_SELECT: u8 = SN76489_LATCH | 0x70; //240
#[allow(clippy::identity_op)]
pub(crate) const SN76489_TONE_1: u8 = SN76489_LATCH | 0x00; //128
pub(crate) const SN76489_TONE_2: u8 = SN76489_LATCH | 0x20; //160
pub(crate) const SN76489_TONE_3: u8 = SN76489_LATCH | 0x40; //192
pub(crate) const SN76489_NOISE: u8 = SN76489_LATCH | 0x60; //224
const SN76489_CHN_MASK: u8 = 0x60; //112
#[allow(dead_code)]
const SN76489_FB_PERIODIC: u8 = 0x00;
#[allow(dead_code)]
const SN76489_FB_WHITE: u8 = 0x01;
#[allow(dead_code)]
const SN76489_CLOCK_SOURCE_HALF: u8 = 0x00;
#[allow(dead_code)]
const SN76489_CLOCK_SOURCE_FOURTH: u8 = 0x01;
#[allow(dead_code)]
const SN76489_CLOCK_SOURCE_EIGHTH: u8 = 0x02;
#[allow(dead_code)]
const SN76489_CLOCK_SOURCE_TONE_3: u8 = 0x03;
const SN76489_ATTENUATOR_1: u8 = SN76489_LATCH | 0x10; //144
const SN76489_ATTENUATOR_2: u8 = SN76489_LATCH | 0x30; //176
const SN76489_ATTENUATOR_3: u8 = SN76489_LATCH | 0x50; //208
const SN76489_ATTENUATOR_NOISE: u8 = SN76489_LATCH | 0x70; //240
#[allow(dead_code)]
const SN76489_ATTENUATION_MIN: u8 = 0x00;
const SN76489_ATTENUATION_MAX: u8 = 0x0F;
pub(crate) const MIDI_CHANNEL_SN76489_BASE: u4 = u4::new(0x0A);
// used for Note On/Off-Detection
//Public SN76489_LastVol(0x0..=0x7) As Byte
pub(crate) struct SN76489State {
	feedback: [u8; 2],
	clock_source: [u8; 2],
	attenuation_1: [u8; 8],
	attenuation_2: [u8; 8],
	gg_pan: [[u8; 5]; 2],
	chip_num: u8,
	pub note_delay: [u32; 8],
	factored: FactoredState,
	last_command: [u8; 2],
}
impl Default for SN76489State {
	fn default() -> Self {
		SN76489State {
			feedback: [0; 2],
			clock_source: [0; 2],
			attenuation_1: [0; 8],
			attenuation_2: [0xFF; 8],
			gg_pan: [[0, 0, 0, 0, 0xFF]; 2],
			chip_num: 0,
			note_delay: [0; 8],
			factored: Default::default(),
			last_command: [0x00; 2],
		}
	}
}
pub(crate) struct SN76489<'config> {
	pub(crate) state: SN76489State,
	t6w28: bool, //FIXME: dumb naming. This should probably be refactored into its own class anyway.
	clock: u32,
	config: &'config Config,
}
impl<'config> SN76489<'config> {
	//WARNING: This won't behave well if fnum is small.
	fn hz(fnum: u32, clock: u32) -> f64 {
		if fnum == 0 {
			0.0
		} else {
			(clock as f64 / 32.0) / fnum as f64
		}
	}
	pub(crate) fn new<'c: 'config>(
		t6w28: bool,
		clock: u32,
		config: &'c Config,
		opt_state: Option<SN76489State>,
	) -> Self {
		SN76489 {
			state: opt_state.unwrap_or_default(),
			t6w28,
			clock,
			config,
		}
	}
	pub(crate) fn command_handle(
		&mut self,
		data: u8,
		chip_num: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		self.state.chip_num = validate_chip_number(chip_num)?;
		if data & 0x80 != 0 {
			let ret = match data & SN76489_CHANNEL_SELECT {
				// skip and wait for data command
				SN76489_TONE_1 | SN76489_TONE_2 | SN76489_TONE_3 => Ok(()),
				_ => self.command_handle_internal(data, data, midi), // Volume Changes use LSB
			};
			self.state.last_command[self.state.chip_num as usize] = data;
			ret
		} else {
			// do data write
			self.command_handle_internal(
				self.state.last_command[self.state.chip_num as usize],
				data,
				midi,
			)
		}
	}
	fn command_handle_internal(&mut self, msb: u8, lsb: u8, midi: &mut MIDIShim) -> Result<()> {
		if msb & 0x80 == 0x0 {
			return Ok(());
		}
		let mut channel = (msb & SN76489_CHN_MASK) / 0x20;
		match channel.cmp(&0x03) {
			std::cmp::Ordering::Less
				if self.config.sn76489_ch_disabled[channel as usize] =>
			{
				return Ok(())
			},
			std::cmp::Ordering::Equal if self.config.sn76489_noise_disabled => {
				return Ok(())
			},
			std::cmp::Ordering::Greater => bail!("Invalid midi channel"),
			_ => (),
		}
		channel |= self.state.chip_num << 2;
		let mut midi_channel = MIDI_CHANNEL_SN76489_BASE + (channel & 0x03).into();
		if self.state.chip_num == 0x01 {
			midi_channel -= MIDI_CHANNEL_SN76489_BASE
		}
		match msb & SN76489_CHANNEL_SELECT {
			SN76489_TONE_1 | SN76489_TONE_2 | SN76489_TONE_3 => self
				.process_tone_channels(
					msb,
					lsb,
					channel.into(),
					midi_channel,
					midi,
				)?,
			SN76489_NOISE => self.process_noise_channel(lsb, channel.into()),
			SN76489_ATTENUATOR_1 | SN76489_ATTENUATOR_2 | SN76489_ATTENUATOR_3 => self
				.process_tone_attenuation(channel.into(), lsb, midi_channel, midi)?,
			SN76489_ATTENUATOR_NOISE => self.process_noise_attenuation(
				channel.into(),
				lsb,
				midi_channel,
				midi,
			),
			_ => strict!("Invalid channel selected"),
		}
		Ok(())
	}
	fn process_tone_channels(
		&mut self,
		msb: u8,
		lsb: u8,
		channel_ptr: usize,
		midi_channel: u4,
		midi: &mut MIDIShim,
	) -> Result<()> {
		self.state.factored.fnum_1[channel_ptr] = shift(
			&mut self.state.factored.fnum_2[channel_ptr],
			((lsb as u32 & 0x3F) << 4) + (msb & 0x0F) as u32,
		);
		self.state.factored.hz_1[channel_ptr] = shift(
			&mut self.state.factored.hz_2[channel_ptr],
			Self::hz(self.state.factored.fnum_2[channel_ptr], self.clock),
		);
		if !(self.t6w28 && self.state.chip_num > 0x00) {
			let mut temp_note = hz_to_note(self.state.factored.hz_2[channel_ptr]);
			if temp_note >= 0x80.into() {
				//TempNote = 0x7F - self.state.factored.fnum_2[channel]
				temp_note = 0xFF.into();
			}
			self.state.factored.note_1[channel_ptr] =
				shift(&mut self.state.factored.note_2[channel_ptr], temp_note);
			if self.t6w28 {
				self.state.factored.note_1[0x04 + channel_ptr] = shift(
					&mut self.state.factored.note_2[0x04 + channel_ptr],
					temp_note,
				);
			}
		} else {
			//if T6W28_SN76489 & SN76489_NUM > 0x0{
			let mut temp_note = hz_to_note(self.state.factored.hz_2[channel_ptr] / 2.0);
			if (channel_ptr & 0x03) == 0x02 {
				if temp_note < 0xFF.into() {
					temp_note -= 24.0; // - 2 Octaves
					if temp_note >= 0x80.into() {
						temp_note = 0x7F.into();
					}
				}
				self.state.factored.note_2[0x3] = temp_note;
				self.state.factored.note_2[0x7] = temp_note;
			}
			return Ok(());
		}
		if self.state.factored.fnum_2[channel_ptr] == 0 {
			if self.state.factored.note_on_2[channel_ptr] {
				if self.state.factored.midi_note[channel_ptr] < 0xFF {
					midi.do_note_on(
						self.state.factored.note_1[channel_ptr],
						self.state.factored.note_2[channel_ptr],
						midi_channel,
						&mut self.state.factored.midi_note[channel_ptr],
						&mut self.state.factored.midi_wheel[channel_ptr],
						None,
						None,
					)?;
					self.state.factored.midi_note[channel_ptr] = 0xFF.into(); // TODO: Check this
				}
				self.state.factored.note_on_1[channel_ptr] =
					shift(&mut self.state.factored.note_on_2[channel_ptr], false);
			}
		} else if self.state.factored.fnum_2[channel_ptr] > 0
			&& self.state.factored.fnum_2[channel_ptr]
				!= self.state.factored.fnum_1[channel_ptr]
		{
			let dn_ret = midi.do_note_on(
				self.state.factored.note_1[channel_ptr],
				self.state.factored.note_2[channel_ptr],
				midi_channel,
				&mut self.state.factored.midi_note[channel_ptr],
				&mut self.state.factored.midi_wheel[channel_ptr],
				Some(if self.state.factored.note_on_2[channel_ptr] {
					0x0
				} else {
					0xFF
				}),
				None,
			)?;
			self.state.factored.note_on_1[channel_ptr] =
				shift(&mut self.state.factored.note_on_2[channel_ptr], true);
			if dn_ret {
				self.state.note_delay[channel_ptr] = 0x0;
			}
		}
		if self.t6w28 {
			self.tone_t6w28(channel_ptr, midi_channel, midi)?;
		}
		if (channel_ptr & 0x3) == 0x2
			&& self.state.clock_source[self.state.chip_num as usize] == 0x3
		{
			self.command_handle_internal(
				0xE0,
				(self.state.feedback[self.state.chip_num as usize] << 2)
					| self.state.clock_source[self.state.chip_num as usize],
				midi,
			)?;
		}
		Ok(())
	}
	fn tone_t6w28(&mut self, mut channel: usize, mut midi_channel: u4, midi: &mut MIDIShim) -> Result<()> {
		channel += 0x04;
		midi_channel -= MIDI_CHANNEL_SN76489_BASE;
		if self.state.factored.note_2[channel] == 0xFF.into() {
			if self.state.factored.note_on_2[channel] {
				if self.state.factored.midi_note[channel] < 0xFF {
					midi.do_note_on(
						self.state.factored.note_1[channel],
						self.state.factored.note_2[channel],
						midi_channel,
						&mut self.state.factored.midi_note[channel],
						&mut self.state.factored.midi_wheel[channel],
						None,
						None,
					)?;
					//self.state.factored.midi_note[channel] = 0xFF
				}
				self.state.factored.note_on_1[channel] =
					shift(&mut self.state.factored.note_on_2[channel], false);
			}
		} else if self.state.factored.note_2[channel] < 0xFF.into()
			&& self.state.factored.note_2[channel]
				!= self.state.factored.note_1[channel]
		{
			let dn_ret = midi.do_note_on(
				self.state.factored.note_1[channel],
				self.state.factored.note_2[channel],
				midi_channel,
				&mut self.state.factored.midi_note[channel],
				&mut self.state.factored.midi_wheel[channel],
				Some(if self.state.factored.note_on_2[channel] {
					0x0
				} else {
					0xFF
				}),
				None,
			)?;
			self.state.factored.note_on_1[channel] =
				shift(&mut self.state.factored.note_on_2[channel], true);
			if dn_ret {
				self.state.note_delay[channel] = 0x00;
			}
		}
		Ok(())
	}
	fn process_tone_attenuation(
		&mut self,
		channel: usize,
		lsb: u8,
		midi_channel: u4,
		midi: &mut MIDIShim,
	) -> Result<()> {
		self.state.attenuation_1[channel] = shift(
			&mut self.state.attenuation_2[channel],
			lsb & SN76489_ATTENUATION_MAX,
		);
		if self.state.attenuation_2[channel] != self.state.attenuation_1[channel] {
			//self.state.attenuation_2[channel] = (LSB & SN76489_ATTENUATION_MAX) * 8.45 //(127 / 15)
			self.state.attenuation_2[channel] = lsb & SN76489_ATTENUATION_MAX; // I like round values
								   //self.state.factored.midi_volume[0] = MIDI_VOLUME_MAX + 1 - 0x8 - self.state.attenuation_2[channel] * 0x8
								   //if self.state.factored.midi_volume[0] > 0x7F{ self.state.factored.midi_volume[0] = 0x7F
			self.state.factored.midi_volume[0] =
				db_to_midi_vol(Self::vol_to_db(lsb & 0x0F));
			midi.controller_write(
				midi_channel,
				MIDI_VOLUME,
				self.state.factored.midi_volume[0],
			);
			// write Note On/Off
			if self.config.sn76489_voldep_notes >= 1
				&& self.state.factored.note_on_2[channel]
			{
				if self.state.factored.midi_volume[0] == 0 {
					midi.do_note_on(
						self.state.factored.note_1[channel],
						0xFF.into(),
						midi_channel,
						&mut self.state.factored.midi_note[channel],
						&mut self.state.factored.midi_wheel[channel],
						Some(0xFF),
						None,
					)?;
					self.state.note_delay[channel] = 44100;
				//} else if (SN76489_LastVol[channel] = 0x0 & SN76489_NoteDelay[channel] >= 10) | _
				//	(self.config.sn76489_voldep_notes >= 0x2 & SN76489_NoteDelay[channel] >= 735 &
				//	SN76489_LastVol[channel] + 20 < self.state.factored.midi_volume[0]) {
				} else if (self.state.attenuation_1[channel] == 0x0F
					&& self.state.note_delay[channel] >= 10) || (self
					.config
					.sn76489_voldep_notes
					>= 0x2
					&& self.state.note_delay[channel] >= 735
					&& self.state.attenuation_1[channel] - 2
						> self.state.attenuation_2[channel])
				{
					midi.do_note_on(
						self.state.factored.note_1[channel],
						self.state.factored.note_2[channel],
						midi_channel,
						&mut self.state.factored.midi_note[channel],
						&mut self.state.factored.midi_wheel[channel],
						Some(0xFF),
						None,
					)?;
					self.state.note_delay[channel] = 0;
				}
			}
			//SN76489_LastVol[channel] = self.state.factored.midi_volume[0]
		}
		Ok(())
	}
	fn process_noise_attenuation(
		&mut self,
		channel_ptr: usize,
		lsb: u8,
		midi_channel: u4,
		midi: &mut MIDIShim,
	) {
		self.state.attenuation_1[channel_ptr] = shift(
			&mut self.state.attenuation_2[channel_ptr],
			lsb & SN76489_ATTENUATION_MAX,
		);
		self.state.factored.midi_volume[0] = self.calculate_noise_volume(channel_ptr);
		if self.state.attenuation_2[channel_ptr]
			!= self.state.attenuation_1[channel_ptr]
			|| self.state.note_delay[channel_ptr] >= 735
		{
			if self.state.factored.midi_volume[0] > 0 {
				// old Note-Height: 39
				if self.state.factored.note_1[channel_ptr] < 0xFF.into() {
					midi.note_off_write(
						midi_channel,
						midi_from_note(
							self.state.factored.note_1
								[channel_ptr],
						),
						0x00.into(),
					);
				}
				if self.state.factored.note_2[channel_ptr] < 0xFF.into() {
					midi.note_on_write(
						midi_channel,
						midi_from_note(
							self.state.factored.note_2
								[channel_ptr],
						),
						self.state.factored.midi_volume[0],
					);
				}
				self.state.factored.note_1[channel_ptr] =
					self.state.factored.note_2[channel_ptr];
			} else if self.state.factored.midi_volume[0] == 0 && self.state.attenuation_1[channel_ptr] < 0x0F && self.state.factored.note_1[channel_ptr]
						< 0xFF.into() {
   					midi.note_off_write(
   						midi_channel,
   						midi_from_note(
   							self.state.factored.note_1
   								[channel_ptr],
   						),
   						0x00.into(),
   					);
   					self.state.factored.note_1[channel_ptr] = 0xFF.into();
   				}
			self.state.note_delay[channel_ptr] = 0x0;
		}
	}
	fn calculate_noise_volume(&mut self, channel_ptr: usize) -> midly::num::u7 {
		(MIDI_VOLUME_MAX.as_int() + 1 - 8 - (self.state.attenuation_2[channel_ptr] * 8)).into()
	}
	fn process_noise_channel(&mut self, lsb: u8, channel_ptr: usize) {
		self.state.feedback[self.state.chip_num as usize] = (lsb & 0x04) >> 2; //FIXME: Is this supposed to be a boolean?
		self.state.clock_source[self.state.chip_num as usize] = lsb & 0x03;
		// Noise-Frequency
		let noise_frequency = self.calculate_noise_frequency(channel_ptr);
		self.state.factored.fnum_1[channel_ptr] =
			shift(&mut self.state.factored.fnum_2[channel_ptr], noise_frequency);
		if self.state.factored.fnum_2[channel_ptr] == 0 {
			self.state.factored.fnum_2[channel_ptr] = 1
		}
		self.state.factored.hz_1[channel_ptr] = shift(
			&mut self.state.factored.hz_2[channel_ptr],
			Self::hz(self.state.factored.fnum_2[channel_ptr], self.clock),
		);
		//if T6W28_SN76489 & SN76489_NUM > 0x0{ return }
		self.state.factored.note_2[channel_ptr] =
			hz_to_note(self.state.factored.hz_2[channel_ptr]) / 1.5;
		//SN76489_NoteDelay[channel] = 0x0
	}
	fn calculate_noise_frequency(&mut self, channel: usize) -> u32 {
		if self.state.clock_source[self.state.chip_num as usize] == 0x3 {
			self.state.factored.fnum_2[channel - 1] << 1
		} else {
			1 << (5 + self.state.clock_source[self.state.chip_num as usize] as u32)
		}
	}
	pub(crate) fn gg_stereo_handle(
		&mut self,
		register: u8,
		chip_num: u8,
		midi: &mut MIDIShim,
	) -> Result<()> {
		self.state.chip_num = validate_chip_number(chip_num)?;
		let mut pan_val: u8;
		let mut channel: u4;
		for cur_bit in 0x0..=0x3 {
			let channel_mask = 1 << cur_bit;
			pan_val = 0x0;
			if (register & (channel_mask << 4)) != 0 {
				pan_val |= 0x01; // Left Channel On
			}
			if (register & channel_mask) != 0 {
				pan_val |= 0x02; // Right Channel On
			}
			if self.state.gg_pan[self.state.chip_num as usize][cur_bit as usize]
				!= pan_val || self.state.gg_pan[self.state.chip_num as usize][0x4]
				== register
			{
				//if CurBit = 0x0{
				//	CH = CHN_DAC
				//} else {
				channel = MIDI_CHANNEL_SN76489_BASE + cur_bit.into();
				if self.state.chip_num == 0x1 {
					channel -= MIDI_CHANNEL_SN76489_BASE
				}
				//}
				match pan_val {
					0x1 => midi.controller_write(
						channel,
						MIDI_PAN,
						MIDI_PAN_LEFT,
					),
					0x2 => midi.controller_write(
						channel,
						MIDI_PAN,
						MIDI_PAN_RIGHT,
					),
					0x3 => midi.controller_write(
						channel,
						MIDI_PAN,
						MIDI_PAN_CENTER,
					),
					_ => strict!("Illegal pan_val"),
				};
			}
			self.state.gg_pan[self.state.chip_num as usize][cur_bit as usize] = pan_val
		}
		self.state.gg_pan[self.state.chip_num as usize][0x4] = register;
		Ok(())
	}
	pub(crate) fn vol_to_db(tl: u8) -> f64 {
		if tl < 0x0F {
			-f64::from(tl) * 2.0
		} else {
			-400.0 // results in volume 0
		}
	}
}
fn validate_chip_number(num: u8) -> Result<u8> {
	match num {
		0x00 => Ok(0x00),
		0x01 => Ok(0x01),
		_ => bail!("Illegal chip number"),
	}
}