#include <stdint.h>
#include <stdbool.h>

// Base address I2C HPS
#define I2C0_BASE 0xFFC05000

// I2C Register offsets
#define I2C_CON         0x00  // Control Register
#define I2C_TAR         0x04  // Target Address Register
#define I2C_DATA_CMD    0x10  // Data Buffer and Command Register
#define I2C_SS_SCL_HCNT 0x14  // Standard Speed Clock SCL High Count
#define I2C_SS_SCL_LCNT 0x18  // Standard Speed Clock SCL Low Count
#define I2C_FS_SCL_HCNT 0x1C  // Fast Speed Clock SCL High Count
#define I2C_FS_SCL_LCNT 0x20  // Fast Speed Clock SCL Low Count
#define I2C_INTR_STAT   0x2C  // Interrupt Status
#define I2C_INTR_MASK   0x30  // Interrupt Mask
#define I2C_RAW_INTR_STAT 0x34 // Raw Interrupt Status
#define I2C_RX_TL       0x38  // Receive FIFO Threshold
#define I2C_TX_TL       0x3C  // Transmit FIFO Threshold
#define I2C_CLR_INTR    0x40  // Clear Combined and Individual Interrupt
#define I2C_CLR_RX_UNDER 0x44 // Clear RX_UNDER Interrupt
#define I2C_CLR_RX_OVER  0x48 // Clear RX_OVER Interrupt
#define I2C_CLR_TX_OVER  0x4C // Clear TX_OVER Interrupt
#define I2C_CLR_RD_REQ   0x50 // Clear RD_REQ Interrupt
#define I2C_CLR_TX_ABRT  0x54 // Clear TX_ABRT Interrupt
#define I2C_CLR_RX_DONE  0x58 // Clear RX_DONE Interrupt
#define I2C_CLR_ACTIVITY 0x5C // Clear ACTIVITY Interrupt
#define I2C_CLR_STOP_DET 0x60 // Clear STOP_DET Interrupt
#define I2C_CLR_START_DET 0x64 // Clear START_DET Interrupt
#define I2C_CLR_GEN_CALL  0x68 // Clear GEN_CALL Interrupt
#define I2C_ENABLE      0x6C  // Enable Register
#define I2C_STATUS      0x70  // Status Register
#define I2C_TXFLR       0x74  // Transmit FIFO Level Register
#define I2C_RXFLR       0x78  // Receive FIFO Level Register
#define I2C_TX_ABRT_SOURCE 0x80 // Transmit Abort Source Register

// I2C Control Register bits
#define IC_CON_MASTER_MODE   (1 << 0)
#define IC_CON_SPEED_STD     (1 << 1)
#define IC_CON_SPEED_FAST    (2 << 1)
#define IC_CON_RESTART_EN    (1 << 5)
#define IC_CON_SLAVE_DISABLE (1 << 6)

// I2C Data Command Register bits
#define IC_DATA_CMD_READ     (1 << 8)
#define IC_DATA_CMD_STOP     (1 << 9)
#define IC_DATA_CMD_RESTART  (1 << 10)

// MPU6050 I2C Address (AD0 pin LOW = 0x68, HIGH = 0x69)
#define MPU6050_ADDR 0x68

