#include <Arduino.h>
#include "timing.hpp"
#include "scheduler.hpp"
#include "data.hpp"
#include "entry.hpp"
#include "tones.hpp"
#include "util.hpp"
#include "buttons.hpp"

using namespace scheduler;

static uint32_t ts_init = 0;
static uint32_t ts_last = 0;
static uint32_t tick_offset = 0;

struct {
	uint8_t amplitude = 1;
	uint32_t us_per_tick = 1;
} config;
struct {
	entry::NextType type = entry::nt_none;
	union {
		entry::Tone tone;
		entry::Tempo tempo;
	};
} next;

void scheduler::reset() {
	tones::clear_all();
	ts_init = timestamp = micros();
	tick_offset = 0;
}

static void do_next() {
	
	int i = 1;

	if(state == State::play) {
		switch(next.type) {
		case entry::nt_tone:
			tones::set(next.tone.index, next.tone.frequency, next.tone.amplitude);
			break;
		case entry::nt_tempo:
			ts_init += next.tempo.ticks * config.us_per_tick;
			tick_offset = next.tempo.ticks;
			config.us_per_tick = next.tempo.us_per_tick;
			break;
		}
	}
	next.type = entry::nt_none;

	uint64_t v = 0;
	uint8_t buff[8];

	if(data::read(buff, 1) != 1) {
		return;
	}
	
	entry::Type type = buff[0] >> 5;
	int entry_size = entry::get_size(type);
	
	if(data::read(buff + 1, entry_size - 1) != entry_size - 1) {
		return;
	}

	for(int i = 0; i < entry_size; i++) {
		v = (v << 8) | buff[i];
	}

	switch(type) {
		case entry::Type::config: {
			buttons::set_addr_next((v & bm(24)) + data::get_addr());
			v >>= 24;
			config.amplitude = v & bm(8);
			v >>= 8;
			config.us_per_tick = v & bm(29);
			state = State::play;
			scheduler::reset();
			break;
		}
		case entry::Type::tempo: {
			next.type = entry::nt_tempo;
			v >>= 4;
			next.tempo.ticks = (v & bm(20)) - tick_offset;
			v >>= 20;
			next.tempo.us_per_tick = v & bm(29);
			timestamp = next.tempo.ticks * config.us_per_tick + ts_init;
			break;
		}
		case entry::Type::set: {
			v >>= 4;
			next.type = entry::nt_tone;
			next.tone.frequency = v & bm(12);
			v >>= 12;
			next.tone.index = v & bm(5);
			next.tone.amplitude = config.amplitude;
			break;
		}
		case entry::Type::set_ts: {
			next.type = entry::nt_tone;
			next.tone.frequency = v & bm(12);
			v >>= 12;
			uint32_t ticks = (v & bm(20)) - tick_offset;
			v >>= 20;
			next.tone.index = v & bm(5);
			next.tone.amplitude = config.amplitude;
			timestamp = ticks * config.us_per_tick + ts_init;
			break;
		}
		case entry::Type::clear: {
			next.type = entry::nt_tone;
			next.tone.index = v & bm(5);
			next.tone.frequency = 0;
			next.tone.amplitude = 0;
			break;
		}
		case entry::Type::clear_ts: {
			v >>= 4;
			uint32_t ticks = (v & bm(20)) - tick_offset;
			v >>= 20;
			next.type = entry::nt_tone;
			next.tone.index = v & bm(5);
			next.tone.frequency = 0;
			next.tone.amplitude = 0;
			timestamp = ticks * config.us_per_tick + ts_init;
			break;
		}
		default: {
			state = State::end;
			tones::clear_all();
			break;
		}
	}
}

void scheduler::do_all() {
	do {
		do_next();
	} while(should_update());
	tones::recalc();
}