#include "audioc.hpp"
#include "scope.hpp"
#include "osclsk.hpp"



#define _HUUUGE_FLOAT		(std::numeric_limits<float>::infinity())

int
osclsk_scope::init(uint8_t *dma_buf, size_t dma_sz)
{
	AC_PRECOND(dma_buf != NULL && dma_sz > 0);

	if (tft.init(dma_buf, dma_sz) != 0)
		return (-1);


	fg = tft_color(0, 0xff, 0);
	bg = tft_color(0, 0, 0);

	if (init_block() != 0)
		return (-1);
	if (init_screen() != 0)
		return (-1);

	AC_ASSERT(st == osc_state::RENDER || st == osc_state::RENDER_DONE);

	return (0);
}

#define OSCLSK_WAIT_READY_INTERVAL	100
int
osclsk_scope::wait_ready(int ms)
{
	int time;

	switch (state()) {
	case osclsk_scope::osc_state::INIT:
		AC_LOG("scope.state == INIT");
		break;
	case osclsk_scope::osc_state::TRIGGER:
		AC_LOG("scope.state == TRIGGER");
		break;
	case osclsk_scope::osc_state::SAMPLE:
		AC_LOG("scope.state == SAMPLE");
		break;
	case osclsk_scope::osc_state::SAMPLE_DONE:
		AC_LOG("scope.state == SAMPLE_DONE");
		break;
	case osclsk_scope::osc_state::RENDER:
		AC_LOG("scope.state == RENDER");
		break;
	case osclsk_scope::osc_state::RENDER_DONE:
		AC_LOG("scope.state == RENDER_DONE");
		break;
	case osclsk_scope::osc_state::READY:
		AC_LOG("scope.state == READY");
		break;
	default:
		AC_LOG("scope.state invalid");
		break;
	}

	time = 0;
	while (true) {
		if (prep() == 0 || time > ms)
			break;
		System::Delay(OSCLSK_WAIT_READY_INTERVAL);
		time += OSCLSK_WAIT_READY_INTERVAL;
	}

	if (st != osc_state::READY)
		return (-1);

	return (0);
}

int
osclsk_scope::prep(void)
{
	switch (st) {
	case osc_state::READY:
		return (0);
	case osc_state::RENDER_DONE:
		st = osc_state::READY;

		return (0);
	default:
		return (-1);
	}
}

int
osclsk_scope::trig(void)
{
	switch (st) {
	case osc_state::READY:
		trig_num = 0;
		trig_state = osc_trig_state::READY;
		st = osc_state::TRIGGER;

		return (0);
	default:
		return (-1);
	}
}

int
osclsk_scope::init_block(void)
{
	block_fill = 0;
	block_fill_nsamp = 0;

	st = osc_state::SAMPLE;

	for (block_fill = 0; block_fill < OSCLSK_BLOCK_LEN; block_fill++) {
		block_y_max[block_fill] = 0.f;
		block_y_min[block_fill] = 0.f;
	}

	st = osc_state::SAMPLE_DONE;

	AUDIOC_ASSERT(block_fill == OSCLSK_BLOCK_LEN);

	return (0);
}

int
osclsk_scope::init_screen(void)
{
	return (render_block());
}

void
osclsk_scope::sample(const float *sig, size_t num)
{
	size_t i;

	switch (st) {
	case osc_state::SAMPLE:
		for (i = 0; i < num && block_fill < OSCLSK_BLOCK_LEN; i++) {
			if (block_fill_nsamp == 0) {
				block_y_max[block_fill] = -_HUUUGE_FLOAT;
				block_y_min[block_fill] = _HUUUGE_FLOAT;
			}

			block_y_max[block_fill] = std::max(
			    block_y_max[block_fill], sig[i]);
			block_y_min[block_fill] = std::min(
			    block_y_min[block_fill], sig[i]);

			/* block_fill_nsamp increments and wraps.
			 * wrapping increments block_fill */
			block_fill_nsamp++;
			if (block_fill_nsamp >= OSCLSK_RATE_DIV) {
				block_fill++;
				block_fill_nsamp = 0;
			}
		}

		if (block_fill >= OSCLSK_BLOCK_LEN) {
			st = osc_state::SAMPLE_DONE;

			/* use the leftover samples to sample the trig lookback
			 * data buffer. we reset the nsamp for the current
			 * lookback buffer index to get a clean slate */
			trig_lkb_idx_nsamp = 0;
			trig_lkb_num = 0;
			sample_trigger(&sig[i], num - i);
		}
		return;
	case osc_state::TRIGGER:
	case osc_state::SAMPLE_DONE:
	case osc_state::RENDER:
	case osc_state::RENDER_DONE:
	case osc_state::READY:
		sample_trigger(sig, num);
		return;
	default:
		break;
	}
}

