/*
 * @file lsense.cpp
 * @brief Light sensor implementation: software I2C, detection, and channel
 *        readout for the six TCS-style light sensors on three buses.
 *
 * Created: 21.09.2025
 * Author: ThePetrovich
 *
 * Copyright YKSA - Sakha Aerospace Systems, LLC.
 * See the LICENSE file for details.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <Arduino.h>
#include <SoftwareI2C.h>
#include <stdint.h>

#include "config.h"
#include "iodefs.h"
#include "lsense.h"

static SoftwareI2C g_i2c_buses[NUM_BUSES];

typedef union __attribute__((packed)) {
	struct __attribute__((packed)) {
		uint16_t red;
		uint16_t green;
		uint16_t blue;
		uint16_t reserved;
		uint16_t ir;
		uint16_t green2;
	} data;
	uint8_t bytes[LSENSE_DATA_SIZE];
} light_sensor_data_t;

static light_sensor_data_t g_sensor_data[NUM_BUSES][SENSORS_PER_BUS];
static uint8_t g_sensor_addresses[NUM_BUSES][SENSORS_PER_BUS];

static uint8_t g_sensors_detected_total = 0;
static uint8_t g_sensors_detected_per_bus[NUM_BUSES];

static void configure_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr)
{
	uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;

	i2c_bus.beginTransmission(addr);
	i2c_bus.write(0x41);
	i2c_bus.endTransmission();

	i2c_bus.beginTransmission(addr);
	i2c_bus.write(0x41);
	i2c_bus.write(0b00101101); /* IR gain x1, RGB gain x1, 35ms mode */
	i2c_bus.write(0b00010000); /* RGB_EN = 1, measurement active */
	i2c_bus.endTransmission();
}

static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_sensor_data_t *sensor_data)
{
	if (sensor_data == nullptr)
		return;

	uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;

	i2c_bus.beginTransmission(addr);
	i2c_bus.write(0x50);
	i2c_bus.endTransmission();

	i2c_bus.requestFrom(addr, (uint8_t)LSENSE_DATA_SIZE);
	for (uint8_t i = 0; i < LSENSE_DATA_SIZE; i++) {
		sensor_data->bytes[i] = i2c_bus.read();
	}
	i2c_bus.endTransmission();
}

static uint8_t detect_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr)
{
	uint8_t response = 0;
	uint8_t addr = LSENSE_I2C_BASE_ADDR | sensor_addr;

	i2c_bus.beginTransmission(addr);
	i2c_bus.write(0x92);
	i2c_bus.endTransmission();

	i2c_bus.requestFrom(addr, (uint8_t)1);
	if (i2c_bus.available()) {
		response = i2c_bus.read();
		if (response == 0xFF) {
			response = 0;
		}
	}
	return response;
}

static void initialize_i2c_bus(int bus_number, bool pin_order_inverted)
{
	uint8_t sda_pin, scl_pin;

	switch (bus_number) {
		case BUS_X:
			sda_pin = pin_order_inverted ? SCL1 : SDA1;
			scl_pin = pin_order_inverted ? SDA1 : SCL1;
			break;
		case BUS_Y:
			sda_pin = pin_order_inverted ? SCL0 : SDA0;
			scl_pin = pin_order_inverted ? SDA0 : SCL0;
			break;
		case BUS_Z:
			sda_pin = pin_order_inverted ? SCL2 : SDA2;
			scl_pin = pin_order_inverted ? SDA2 : SCL2;
			break;
		default:
			return;
	}

	SoftwareI2C *i2c_bus = &g_i2c_buses[bus_number];
	i2c_bus->begin(sda_pin, scl_pin);
	configure_light_sensor(*i2c_bus, 0);
	configure_light_sensor(*i2c_bus, 1);
}

