#include "multiplexer.h"
#include "ti/driverlib/dl_i2c.h"
#include "ti_msp_dl_config.h"
#include <stdio.h>
#include <stdint.h>

/*
Multiplexer TCA9548A:
Address:0x70
The TCA9548A is example of a single-register device, which is controlled via I2C commands. Since it has 1 bit to enable or disable a channel, there is only 1 register
needed, and the controller merely writes the register data after the target address, skipping the register number.
ADC connected to channel 0
Both SDA and SL lines of the multiplexer is connected to VIN through pull-up resistors
The following is the general procedure for a controller to access a target device:
1. If a controller wants to send data to a target:
    • Controller-transmitter sends a START condition and addresses the target-receiver.
    • Controller-transmitter sends data to target-receiver.
    • Controller-transmitter terminates the transfer with a STOP condition.
2. If a controller wants to receive or read data from a target:
    • Controller-receiver sends a START condition and addresses the target-transmitter.
    • Controller-receiver sends the requested register to read to target-transmitter.
    • Controller-receiver receives data from the target-transmitter.
    • Controller-receiver terminates the transfer with a STOP condition.
The TCA9548A is example of a single-register device, which is controlled via I2C commands. Since it has 1 bit to enable or disable a channel, 
there is only 1 register needed, and the controller merely writes the register data after the target address, skipping the register number.
*/

/*
Scans all the addresses of the peripherals:
*/
void I2C_scanBus() {
    printf("Scanning I2C Bus...\n");

    // **Step 1: Reset I2C Controller if Busy**
    if (DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        printf("I2C Bus Busy! Resetting I2C Controller...\n");
        DL_I2C_disableController(I2C_controller_INST);  // Disable I2C
        delay_cycles(20000);
        DL_I2C_enableController(I2C_controller_INST);   // Re-enable I2C
        delay_cycles(20000);
    }

    // **Step 2: Scan I2C Bus**
    for (uint8_t addr = 0x08; addr < 0x78; addr++) {  // Valid I2C Address Range
        DL_I2C_startControllerTransfer(I2C_controller_INST, addr, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
        delay_cycles(5000);
        
        if (!(DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_ERROR)) {
            printf("Device found at: 0x%02X\n", addr);
        }else {
            // Clear the error by resetting the I2C controller
            DL_I2C_disableController(I2C_controller_INST);
            DL_I2C_enableController(I2C_controller_INST);
        }
    }

    printf("I2C Scan Complete!\n");
}

/*Function for Multiplexer*/

void Multiplexer_SelectChannel(uint8_t channel)
{
    if (channel > 7) {
        printf("ERROR: Invalid Multiplexer Channel! Must be 0-7.\n");
        return;
    }
    uint8_t data = (channel);
    printf("Selecting Multiplexer I2C Channel: 0x%02X\n", data);

    // Ensure bus is idle before starting communication
    while (DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);

    // SEND Command to Multiplexer
    DL_I2C_startControllerTransfer(I2C_controller_INST, MULTIPLEXER_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_controller_INST, &data, 1);

    // **Ensure STOP condition is sent**
    while (DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);

    // **Slightly Increase Delay for Multiplexer to Process Command**
    delay_cycles(30000); 

    // Verify Multiplexer Response:
    //uint8_t response= 0x00;
    DL_I2C_startControllerTransfer(I2C_controller_INST, MULTIPLEXER_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, 1);

    // Wait for a response from the multiplexer
    while (DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
    //uint8_t response = DL_I2C_receiveControllerData(I2C_controller_INST);

    // **Debug: Print Expected vs. Received Response**
    //printf("Multiplexer Response: 0x%02X (Expected: 0x%02X)\n", response, data);
    
    
    // **CHECK FOR ADDRESS ACKNOWLEDGMENT**
    //(if (!(DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_ERROR)) {
    //    printf("Multiplexer detected at 0x70.\n");
    //    return;
    //} else{
    //    printf("ERROR: Multiplexer (0x70) detected.\n");
    //}

    // Wait for transaction completion
    //while (DL_I2C_getControllerStatus(I2C_controller_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS);

    //if(response != data){
    //    printf("ERROR: Multiplexer did not set the correct channel!\n");
        // **Retry Mechanism: Attempt to Set the Channel Again**
    //    delay_cycles(10000);
    //    Multiplexer_SelectChannel(channel);
    //} else {
    //    printf("Multiplexer Active Channel: 0x%X\n", data);
    //    return;
    //}
}

void Reset_I2C_Bus() {
    printf("I2C Bus is stuck! Resetting...\n");
    
    // Disable I2C Controller
    DL_I2C_disableController(I2C_controller_INST);
    delay_cycles(50000);

    // Re-enable I2C Controller
    DL_I2C_enableController(I2C_controller_INST);
    delay_cycles(50000);

    // Check if bus is free now
    uint32_t status = DL_I2C_getControllerStatus(I2C_controller_INST);
    printf("I2C Bus Status After Reset: 0x%08X\n", status);
}