// MPU6050 Register addresses
#define MPU6050_WHO_AM_I     0x75  // Device ID (should read 0x68)
#define MPU6050_PWR_MGMT_1   0x6B  // Power Management 1
#define MPU6050_PWR_MGMT_2   0x6C  // Power Management 2
#define MPU6050_SMPLRT_DIV   0x19  // Sample Rate Divider
#define MPU6050_CONFIG       0x1A  // Configuration
#define MPU6050_GYRO_CONFIG  0x1B  // Gyroscope Configuration
#define MPU6050_ACCEL_CONFIG 0x1C  // Accelerometer Configuration
#define MPU6050_INT_ENABLE   0x38  // Interrupt Enable
#define MPU6050_ACCEL_XOUT_H 0x3B  // Accelerometer X-axis High Byte
#define MPU6050_ACCEL_XOUT_L 0x3C  // Accelerometer X-axis Low Byte
#define MPU6050_ACCEL_YOUT_H 0x3D  // Accelerometer Y-axis High Byte
#define MPU6050_ACCEL_YOUT_L 0x3E  // Accelerometer Y-axis Low Byte
#define MPU6050_ACCEL_ZOUT_H 0x3F  // Accelerometer Z-axis High Byte
#define MPU6050_ACCEL_ZOUT_L 0x40  // Accelerometer Z-axis Low Byte
#define MPU6050_TEMP_OUT_H   0x41  // Temperature High Byte
#define MPU6050_TEMP_OUT_L   0x42  // Temperature Low Byte
#define MPU6050_GYRO_XOUT_H  0x43  // Gyroscope X-axis High Byte
#define MPU6050_GYRO_XOUT_L  0x44  // Gyroscope X-axis Low Byte
#define MPU6050_GYRO_YOUT_H  0x45  // Gyroscope Y-axis High Byte
#define MPU6050_GYRO_YOUT_L  0x46  // Gyroscope Y-axis Low Byte
#define MPU6050_GYRO_ZOUT_H  0x47  // Gyroscope Z-axis High Byte
#define MPU6050_GYRO_ZOUT_L  0x48  // Gyroscope Z-axis Low Byte

// MPU6050 Power Management bits
#define MPU6050_PWR1_DEVICE_RESET  (1 << 7)
#define MPU6050_PWR1_SLEEP         (1 << 6)
#define MPU6050_PWR1_CLKSEL_PLL_X  0x01

// Gyroscope Full Scale Range
#define MPU6050_GYRO_FS_250   0x00  // ±250 °/s
#define MPU6050_GYRO_FS_500   0x08  // ±500 °/s
#define MPU6050_GYRO_FS_1000  0x10  // ±1000 °/s
#define MPU6050_GYRO_FS_2000  0x18  // ±2000 °/s

// Accelerometer Full Scale Range
#define MPU6050_ACCEL_FS_2G   0x00  // ±2g
#define MPU6050_ACCEL_FS_4G   0x08  // ±4g
#define MPU6050_ACCEL_FS_8G   0x10  // ±8g
#define MPU6050_ACCEL_FS_16G  0x18  // ±16g

// Struttura per i dati dell'accelerometro
typedef struct {
    int16_t x;
    int16_t y;
    int16_t z;
} MPU6050_Accel_Data;

// Struttura per i dati del giroscopio
typedef struct {
    int16_t x;
    int16_t y;
    int16_t z;
} MPU6050_Gyro_Data;

// Struttura per tutti i dati del sensore
typedef struct {
    MPU6050_Accel_Data accel;
    MPU6050_Gyro_Data gyro;
    int16_t temperature;
} MPU6050_All_Data;

// Funzioni di utilità per accesso ai registri I2C
static inline void i2c_write_reg(uint32_t reg, uint32_t value) {
    *((volatile uint32_t*)(I2C0_BASE + reg)) = value;
}

static inline uint32_t i2c_read_reg(uint32_t reg) {
    return *((volatile uint32_t*)(I2C0_BASE + reg));
}

// Delay semplice (da calibrare in base al clock)
void delay_us(uint32_t us) {
    volatile uint32_t count = us * 50; // Approssimativo, da calibrare
    while(count--);
}

