/*
SDA - P1.2
SCL - P1.3

RS - P2.0
RW - P2.1
E  - P2.2

D0 - P3.0
D1 - P3.1
D2 - P3.2
D3 - P5.3 P3.3 is broken
D4 - P3.4
D5 - P3.5
D6 - P3.6
D7 - P3.7
*/

#include <msp430.h>
#include <stdint.h>
#include <stdio.h>

#define LIS3DH_ADDR     0x19       // I2C address for the LIS3DH (use 0x18 if SDO is low)
#define WHO_AM_I_REG    0x0F       // WHO_AM_I register address (should return 0x33)
#define CTRL_REG1       0x20       // Control register 1; writing here enables the sensor
#define CTRL_REG4       0x23
#define LIS3DH_OUT_X_L                0x28
#define LIS3DH_TEMP_CFG_REG           0x1F

int pos = 0;
int line = 0;

void delay_ms(unsigned int ms)
{
    while(ms--)
        __delay_cycles(1000);  // 1ms delay if MCLK is 1MHz
}

void send_command_lcd(unsigned char data){
    send_raw_lcd(data, 0x0);
}

void send_data_lcd(unsigned char data){
    send_raw_lcd(data, BIT0);
    pos++;
}

void send_sentence_lcd(char str[]){
    unsigned int j = 0;
    while (str[j] != '\0'){
        send_data_lcd(str[j]);
        delay_ms(1);
        if (pos == 16){
            pos = 0;
            if (line == 0){
                send_command_lcd(0xC0);
                line = 1;
            }
            else{
                send_command_lcd(0x01);
                line = 0;
            }

        }
        j++;
    }
}

void set_payload_lcd(unsigned char data){
    P3OUT = data;
    data = data & BIT3;
    P5OUT |= data;
    data = P3OUT;
}

void send_raw_lcd(unsigned char data, unsigned char mask){

    P2OUT = 0;
    P2OUT |= mask;
    P2OUT &= ~BIT1;

    delay_ms(1);

    set_payload_lcd(data);

    delay_ms(1);

    P2OUT |= BIT2;

    delay_ms(1);

    P2OUT &= ~BIT2;

    delay_ms(1);

    P5OUT &= ~BIT3;
    P3OUT = 0;
    P2OUT = 0;

    delay_ms(1);
}

void lcd_init(){
    P5DIR |= BIT3;
    P3DIR |= 0x0FF;
    P2DIR |= 0x07;

    send_command_lcd(0x38); // 8 bit mode

    send_command_lcd(0x01); // clear screen

    send_command_lcd(0x0C); // turn screen on

    send_command_lcd(0x06); // set entry mode
}

void i2c_init(void){

    UCB0CTLW0 |= UCSWRST;

    UCB0CTLW0 |= UCMST | UCMODE_3 | UCSYNC;

    UCB0CTLW0 |= UCSSEL__SMCLK;

    UCB0BRW = 10;

    UCB0I2CSA = LIS3DH_ADDR;

    P1SEL1 &= ~(BIT2 | BIT3);  // Select primary module function
    P1SEL0 |=  (BIT2 | BIT3);  // for P1.2 (SDA) and P1.3 (SCL)

    UCB0CTLW0 &= ~UCSWRST;  // Release I2C module for operation

}

uint8_t read_i2c(uint8_t reg){ // unsigned integer, 8 bits
    uint8_t data;

    while (UCB0STATW & UCBBUSY); // wait until not busy

    // Set to Transmit mode and send a START condition with the register address
    UCB0CTLW0 |= UCTR | UCTXSTT;           // UCTR=TX mode, send START
    while (!(UCB0IFG & UCTXIFG0));          // Wait for TX buffer to be ready
    UCB0TXBUF = reg;                      // Send register address
    while (!(UCB0IFG & UCTXIFG0));          // Wait for transmission to complete

    // Switch to Receiver mode to read the data
    UCB0CTLW0 &= ~UCTR;                   // Clear UCTR (switch to RX mode)
    UCB0CTLW0 |= UCTXSTT;                 // Send repeated START for reading
    while (UCB0CTLW0 & UCTXSTT);          // Wait until the repeated START is sent

    // Send STOP condition; the STOP will be sent after the first byte is received
    UCB0CTLW0 |= UCTXSTP;
    while (!(UCB0IFG & UCRXIFG0));         // Wait for a byte to be received
    data = UCB0RXBUF;                     // Read the received byte
    while (UCB0CTLW0 & UCTXSTP);          // Wait until the STOP condition is sent

    return data;
}

