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

static uint32_t ts_init = 0;
static uint32_t ts_last = 0;
struct {
	float amplitude = 1;
	uint32_t us_per_tick = 1;
} config;
struct {
	uint8_t index;
	uint16_t frequency;
	float amplitude;
} next;

constexpr uint64_t bm(int n) {
	return ((uint64_t)1 << n) - 1;
}

void scheduler::do_next() {
	
	int i = 1;

	if(running) {
		tones::set(next.index, next.frequency, next.amplitude);
	} else {
		eeprom::jump(0);
		tones::clear_all();
		ts_init = micros();
		running = true;
	}

	uint64_t v = 0;
	uint8_t buff[8];
	eeprom::read(buff, 1);
	
	entry::Type type = buff[0] >> 5;
	int entry_size = entry::get_size(type);
	eeprom::read(buff + 1, entry_size - 1);


	Serial.print("LOAD (");

	for(int i = 0; i < entry_size; i++) {
		v = (v << 8) | buff[i];
		Serial.print(buff[i]);
		Serial.print(", ");
	}


	Serial.print(") ");

	switch(type) {
		case entry::Type::config: {
			config.amplitude = *(float*)(buff + 4);
			v >>= 32;
			config.us_per_tick = v & bm(29);
			ts_init = micros();

			Serial.print("config\t");
			Serial.print(config.amplitude);
			Serial.print('\t');
			Serial.println(config.us_per_tick);
			break;
		}
		case entry::Type::set: {
			Serial.println("set");
			v >>= 4;
			next.frequency = v & bm(12);
			v >>= 12;
			next.index = v & bm(5);
			next.amplitude = config.amplitude;
			break;
		}
		case entry::Type::set_ts: {
			Serial.println("set_ts");
			next.frequency = v & bm(12);
			v >>= 12;
			uint32_t ticks = v & bm(20);
			v >>= 20;
			next.index = v & bm(5);
			next.amplitude = config.amplitude;
			timestamp = ticks * config.us_per_tick + ts_init;
			break;
		}
		case entry::Type::clear: {
			Serial.println("clear");
			next.index = v & bm(5);
			next.frequency = 0;
			next.amplitude = 0;
			break;
		}
		case entry::Type::clear_ts: {
			Serial.println("clear_ts");
			v >>= 4;
			uint32_t ticks = v & bm(20);
			v >>= 20;
			next.index = v & bm(5);
			next.frequency = 0;
			next.amplitude = 0;
			timestamp = ticks * config.us_per_tick + ts_init;
			break;
		}
		default: {
			running = false;
			tones::clear_all();
			Serial.println("stop");
			break;
		}
	}

	Serial.println("DONE");
}

void scheduler::clear() {
	eeprom::jump(0);
	ts_last = 0;
}

void scheduler::add_set(uint32_t ts, uint8_t index, uint16_t freq) {
//	if(index >= size(tones::all)) {
//		return;
//	}
//	if(ts <= ts_last) {
//		entry::Set e {.type=entry::Type::SET, .index=index, .frequency=freq};
//		eeprom::write(&e, sizeof(e));
//	} else {
//		entry::SetTS e {.type=entry::Type::SET_TS, .index=index, .ticks=ts, .frequency=freq};
//		eeprom::write(&e, sizeof(e));
//		ts_last = ts;
//	}
}

void scheduler::add_clear(uint32_t ts, uint8_t index) {
//	if(index >= size(tones::all)) {
//		return;
//	}
//	if(ts <= ts_last) {
//		entry::Clear e {.type=entry::Type::CLEAR, .index=index};
//		eeprom::write(&e, sizeof(e));
//	} else {
//		entry::ClearTS e {.type=entry::Type::CLEAR_TS, .index=index, .ticks=ts};
//		eeprom::write(&e, sizeof(e));
//		ts_last = ts;
//	}
}

void scheduler::add_config(float amplitude, uint32_t us_per_tick) {
//	entry::Config e {.type=entry::Type::CONFIG, .p=0, .us_per_tick=us_per_tick, .amplitude=amplitude};
//	eeprom::write(&e, sizeof(e));
}

void scheduler::finalize() {
//	entry::Header e {.type=entry::Type::STOP};
//	eeprom::write(&e, sizeof(e));
}