// Inizializza l'interfaccia I2C
bool i2c_init(void) {
    // Disabilita I2C
    i2c_write_reg(I2C_ENABLE, 0);
    delay_us(10);
    
    // Configura I2C in modalità Master, Fast Mode (400kHz), con restart abilitato
    i2c_write_reg(I2C_CON, 
                  IC_CON_MASTER_MODE | 
                  IC_CON_SPEED_FAST | 
                  IC_CON_RESTART_EN | 
                  IC_CON_SLAVE_DISABLE);
    
    // Imposta l'indirizzo target dell'MPU6050
    i2c_write_reg(I2C_TAR, MPU6050_ADDR);
    
    // Configura i timing per Fast Mode (400kHz)
    // Assumendo clock I2C di 100MHz:
    // High time: 0.6us @ 400kHz = 60 cicli
    // Low time: 1.3us @ 400kHz = 130 cicli
    i2c_write_reg(I2C_FS_SCL_HCNT, 60);
    i2c_write_reg(I2C_FS_SCL_LCNT, 130);
    
    // Imposta threshold FIFO
    i2c_write_reg(I2C_RX_TL, 0);
    i2c_write_reg(I2C_TX_TL, 0);
    
    // Disabilita tutti gli interrupt
    i2c_write_reg(I2C_INTR_MASK, 0);
    
    // Abilita I2C
    i2c_write_reg(I2C_ENABLE, 1);
    delay_us(10);
    
    return true;
}

// Scrive un byte in un registro dell'MPU6050
bool mpu6050_write_register(uint8_t reg, uint8_t value) {
    // Attendi che il TX FIFO non sia pieno
    uint32_t timeout = 10000;
    while ((i2c_read_reg(I2C_STATUS) & (1 << 1)) && timeout--) {
        delay_us(1);
    }
    if (timeout == 0) return false;
    
    // Scrivi l'indirizzo del registro
    i2c_write_reg(I2C_DATA_CMD, reg);
    
    // Scrivi il valore con STOP
    i2c_write_reg(I2C_DATA_CMD, value | IC_DATA_CMD_STOP);
    
    // Attendi che la trasmissione sia completata
    timeout = 10000;
    while ((i2c_read_reg(I2C_STATUS) & (1 << 0)) && timeout--) {
        delay_us(1);
    }
    
    return timeout > 0;
}

// Legge un byte da un registro dell'MPU6050
bool mpu6050_read_register(uint8_t reg, uint8_t *value) {
    // Attendi che il TX FIFO non sia pieno
    uint32_t timeout = 10000;
    while ((i2c_read_reg(I2C_STATUS) & (1 << 1)) && timeout--) {
        delay_us(1);
    }
    if (timeout == 0) return false;
    
    // Scrivi l'indirizzo del registro
    i2c_write_reg(I2C_DATA_CMD, reg);
    
    // Richiedi lettura con STOP
    i2c_write_reg(I2C_DATA_CMD, IC_DATA_CMD_READ | IC_DATA_CMD_STOP);
    
    // Attendi che ci sia un dato nel RX FIFO
    timeout = 10000;
    while ((i2c_read_reg(I2C_RXFLR) == 0) && timeout--) {
        delay_us(1);
    }
    if (timeout == 0) return false;
    
    // Leggi il dato
    *value = (uint8_t)(i2c_read_reg(I2C_DATA_CMD) & 0xFF);
    
    return true;
}

// Legge più byte consecutivi dall'MPU6050
bool mpu6050_read_registers(uint8_t reg, uint8_t *buffer, uint8_t length) {
    uint32_t timeout;
    
    // Attendi che il TX FIFO non sia pieno
    timeout = 10000;
    while ((i2c_read_reg(I2C_STATUS) & (1 << 1)) && timeout--) {
        delay_us(1);
    }
    if (timeout == 0) return false;
    
    // Scrivi l'indirizzo del registro di partenza
    i2c_write_reg(I2C_DATA_CMD, reg);
    
    // Richiedi letture multiple
    for (uint8_t i = 0; i < length; i++) {
        uint32_t cmd = IC_DATA_CMD_READ;
        if (i == length - 1) {
            cmd |= IC_DATA_CMD_STOP;  // STOP sull'ultima lettura
        }
        i2c_write_reg(I2C_DATA_CMD, cmd);
    }
    
    // Leggi i dati dal RX FIFO
    for (uint8_t i = 0; i < length; i++) {
        timeout = 10000;
        while ((i2c_read_reg(I2C_RXFLR) == 0) && timeout--) {
            delay_us(1);
        }
        if (timeout == 0) return false;
        
        buffer[i] = (uint8_t)(i2c_read_reg(I2C_DATA_CMD) & 0xFF);
    }
    
    return true;
}