static void detect_sensors_on_bus(int bus_number)
{
	if (bus_number < 0 || bus_number >= NUM_BUSES)
		return;

	g_sensors_detected_per_bus[bus_number] = 0;

	initialize_i2c_bus(bus_number, false);
	g_sensor_addresses[bus_number][SENSOR_NEG] = detect_light_sensor(g_i2c_buses[bus_number], 0);
	if (g_sensor_addresses[bus_number][SENSOR_NEG] == LSENSE_EXPECTED_ID) {
		g_sensors_detected_total++;
		g_sensors_detected_per_bus[bus_number]++;
	}
	g_sensor_addresses[bus_number][SENSOR_POS] = detect_light_sensor(g_i2c_buses[bus_number], 1);
	if (g_sensor_addresses[bus_number][SENSOR_POS] == LSENSE_EXPECTED_ID) {
		g_sensors_detected_total++;
		g_sensors_detected_per_bus[bus_number]++;
	}

	if (g_sensors_detected_per_bus[bus_number] == 0) {
		initialize_i2c_bus(bus_number, true);
		g_sensor_addresses[bus_number][SENSOR_NEG] = detect_light_sensor(g_i2c_buses[bus_number], 0);
		if (g_sensor_addresses[bus_number][SENSOR_NEG] == LSENSE_EXPECTED_ID) {
			g_sensors_detected_total++;
			g_sensors_detected_per_bus[bus_number]++;
		}
		g_sensor_addresses[bus_number][SENSOR_POS] = detect_light_sensor(g_i2c_buses[bus_number], 1);
		if (g_sensor_addresses[bus_number][SENSOR_POS] == LSENSE_EXPECTED_ID) {
			g_sensors_detected_total++;
			g_sensors_detected_per_bus[bus_number]++;
		}
	}
}

void lsense_detect_all(void)
{
	g_sensors_detected_total = 0;
	detect_sensors_on_bus(BUS_X);
	detect_sensors_on_bus(BUS_Y);
	detect_sensors_on_bus(BUS_Z);
}

void lsense_get_presence(lsense_presence_t *out)
{
	if (out == nullptr)
		return;
	out->total_detected = g_sensors_detected_total;
	out->bus_x_count = g_sensors_detected_per_bus[BUS_X];
	out->bus_x_neg_addr = g_sensor_addresses[BUS_X][SENSOR_NEG];
	out->bus_x_pos_addr = g_sensor_addresses[BUS_X][SENSOR_POS];
	out->bus_y_count = g_sensors_detected_per_bus[BUS_Y];
	out->bus_y_neg_addr = g_sensor_addresses[BUS_Y][SENSOR_NEG];
	out->bus_y_pos_addr = g_sensor_addresses[BUS_Y][SENSOR_POS];
	out->bus_z_count = g_sensors_detected_per_bus[BUS_Z];
	out->bus_z_neg_addr = g_sensor_addresses[BUS_Z][SENSOR_NEG];
	out->bus_z_pos_addr = g_sensor_addresses[BUS_Z][SENSOR_POS];
}

static uint16_t extract_channel(const light_sensor_data_t *d, uint8_t channel)
{
	switch (channel) {
		case 0:
			return d->data.red;
		case 1:
			return d->data.green;
		case 2:
			return d->data.blue;
		case 3:
			return d->data.ir;
		case 4:
			return d->data.green2;
		case 5: /* visible: R+G+B+G2 */
			return (uint16_t)((uint32_t)d->data.red + d->data.green + d->data.blue + d->data.green2);
		case 6: /* all: R+G+B+G2+IR */
			return (uint16_t)((uint32_t)d->data.red + d->data.green + d->data.blue + d->data.green2 +
					  d->data.ir);
		default:
			return 0;
	}
}

void lsense_read_channel(uint8_t channel, lsense_channel_values_t *out)
{
	if (out == nullptr)
		return;

	for (int bus = 0; bus < NUM_BUSES; bus++) {
		read_light_sensor(g_i2c_buses[bus], 0, &g_sensor_data[bus][SENSOR_NEG]);
		read_light_sensor(g_i2c_buses[bus], 1, &g_sensor_data[bus][SENSOR_POS]);
	}

	out->values[0] = extract_channel(&g_sensor_data[BUS_X][SENSOR_NEG], channel);
	out->values[1] = extract_channel(&g_sensor_data[BUS_X][SENSOR_POS], channel);
	out->values[2] = extract_channel(&g_sensor_data[BUS_Y][SENSOR_NEG], channel);
	out->values[3] = extract_channel(&g_sensor_data[BUS_Y][SENSOR_POS], channel);
	out->values[4] = extract_channel(&g_sensor_data[BUS_Z][SENSOR_NEG], channel);
	out->values[5] = extract_channel(&g_sensor_data[BUS_Z][SENSOR_POS], channel);
}