#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <string.h>

#include <ir_recv.h>
#include <ir.h>
#include <lasertag_utils.h>

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include "ir_recv.h"
#include "lasertag_utils.h"

LOG_MODULE_REGISTER(ir_recv, CONFIG_IR_RECV_LOG_LEVEL);

#define ADC_CHANNELS 4
#define SAMPLES_PER_BUFFER CONFIG_IR_RECV_SAMPLES_PER_BUFFER
#define BUFFER_COUNT CONFIG_IR_RECV_BUFFER_COUNT

static int16_t adc_buffers[BUFFER_COUNT][SAMPLES_PER_BUFFER * ADC_CHANNELS];
static uint8_t write_idx = 0, read_idx = 0;
static struct k_sem adc_sem;

/* --- Enhanced Simulator --- */
#ifdef CONFIG_IR_RECV_SIMULATOR
#include <zephyr/random/random.h>

#define SIM_MAX_SAMPLES 1000
static bool sim_buffer[SIM_MAX_SAMPLES];
static uint32_t sim_total_samples = 0;
static bool sim_trigger = false;
static uint32_t sim_sample_pos = 0;

/* Hilfsfunktion für den Jitter: Liefert einen Zufallswert zwischen -max und +max */
static int get_jitter(uint8_t max_jitter)
{
	if (max_jitter == 0)
		return 0;
	return (int)(sys_rand32_get() % (max_jitter * 2 + 1)) - max_jitter;
}

void ir_recv_sim_send_packet(ir_packet_t *packet, ir_sim_error_t *err)
{
	/* Blockieren, bis vorheriges Paket abgearbeitet ist */
	while (sim_trigger)
	{
		k_msleep(5);
	}

	ir_packet_t pkt;
	memcpy(&pkt, packet, sizeof(ir_packet_t));
	pkt.data.fields.crc = lastertag_crc8(pkt.data.bytes, 2);

	ir_sim_error_t default_err = {0};
	if (err == NULL)
	{
		err = &default_err;
	}

	sim_total_samples = 0;

	/* 1. Header Mark */
	int m_len = 32 + get_jitter(err->jitter_mark);
	for (int i = 0; i < m_len && sim_total_samples < SIM_MAX_SAMPLES; i++)
	{
		sim_buffer[sim_total_samples++] = 1;
	}

	/* 2. Header Gap */
	int g_len = 8 + get_jitter(err->jitter_space_0);
	for (int i = 0; i < g_len && sim_total_samples < SIM_MAX_SAMPLES; i++)
	{
		sim_buffer[sim_total_samples++] = 0;
	}

	/* 3. Payload Bits (24 Bit) */
	for (int i = 0; i < 24; i++)
	{
		bool bit = (pkt.data.bytes[i / 8] >> (i % 8)) & 1;

		/* Space Phase */
		int s_base = bit ? 16 : 8;
		int s_jit = bit ? get_jitter(err->jitter_space_1) : get_jitter(err->jitter_space_0);
		int s_len = s_base + s_jit;
		for (int j = 0; j < s_len && sim_total_samples < SIM_MAX_SAMPLES; j++)
		{
			sim_buffer[sim_total_samples++] = 0;
		}

		/* Mark Phase */
		int bm_len = 8 + get_jitter(err->jitter_mark);
		for (int j = 0; j < bm_len && sim_total_samples < SIM_MAX_SAMPLES; j++)
		{
			sim_buffer[sim_total_samples++] = 1;
		}
	}

	/* 4. Rauschen injizieren (Bit Flips) */
	if (err->noise_flips_per_8 > 0)
	{
		for (uint32_t i = 0; i < sim_total_samples; i += 8)
		{
			for (int f = 0; f < err->noise_flips_per_8; f++)
			{
				uint32_t flip_idx = i + (sys_rand32_get() % 8);
				if (flip_idx < sim_total_samples)
				{
					sim_buffer[flip_idx] = !sim_buffer[flip_idx]; /* Bit kippen */
				}
			}
		}
	}

	sim_sample_pos = 0;
	sim_trigger = true;

	LOG_DBG("Simulator: Queued (Type: %u, CRC: 0x%02X), Total Samples: %u",
			pkt.data.fields.type, pkt.data.fields.crc, sim_total_samples);
}