// Inizializza l'MPU6050
bool mpu6050_init(void) {
    uint8_t who_am_i;
    
    // Reset del dispositivo
    if (!mpu6050_write_register(MPU6050_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET)) {
        return false;
    }
    delay_us(100000);  // Attendi 100ms dopo il reset
    
    // Verifica WHO_AM_I (dovrebbe essere 0x68)
    if (!mpu6050_read_register(MPU6050_WHO_AM_I, &who_am_i)) {
        return false;
    }
    
    if (who_am_i != 0x68) {
        return false;  // Device ID non valido
    }
    
    // Sveglia il dispositivo e imposta il clock su PLL con X gyro reference
    if (!mpu6050_write_register(MPU6050_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_PLL_X)) {
        return false;
    }
    delay_us(10000);  // Attendi 10ms
    
    // Abilita tutti i sensori (accelerometro e giroscopio)
    if (!mpu6050_write_register(MPU6050_PWR_MGMT_2, 0x00)) {
        return false;
    }
    
    // Imposta sample rate divider a 0 (1kHz / (1 + 0) = 1kHz)
    if (!mpu6050_write_register(MPU6050_SMPLRT_DIV, 0x00)) {
        return false;
    }
    
    // Configura il filtro passa-basso digitale (DLPF) a 94Hz
    if (!mpu6050_write_register(MPU6050_CONFIG, 0x02)) {
        return false;
    }
    
    // Configura il range del giroscopio: ±500 °/s
    if (!mpu6050_write_register(MPU6050_GYRO_CONFIG, MPU6050_GYRO_FS_500)) {
        return false;
    }
    
    // Configura il range dell'accelerometro: ±4g
    if (!mpu6050_write_register(MPU6050_ACCEL_CONFIG, MPU6050_ACCEL_FS_4G)) {
        return false;
    }
    
    delay_us(10000);  // Attendi stabilizzazione
    
    return true;
}

// Legge i dati raw di accelerazione dall'MPU6050
bool mpu6050_read_accel_raw(MPU6050_Accel_Data *data) {
    uint8_t buffer[6];
    
    // Leggi tutti i 6 byte dei dati accelerometro (XOUT_H, XOUT_L, YOUT_H, YOUT_L, ZOUT_H, ZOUT_L)
    if (!mpu6050_read_registers(MPU6050_ACCEL_XOUT_H, buffer, 6)) {
        return false;
    }
    
    // Combina i byte in valori a 16 bit con segno
    data->x = (int16_t)((buffer[0] << 8) | buffer[1]);
    data->y = (int16_t)((buffer[2] << 8) | buffer[3]);
    data->z = (int16_t)((buffer[4] << 8) | buffer[5]);
    
    return true;
}

// Legge i dati raw del giroscopio dall'MPU6050
bool mpu6050_read_gyro_raw(MPU6050_Gyro_Data *data) {
    uint8_t buffer[6];
    
    // Leggi tutti i 6 byte dei dati giroscopio (XOUT_H, XOUT_L, YOUT_H, YOUT_L, ZOUT_H, ZOUT_L)
    if (!mpu6050_read_registers(MPU6050_GYRO_XOUT_H, buffer, 6)) {
        return false;
    }
    
    // Combina i byte in valori a 16 bit con segno
    data->x = (int16_t)((buffer[0] << 8) | buffer[1]);
    data->y = (int16_t)((buffer[2] << 8) | buffer[3]);
    data->z = (int16_t)((buffer[4] << 8) | buffer[5]);
    
    return true;
}

