/*
 * Kneron Hardware Test: I2C
 *
 * Copyright (C) 2023 Kneron, Inc. All rights reserved.
 */

#include "cmsis_os2.h"
#include "kdrv_i2c.h"
#include "kdrv_pinmux.h"
#include "kmdw_console.h"

// OV5647 Sensor specific defines
#define OV5647_I2C_ADDR      0x36
#define OV5647_CHIP_ID_H_REG 0x300A
#define OV5647_CHIP_ID_L_REG 0x300B
#define OV5647_SYS_CTRL0_REG 0x0100
#define OV5647_SW_RESET_REG  0x0103

// VEYE-MIPI-IMX462 Sensor specific defines
#define VEYE_IMX462_I2C_ADDR 0x3B

int hw_test_i2c_run(void)
{
    kdrv_status_t status;
    uint8_t found_devices = 0;
    int result = -1;

    kmdw_printf("Starting I2C Bus Scan Test...\n");

    // 1. Configure Pinmux for I2C0
    kmdw_printf("Configuring pins for I2C0...\n");
    kdrv_pinmux_config(KDRV_PIN_I2C0_SCL, PIN_MODE_0, PIN_PULL_NONE, PIN_DRIVING_8MA);
    kdrv_pinmux_config(KDRV_PIN_I2C0_SDA, PIN_MODE_0, PIN_PULL_NONE, PIN_DRIVING_8MA);

    // 2. Initialize I2C0
    kmdw_printf("Initializing I2C0 at 100kHz...\n");
    status = kdrv_i2c_initialize(KDRV_I2C_CTRL_0, KDRV_I2C_SPEED_100K);
    if (status != KDRV_STATUS_OK) {
        kmdw_printf("I2C initialization failed. Status: %d\n", status);
        return -1;
    }

    // kmdw_printf("=========================================================\n");
    // kmdw_printf("Scanning I2C bus (addresses 0x01 to 0x7F)...\n");
    // kmdw_printf("=========================================================\n");

    // for (uint16_t addr = 1; addr < 128; addr++) {
    //     // 3. Try to write a single null byte to probe the address.
    //     // The kdrv_i2c_write_register function is a bit complex for a simple probe.
    //     // A simpler `kdrv_i2c_master_write` or `kdrv_i2c_probe` would be ideal.
    //     // Let's try to use kdrv_i2c_write_register with a dummy write.
    //     // We send 0 bytes of data, but the address is sent on the bus.
    //     status = kdrv_i2c_write_register(KDRV_I2C_CTRL_0, addr, 0, 0, 0, NULL);

    //     if (status == KDRV_STATUS_OK) {
    //         kmdw_printf("Found I2C device at address: 0x%02X\n", addr);
    //         found_devices++;
    //     }

    //     // A short delay between probes
    //     osDelay(10);
    // }

    // kmdw_printf("=========================================================\n");
    // kmdw_printf("I2C Bus Scan Summary:\n");
    // kmdw_printf("  Found %d device(s).\n", found_devices);

    kmdw_printf("=========================================================\n");
    kmdw_printf("Checking for OV5647 sensor at address 0x%02X...\n", OV5647_I2C_ADDR);

    uint8_t chip_id_h, chip_id_l;
    uint16_t chip_id;

    // Read Chip ID High Byte
    status = kdrv_i2c_read_register(KDRV_I2C_CTRL_0, OV5647_I2C_ADDR, OV5647_CHIP_ID_H_REG, 2, 1, &chip_id_h);
    if (status != KDRV_STATUS_OK) {
        kmdw_printf("Failed to communicate with device at 0x%02X. It might not be an OV5647 or is not connected.\n", OV5647_I2C_ADDR);
    } else {
        // Read Chip ID Low Byte
        kdrv_i2c_read_register(KDRV_I2C_CTRL_0, OV5647_I2C_ADDR, OV5647_CHIP_ID_L_REG, 2, 1, &chip_id_l);
        chip_id = ((uint16_t)chip_id_h << 8) | chip_id_l;
        kmdw_printf("  Read Chip ID: 0x%04X\n", chip_id);

        if (chip_id == 0x5647) {
            kmdw_printf("  OV5647 sensor detected successfully!\n");

            result = 0; // Passed
            // --- Additional Status Checks ---
            // 1. Check System Status (Standby/Streaming)
            uint8_t sys_status;
            status = kdrv_i2c_read_register(KDRV_I2C_CTRL_0, OV5647_I2C_ADDR, OV5647_SYS_CTRL0_REG, 2, 1, &sys_status);
            if (status == KDRV_STATUS_OK) {
                kmdw_printf("  System Status (0x0100): 0x%02X -> %s\n", sys_status, (sys_status == 0x01) ? "Streaming" : "Standby");
            } else {
                kmdw_printf("  Failed to read system status register (0x0100).\n");
            }

            // 2. Perform Software Reset and re-check ID
            kmdw_printf("  Performing software reset...\n");
            uint8_t reset_val = 0x01;
            status = kdrv_i2c_write_register(KDRV_I2C_CTRL_0, OV5647_I2C_ADDR, OV5647_SW_RESET_REG, 2, 1, &reset_val);
            if (status != KDRV_STATUS_OK) {
                kmdw_printf("  Failed to write software reset command.\n");
            } else {
                osDelay(100); // Wait for the sensor to reset
                status = kdrv_i2c_read_register(KDRV_I2C_CTRL_0, OV5647_I2C_ADDR, OV5647_CHIP_ID_H_REG, 2, 1, &chip_id_h);
                if (status == KDRV_STATUS_OK) {
                    kmdw_printf("  Sensor responded after reset. -> PASSED\n");
                } else {
                    kmdw_printf("  Sensor did NOT respond after reset. -> FAILED\n");
                }
            }

        } else {
            kmdw_printf("  Device at 0x%02X is not an OV5647 (unexpected chip ID).\n", OV5647_I2C_ADDR);
        }
    }

//    kmdw_printf("---------------------------------------------------------\n");
//    kmdw_printf("Checking for VEYE-MIPI-IMX462 sensor at address 0x%02X...\n", VEYE_IMX462_I2C_ADDR);

//    // Probe for VEYE-MIPI-IMX462
//    // We use a 0-length write to check if the device ACKs the address.
//    status = kdrv_i2c_write_register(KDRV_I2C_CTRL_0, VEYE_IMX462_I2C_ADDR, 0, 0, 0, NULL);

//    if (status == KDRV_STATUS_OK) {
//        kmdw_printf("  VEYE-MIPI-IMX462 detected at 0x%02X! -> PASSED\n", VEYE_IMX462_I2C_ADDR);
//        result = 0; // Passed if at least one supported sensor is found
//    } else {
//        kmdw_printf("  No response at 0x%02X. (Not connected or different address)\n", VEYE_IMX462_I2C_ADDR);
//    }

    kmdw_printf("=========================================================\n");
    kmdw_printf("Test finished. Returning to main menu...\n");

    // Uninitialize I2C controller
    kdrv_i2c_uninitialize(KDRV_I2C_CTRL_0);

    return result;
}
