#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.
*/

/*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 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;
    //}
}