// Legge tutti i dati (accelerometro + giroscopio + temperatura) in un'unica operazione
bool mpu6050_read_all_raw(MPU6050_All_Data *data) {
    uint8_t buffer[14];
    
    // Leggi tutti i 14 byte (6 accel + 2 temp + 6 gyro)
    if (!mpu6050_read_registers(MPU6050_ACCEL_XOUT_H, buffer, 14)) {
        return false;
    }
    
    // Accelerometro
    data->accel.x = (int16_t)((buffer[0] << 8) | buffer[1]);
    data->accel.y = (int16_t)((buffer[2] << 8) | buffer[3]);
    data->accel.z = (int16_t)((buffer[4] << 8) | buffer[5]);
    
    // Temperatura
    data->temperature = (int16_t)((buffer[6] << 8) | buffer[7]);
    
    // Giroscopio
    data->gyro.x = (int16_t)((buffer[8] << 8) | buffer[9]);
    data->gyro.y = (int16_t)((buffer[10] << 8) | buffer[11]);
    data->gyro.z = (int16_t)((buffer[12] << 8) | buffer[13]);
    
    return true;
}

// Converte i valori raw dell'accelerometro in g (con range ±4g)
void mpu6050_accel_raw_to_g(MPU6050_Accel_Data *raw, float *x_g, float *y_g, float *z_g) {
    // Con ±4g, la sensibilità è 8192 LSB/g
    const float scale = 1.0 / 8192.0;
    
    *x_g = raw->x * scale;
    *y_g = raw->y * scale;
    *z_g = raw->z * scale;
}

// Converte i valori raw del giroscopio in °/s (con range ±500°/s)
void mpu6050_gyro_raw_to_dps(MPU6050_Gyro_Data *raw, float *x_dps, float *y_dps, float *z_dps) {
    // Con ±500°/s, la sensibilità è 65.5 LSB/(°/s)
    const float scale = 1.0 / 65.5;
    
    *x_dps = raw->x * scale;
    *y_dps = raw->y * scale;
    *z_dps = raw->z * scale;
}

// Converte la temperatura raw in gradi Celsius
float mpu6050_temp_raw_to_celsius(int16_t temp_raw) {
    // Formula: Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
    return (temp_raw / 340.0) + 36.53;
}

// Esempio di utilizzo
int main(void) {
    MPU6050_All_Data sensor_data;
    float accel_x_g, accel_y_g, accel_z_g;
    float gyro_x_dps, gyro_y_dps, gyro_z_dps;
    float temperature_c;
    
    // Inizializza I2C
    if (!i2c_init()) {
        // Errore inizializzazione I2C
		printf("Errore inizializzazione I2C\n");
        return -1;
    }
    
    // Inizializza MPU6050
    if (!mpu6050_init()) {
        // Errore inizializzazione MPU6050
		printf("Errore inizializzazione MPU6050\n");
        return -2;
    }
    
    // Loop principale di lettura
    while (1) {
        // Leggi tutti i dati in un'unica operazione (più efficiente)
        if (mpu6050_read_all_raw(&sensor_data)) {
            
            // VALORI RAW disponibili in:
            // sensor_data.accel.x, sensor_data.accel.y, sensor_data.accel.z (accelerazioni)
            // sensor_data.gyro.x, sensor_data.gyro.y, sensor_data.gyro.z (velocità angolari)
            // sensor_data.temperature (temperatura)
            
            // Converti in unità fisiche (opzionale)
            mpu6050_accel_raw_to_g(&sensor_data.accel, &accel_x_g, &accel_y_g, &accel_z_g);
            mpu6050_gyro_raw_to_dps(&sensor_data.gyro, &gyro_x_dps, &gyro_y_dps, &gyro_z_dps);
            temperature_c = mpu6050_temp_raw_to_celsius(sensor_data.temperature);
			
            
            // Valori convertiti disponibili in:
            // accel_x_g, accel_y_g, accel_z_g (in g)
            // gyro_x_dps, gyro_y_dps, gyro_z_dps (in °/s)
            // temperature_c (in °C)
            
            // Qui puoi inviare i dati via UART o utilizzarli
            // Per il debug, potresti voler implementare una printf via UART
        }
        
		printf("%f %f %f --- %f% f% f\n",accel_x_g,accel_y_g,accel_z_g,gyro_x_dps,gyro_y_dps,gyro_z_dps);
        delay_us(100000);  // Ritardo tra le letture (100ms = 10Hz)
    }
    
    return 0;
}