void ir_recv_sim_thread(void *p1, void *p2, void *p3)
{
	while (1)
	{
		k_usleep(75 * SAMPLES_PER_BUFFER);
		if (!sim_trigger)
			continue;

		int16_t *buf = adc_buffers[write_idx];
		for (int i = 0; i < SAMPLES_PER_BUFFER; i++)
		{
			bool level = 0;
			if (sim_sample_pos < sim_total_samples)
			{
				level = sim_buffer[sim_sample_pos];
			}
			sim_sample_pos++; /* Unbedingtes Inkrement */

			bool active = IS_ENABLED(CONFIG_IR_RECV_INVERT_SIGNAL) ? !level : level;
			buf[i * ADC_CHANNELS] = active ? 0x0FFF : 0x0000;
			buf[i * ADC_CHANNELS + 3] = 2400; // VBat Dummy
		}
		write_idx = (write_idx + 1) % BUFFER_COUNT;
		k_sem_give(&adc_sem);

		/* Nach dem Puffer noch eine kurze Pause einhalten, um das Paket abzuschließen */
		if (sim_sample_pos >= sim_total_samples + 50)
		{
			sim_trigger = false;
			LOG_DBG("Simulation sequence finished.");
		}
	}
}
K_THREAD_DEFINE(sim_tid, 1024, ir_recv_sim_thread, NULL, NULL, NULL, 5, 0, 0);
#endif

typedef enum
{
	IR_STATE_IDLE,
	IR_STATE_HEADER_SYNC,
	IR_STATE_WAIT_SPACE,
	IR_STATE_FIND_MARK,
	IR_STATE_SYNC_MARK,
	IR_STATE_VALIDATE
} ir_state_t;

typedef struct
{
	ir_state_t state;
	uint32_t sample_window;
	uint32_t bit_acc;
	uint16_t timer;
	uint8_t bit_count;
	uint8_t max_energy;
	uint16_t timer_at_max;
	uint8_t sync_window;
} ir_ctx_t;

static ir_ctx_t channels[3];

