#include "util.hpp"
#include "tones.hpp"
#include "tone.hpp"

using tones::all;
using tones::active;

void tones::init() {
	for(uint8_t i = 0; i < size(all); i++) {
		lookup[i] = i;
		rlookup[i] = i;
	}
}

void tones::recalc() {
	int j = 0;
	for(int i = 0; i < size(all); i++) {
		if(all[j].active()) {
			j++;
			continue;
		}
		if(!all[i].active()) {
			continue;
		}
		if(i != j) {
			swap(all[i], all[j]);
			swap(rlookup[i], rlookup[j]);
			swap(lookup[rlookup[i]], lookup[rlookup[j]]);
		}
		j++;
	}
	active = j;
}

void tones::clear_all() {
	for(int i = 0; i < size(all); i++) {
		all[i] = {0, 0, 0};
	}
	active = 0;
}

void tones::set(uint8_t index, uint16_t frequency, float amplitude) {
	if(index >= size(all)) {
		return;
	}
	Tone& t = all[lookup[index]];
	if(amplitude == 0) {
		t = {0, 0, 0};
	} else {
		t.frequency = frequency;
		t.amplitude = amplitude;
	}
}