bool
osclsk_scope::is_triggering(float sample)
{
	switch (trig_mode) {
	case osc_trig_mode::RISING_EDGE:
		return (sample > trig_margin);
	case osc_trig_mode::FALLING_EDGE:
		return (sample < -trig_margin);
	case osc_trig_mode::ABS_THRESHOLD:
		return (std::abs(sample) > trig_margin);
	case osc_trig_mode::INSTANT:
		return (true);
	default:
		return (false);
	}
}

void
osclsk_scope::trig_lkb_wr_block(void)
{
	size_t lkb_start, n;
	size_t lkb_cpy, lkb_cpy_part;

	if (trig_mode == osc_trig_mode::INSTANT)
		lkb_cpy = trig_lkb_num;
	else
		lkb_cpy = std::min(trig_lkb_num, trig_lkb_amount);

	lkb_start = trig_lkb_idx >= lkb_cpy ?  trig_lkb_idx - lkb_cpy :
	    OSCLSK_TRIG_LOOKBACK + trig_lkb_idx - lkb_cpy;

	lkb_cpy_part = OSCLSK_TRIG_LOOKBACK - lkb_start >= lkb_cpy ? lkb_cpy :
	    OSCLSK_TRIG_LOOKBACK - lkb_start;

	block_fill = 0;

	std::copy(trig_lkb_y_max + lkb_start,
	    trig_lkb_y_max + lkb_start + lkb_cpy_part, block_y_max);
	std::copy(trig_lkb_y_min + lkb_start,
	    trig_lkb_y_min + lkb_start + lkb_cpy_part, block_y_min);

	block_fill += lkb_cpy_part;

	lkb_cpy -= lkb_cpy_part;
	if (lkb_cpy > 0) {
		std::copy(trig_lkb_y_max, trig_lkb_y_max + lkb_cpy,
		    block_y_max + lkb_cpy_part);
		std::copy(trig_lkb_y_min, trig_lkb_y_min + lkb_cpy,
		    block_y_min + lkb_cpy_part);
	}

	block_fill += lkb_cpy;

	/* we wanna resume sampling from right after the lookback data,
	 * so we write partial data points to block as well */
	block_y_max[block_fill] = trig_lkb_y_max[trig_lkb_idx];
	block_y_min[block_fill] = trig_lkb_y_min[trig_lkb_idx];
	block_fill_nsamp = trig_lkb_idx_nsamp;
}


void
osclsk_scope::sample_trigger(const float *sig, size_t num)
{
	size_t i;

	AC_PRECOND(st != osc_state::INIT && st != osc_state::SAMPLE);

	/* sample data in lookback buffers */
	for (i = 0; i < num; i++) {
		AC_ASSERT(trig_lkb_idx < OSCLSK_TRIG_LOOKBACK);

		if (trig_lkb_idx_nsamp == 0) {
			trig_lkb_y_max[trig_lkb_idx] = -_HUUUGE_FLOAT;
			trig_lkb_y_min[trig_lkb_idx] = _HUUUGE_FLOAT;
		}

		trig_lkb_y_max[trig_lkb_idx] = std::max(
		    trig_lkb_y_max[trig_lkb_idx], sig[i]);
		trig_lkb_y_min[trig_lkb_idx] = std::min(
		    trig_lkb_y_min[trig_lkb_idx], sig[i]);

		trig_lkb_idx_nsamp++;
		if (trig_lkb_idx_nsamp >= OSCLSK_RATE_DIV) {
			trig_lkb_idx++;
			trig_lkb_idx = trig_lkb_idx &
			    (OSCLSK_TRIG_LOOKBACK - 1);
			trig_lkb_idx_nsamp = 0;
			trig_lkb_num = std::min(trig_lkb_num + 1,
			    OSCLSK_TRIG_LOOKBACK);
		}
	}


	/* only do triggering checks in TRIGGER state and if out lookback buffer
	 * is full enough */
	if (st != osc_state::TRIGGER ||
	    (trig_mode != osc_trig_mode::INSTANT &&
	    trig_lkb_num < trig_lkb_amount)) {
		return;
	}

	/* trigger needs to be in NO_TRIG before TRIG state for e.g.
	 * RISING_EDGE trigger mode */
	for (i = 0; i < num; i++) {
		switch (trig_state) {
		case osc_trig_state::READY:
			switch (trig_mode) {
			case osc_trig_mode::RISING_EDGE:
			case osc_trig_mode::FALLING_EDGE:
			case osc_trig_mode::ABS_THRESHOLD:
				if (! is_triggering(sig[i])) {
					trig_state = osc_trig_state::NO_TRIG;
				}
				break;
			case osc_trig_mode::INSTANT:
				trig_state = osc_trig_state::TRIGGERED;
				break;
			}
			break;
		case osc_trig_state::NO_TRIG:
			if (is_triggering(sig[i])) {
				trig_num++;
				trig_state = osc_trig_state::TRIG;
			}
			break;
		case osc_trig_state::TRIG:
			if (is_triggering(sig[i])) {
				trig_num++;
			} else {
				trig_state = osc_trig_state::NO_TRIG;
				trig_num = 0;
			}
			break;
		default:
			break;
		}

		if (trig_num >= trig_num_req) {
			trig_state =
			    osc_trig_state::TRIGGERED;
		}

		if (trig_state == osc_trig_state::TRIGGERED)
			break;
	}

	/* transfer lookback to actual block if triggered */
	switch (trig_state) {
	case osc_trig_state::TRIGGERED:
		trig_lkb_wr_block();
		st = osc_state::SAMPLE;
		break;
	default:
		break;
	}
}