// Read multiple registers in a single transaction (using auto-increment)
// For the LIS3DH, set the MSB of the register address to enable auto-increment
void readx_i2c(uint8_t startReg, uint8_t *buffer, uint8_t count)
{
    uint8_t regAddr = startReg | 0x80; // Set auto-increment bit (bit 7)

    while (UCB0STATW & UCBBUSY);       // Wait until the I2C bus is free

    // Transmit phase: send register address with auto-increment enabled
    UCB0CTLW0 |= UCTR | UCTXSTT;        // Set TX mode, send START condition
    while (!(UCB0IFG & UCTXIFG0));      // Wait until TX buffer is ready
    UCB0TXBUF = regAddr;              // Transmit register address
    while (!(UCB0IFG & UCTXIFG0));      // Wait until address transmitted

    // Switch to RX mode to read the data
    UCB0CTLW0 &= ~UCTR;               // Clear UCTR to switch to receiver mode
    UCB0CTLW0 |= UCTXSTT;             // Send repeated START for reading
    while (UCB0CTLW0 & UCTXSTT);      // Wait until repeated START is sent

    // Read the desired number of bytes
    int i;
    for (i = 0; i < count; i++) {
        if (i == (count - 1)) {        // For the last byte, set the STOP condition
            UCB0CTLW0 |= UCTXSTP;
        }
        while (!(UCB0IFG & UCRXIFG0)); // Wait until a byte is received
        buffer[i] = UCB0RXBUF;         // Store received byte into the buffer
    }
    while (UCB0CTLW0 & UCTXSTP);      // Wait until the STOP condition is complete
}

void write_i2c(uint8_t reg, uint8_t value)
{
    // Wait until the I²C bus is free
    while (UCB0STATW & UCBBUSY);

    // Set eUSCI_B0 to Transmit mode and issue a START condition
    UCB0CTLW0 |= UCTR | UCTXSTT;
    while (!(UCB0IFG & UCTXIFG0));   // Wait for TX buffer ready

    // Send the register address (the location you want to write to)
    UCB0TXBUF = reg;
    while (!(UCB0IFG & UCTXIFG0));   // Wait for the register address to be transmitted

    // Send the data byte you wish to write to the register
    UCB0TXBUF = value;
    while (!(UCB0IFG & UCTXIFG0));   // Wait for the data byte to be transmitted

    // Issue a STOP condition to end the transaction
    UCB0CTLW0 |= UCTXSTP;
    while (UCB0CTLW0 & UCTXSTP);     // Wait for the STOP condition to complete
}

void uint8_to_hex(uint8_t value, char *buf) {
    const char hexDigits[] = "0123456789ABCDEF";
    buf[0] = hexDigits[(value >> 4) & 0x0F];
    buf[1] = hexDigits[value & 0x0F];
    buf[2] = '\0';
}

int main(void)
{
    WDTCTL = WDTPW | WDTHOLD;   // Stop watchdog timer
    PM5CTL0 &= ~LOCKLPM5;       // Disable high-impedance mode

    // Configure LED on P1.0 for debugging
    P1DIR |= BIT0;
    P1OUT &= ~BIT0;

    i2c_init();
    lcd_init();

    delay_ms(100);

    send_sentence_lcd("Hello");

    uint8_t whoami = read_i2c(WHO_AM_I_REG);
    if (whoami != 0x33) {
        send_sentence_lcd("Err WHO:");
        char buf[3];
        uint8_to_hex(whoami, buf);
        send_sentence_lcd(buf);
        while (1) {
            P1OUT ^= BIT0;
            delay_ms(1000);
        }
    }


    uint8_t dataToWrite = 192;
    // 1100000
    // ^^-----
    // 7: ADC enabled
    // 6: Temp enabled

    write_i2c(LIS3DH_TEMP_CFG_REG, dataToWrite);

    dataToWrite = 71;
    // 01000111
    // ^^^^^^^^
    // 7-4: 50hz
    // 3: Normal mode
    // 2: Z on
    // 1: Y on
    // 0: X on

    write_i2c(CTRL_REG1, dataToWrite);

    delay_ms(300);

    // Variables to hold the sensor data.
    uint8_t dataBuffer[6];
    char outStr[10];
    int16_t x_raw, y_raw, z_raw;

    while (1)
    {
        // Use a multi-byte read to grab all six output registers starting at 0x28.
        readx_i2c(LIS3DH_OUT_X_L, dataBuffer, 6);

        // Combine the low and high bytes.
        x_raw = (((int16_t)dataBuffer[1]) << 8) | dataBuffer[0];
        y_raw = (((int16_t)dataBuffer[3]) << 8) | dataBuffer[2];
        z_raw = (((int16_t)dataBuffer[5]) << 8) | dataBuffer[4];

        // For normal mode (10-bit), shift right by 6 bits to get the valid 10-bit value.
        x_raw = x_raw >> 6;
        y_raw = y_raw >> 6;
        z_raw = z_raw >> 6;

        // Clear and update the LCD with the new values.
        send_command_lcd(0x01);
        pos = 0;
        line = 0;
        sprintf(outStr, "X:%d", x_raw);
        send_sentence_lcd(outStr);
        sprintf(outStr, " Y:%d", y_raw);
        send_sentence_lcd(outStr);
        sprintf(outStr, " Z:%d", z_raw);
        send_sentence_lcd(outStr);

        delay_ms(1000);
    }
}