static void process_ir_sample(ir_ctx_t *ctx, int16_t raw)
{
	bool active = (raw > 2048);
	if (IS_ENABLED(CONFIG_IR_RECV_INVERT_SIGNAL))
	{
		active = !active;
	}

	/* 32-Bit Shift-Register aktualisieren */
	ctx->sample_window = (ctx->sample_window << 1) | (active ? 1 : 0);

	/* Energie über 32 Samples (Header) und 8 Samples (Bits) berechnen */
	uint8_t energy_32 = __builtin_popcount(ctx->sample_window);
	uint8_t energy_8 = __builtin_popcount(ctx->sample_window & 0xFF);

	switch (ctx->state)
	{
	case IR_STATE_IDLE:
		if (energy_32 >= 24)
		{
			ctx->state = IR_STATE_HEADER_SYNC;
			ctx->timer = 0;
		}
		break;

	case IR_STATE_HEADER_SYNC:
		ctx->timer++;
		/* Entspannt: 3 von 8 Samples dürfen verrauscht sein, es gilt trotzdem als Gap */
		if (energy_8 <= 3)
		{
			ctx->state = IR_STATE_FIND_MARK;
			ctx->timer = 0;
			ctx->bit_count = 0;
			ctx->bit_acc = 0;
		}
		else if (ctx->timer > 50)
		{
			ctx->state = IR_STATE_IDLE; /* Timeout */
		}
		break;

	case IR_STATE_FIND_MARK:
		ctx->timer++;
		/* Trigger bei 4 bleibt. Ab 4 Einsen fangen wir an, das Maximum zu suchen */
		if (energy_8 >= 4)
		{
			ctx->state = IR_STATE_SYNC_MARK;
			ctx->max_energy = energy_8;
			ctx->timer_at_max = ctx->timer;
			ctx->sync_window = 0;
		}
		else if (ctx->timer > 40)
		{
			ctx->state = IR_STATE_IDLE;
		}
		break;

	case IR_STATE_SYNC_MARK:
		ctx->timer++;
		ctx->sync_window++;

		if (energy_8 > ctx->max_energy)
		{
			ctx->max_energy = energy_8;
			ctx->timer_at_max = ctx->timer;
		}

		if (ctx->sync_window >= 10)
		{
			/* Entspannt: Wenn der Peak mindestens 5 Einsen hat, ist es ein validierter Puls */
			if (ctx->max_energy >= 5)
			{
				bool bit = (ctx->timer_at_max >= 20);

				ctx->bit_acc = (ctx->bit_acc >> 1) | (bit ? (1 << 23) : 0);

				LOG_DBG("Bit %u: %d (T_Max:%u, E_Max:%u)",
						ctx->bit_count, bit, ctx->timer_at_max, ctx->max_energy);

				if (++ctx->bit_count >= 24)
				{
					ctx->state = IR_STATE_VALIDATE;
				}
				else
				{
					ctx->state = IR_STATE_WAIT_SPACE;

					/* BITWEISER RESYNC: Ja, das funktioniert exakt wie gewünscht.
					 * Der Timer wird so weit zurückgeschraubt, als ob er GENAU am
					 * Peak (timer_at_max) auf 0 gesetzt worden wäre. */
					ctx->timer = ctx->timer - ctx->timer_at_max;
				}
			}
			else
			{
				ctx->state = IR_STATE_IDLE;
				LOG_DBG("Mark sync failed. Max Energy: %u", ctx->max_energy);
			}
		}
		break;

	case IR_STATE_WAIT_SPACE:
		ctx->timer++;

		/* Entspannt: Auch hier lassen wir bis zu 3 Stör-Samples im Space zu */
		if (energy_8 <= 3)
		{
			ctx->state = IR_STATE_FIND_MARK;
		}
		else if (ctx->timer > 30)
		{
			ctx->state = IR_STATE_IDLE;
			LOG_DBG("Wait space timeout.");
		}
		break;

	case IR_STATE_VALIDATE:
	{
		ir_packet_t p;
		p.data.bytes[0] = ctx->bit_acc & 0xFF;
		p.data.bytes[1] = (ctx->bit_acc >> 8) & 0xFF;
		p.data.bytes[2] = (ctx->bit_acc >> 16) & 0xFF;

		if (lastertag_crc8(p.data.bytes, 2) == p.data.fields.crc)
		{
			LOG_DBG(FORMAT_BLUE_BOLD("VALID: Type %u, ID %u, Val %u"),
					p.data.fields.type,
					p.data.fields.id,
					p.data.fields.value);
		}
		else
		{
			LOG_WRN("CRC Error! Acc: 0x%06X", ctx->bit_acc);
		}
		ctx->state = IR_STATE_IDLE;
	}
	break;
	}
}
/**
 * @brief Main processing thread for incoming ADC buffers.
 */
void ir_recv_thread(void *arg1, void *arg2, void *arg3)
{
	while (1)
	{
		k_sem_take(&adc_sem, K_FOREVER);
		while (read_idx != write_idx)
		{
			int16_t *buf = adc_buffers[read_idx];
			for (int i = 0; i < SAMPLES_PER_BUFFER; i++)
			{
				/* Now this call matches the static function name above */
				process_ir_sample(&channels[0], buf[i * ADC_CHANNELS + 0]);
				process_ir_sample(&channels[1], buf[i * ADC_CHANNELS + 1]);
				process_ir_sample(&channels[2], buf[i * ADC_CHANNELS + 2]);
			}
			read_idx = (read_idx + 1) % BUFFER_COUNT;
		}
	}
}

K_THREAD_DEFINE(ir_recv_tid, 2048, ir_recv_thread, NULL, NULL, NULL, 2, 0, 0);

/**
 * @brief Initialization of the IR receiver module.
 */
int ir_recv_init(void)
{
	k_sem_init(&adc_sem, 0, BUFFER_COUNT);

	for (int i = 0; i < 3; i++)
	{
		channels[i].state = IR_STATE_IDLE;
	}
	LOG_DBG("IR Receiver initialized. Mode: %s",
			IS_ENABLED(CONFIG_IR_RECV_SIMULATOR) ? "Simulator" : "Hardware");

	return 0;
}