osclsk_scope::osc_state
osclsk_scope::state(void)
{
	return (st);
}

#define OSCLSK_Y_MAX_PX		(OSCLSK_SCREEN_YSZ - 1)

void
osclsk_scope::render_finish_tft_cb(void *ctx)
{
	osclsk_scope *osc;

	osc = (osclsk_scope *)ctx;

	if (osc->st != osc_state::RENDER)
		return;

	osc->st = osc_state::RENDER_DONE;
}

int
osclsk_scope::render_block(void)
{
	int res;
	static int async_id;
	float block_norm;

	res = 0;
	switch (st) {
	case osc_state::SAMPLE_DONE:
		block_norm = 1.f;
		for (render_idx = 0; render_idx < OSCLSK_BLOCK_LEN;
		    render_idx++) {
			block_norm = std::max(
			    std::abs(block_y_max[render_idx]),
			    block_norm
			    );
			block_norm = std::max(
			    std::abs(block_y_min[render_idx]),
			    block_norm
			    );
		}
		for (render_idx = 0; render_idx < OSCLSK_BLOCK_LEN;
		    render_idx++) {
			block_y_max[render_idx] =
			    block_y_max[render_idx] / block_norm;
			block_y_min[render_idx] =
			    block_y_min[render_idx] / block_norm;
		}
		for (render_idx = 0; render_idx < OSCLSK_BLOCK_LEN;
		    render_idx++) {
			screen_px_y_max[render_idx] = (uint16_t)(
			    OSCLSK_Y_MAX_PX/2 * (1 - block_y_max[render_idx])
			    );
			screen_px_y_min[render_idx] = 1u + (uint16_t)(
			    OSCLSK_Y_MAX_PX/2 * (1 - block_y_min[render_idx])
			    );
		}

		tft.async_on_finish(render_finish_tft_cb, this);

		res = tft.async_start();
		AC_ASSERT(res >= 0);
		if (res < 0)
			return (-1);

		async_id = res;
		render_idx = 0;
		st = osc_state::RENDER;

		__attribute__((fallthrough));
	case osc_state::RENDER:
		for (; render_idx < OSCLSK_BLOCK_LEN; render_idx++) {
			res = tft.async_hline_row_bg_fill(async_id, &bg, &fg,
			    screen_px_y_max[render_idx], // inverted
			    screen_px_y_min[render_idx], // inverted
			    render_idx);
			AC_ASSERT(res >= 0);
			if (res < 0)
				return (-1);
			if (res > 0)
				return (1);
		}
		tft.async_end();
		break;
	default:
		break;
	}

	return (0);
}

void
osclsk_scope::set_color(osc_cc which, const tft_color *what)
{
	switch (which) {
	case osc_cc::FOREGROUND:
		fg = *what;
		break;
	case osc_cc::BACKGROUND:
		bg = *what;
		break;
	default:
		break;
	}
}