/*
 * @file lsense.cpp
 * @brief Light sensor implementation
 *
 * 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];

/**
 * @brief Light sensor data structure
 */
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];

// Presence counters
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);
static void read_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr, light_sensor_data_t *sensor_data);
static uint8_t detect_light_sensor(SoftwareI2C &i2c_bus, uint8_t sensor_addr);
static void initialize_i2c_bus(int bus_number, bool pin_order_inverted);
static void detect_sensors_on_bus_generic(int bus_number);

/**
 * @brief Configure a light sensor for measurement
 * @param i2c_bus Reference to I2C bus instance
 * @param sensor_addr LSB of sensor address (0 or 1)
 */
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();
}

/**
 * @brief Read data from a light sensor
 * @param i2c_bus Reference to I2C bus instance
 * @param sensor_addr LSB of sensor address (0 or 1)
 * @param sensor_data Pointer to data structure to populate
 */
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); // Start from red register
	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();
}

/**
 * @brief Detect if a light sensor is present
 * @param i2c_bus Reference to I2C bus instance
 * @param sensor_addr LSB of sensor address (0 or 1)
 * @return Manufacturer ID (0xE0 if valid sensor detected, 0 otherwise)
 */
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); // Manufacturer ID register
	i2c_bus.endTransmission();

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

	return response;
}

/**
 * @brief Initialize an I2C bus and configure sensors
 * @param bus_number Bus number (0, 1, or 2)
 * @param pin_order_inverted Whether to invert SDA/SCL pin order
 */
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:
			// Invalid bus number
			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);
}

/**
 * @brief Generic function to detect sensors on any I2C bus
 * @param bus_number Bus number (0, 1, or 2)
 */
static void detect_sensors_on_bus_generic(int bus_number)
{
	if (bus_number < 0 || bus_number >= NUM_BUSES)
		return;

	g_sensors_detected_per_bus[bus_number] = 0;

	// Try normal pin order first
	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 no sensors found, try inverted pin order
	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]++;
		}
	}
}

/*******************************************************************************/
/*******************************************************************************/
/*                             Command handlers                                */
/*******************************************************************************/
/*******************************************************************************/

void lsense_cmd_presence(void)
{
	g_sensors_detected_total = 0;

	detect_sensors_on_bus_generic(BUS_X);
	detect_sensors_on_bus_generic(BUS_Y);
	detect_sensors_on_bus_generic(BUS_Z);

	Serial.write(g_sensors_detected_total);
	Serial.write(g_sensors_detected_per_bus[BUS_X]);
	Serial.write(g_sensor_addresses[BUS_X][SENSOR_NEG]);
	Serial.write(g_sensor_addresses[BUS_X][SENSOR_POS]);
	Serial.write(g_sensors_detected_per_bus[BUS_Y]);
	Serial.write(g_sensor_addresses[BUS_Y][SENSOR_NEG]);
	Serial.write(g_sensor_addresses[BUS_Y][SENSOR_POS]);
	Serial.write(g_sensors_detected_per_bus[BUS_Z]);
	Serial.write(g_sensor_addresses[BUS_Z][SENSOR_NEG]);
	Serial.write(g_sensor_addresses[BUS_Z][SENSOR_POS]);

	Serial.flush();
	SERIAL_BUFFER_CLEAR();
}

void lsense_cmd_read(void)
{
	detect_sensors_on_bus_generic(BUS_X);
	read_light_sensor(g_i2c_buses[BUS_X], 0, &g_sensor_data[BUS_X][SENSOR_NEG]);
	read_light_sensor(g_i2c_buses[BUS_X], 1, &g_sensor_data[BUS_X][SENSOR_POS]);

	detect_sensors_on_bus_generic(BUS_Y);
	read_light_sensor(g_i2c_buses[BUS_Y], 0, &g_sensor_data[BUS_Y][SENSOR_NEG]);
	read_light_sensor(g_i2c_buses[BUS_Y], 1, &g_sensor_data[BUS_Y][SENSOR_POS]);

	detect_sensors_on_bus_generic(BUS_Z);
	read_light_sensor(g_i2c_buses[BUS_Z], 0, &g_sensor_data[BUS_Z][SENSOR_NEG]);
	read_light_sensor(g_i2c_buses[BUS_Z], 1, &g_sensor_data[BUS_Z][SENSOR_POS]);

	Serial.write(g_sensor_data[BUS_X][SENSOR_NEG].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_X][SENSOR_NEG].data.blue >> 8);
	Serial.write(g_sensor_data[BUS_X][SENSOR_POS].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_X][SENSOR_POS].data.blue >> 8);
	Serial.write(g_sensor_data[BUS_Y][SENSOR_NEG].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_Y][SENSOR_NEG].data.blue >> 8);
	Serial.write(g_sensor_data[BUS_Y][SENSOR_POS].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_Y][SENSOR_POS].data.blue >> 8);
	Serial.write(g_sensor_data[BUS_Z][SENSOR_NEG].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_Z][SENSOR_NEG].data.blue >> 8);
	Serial.write(g_sensor_data[BUS_Z][SENSOR_POS].data.blue & 0xFF);
	Serial.write(g_sensor_data[BUS_Z][SENSOR_POS].data.blue >> 8);

	Serial.flush();
	SERIAL_BUFFER_CLEAR();
}