#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "VL53L1X_Fonctions.h"
#include "SelectionCapteur.h"
#include "hardware/pio.h"
#include "hardware/i2c.h"
#include "ws2812.h"
#include "ws2812.pio.h"
#include "i2c_fifo.h"
#include "i2c_slave.h"


#include "Tests.h"

#define I2C0_SDA_PIN 0
#define I2C0_SCL_PIN 1

#define I2C1_SDA_PIN 18
#define I2C1_SCL_PIN 19


void i2c_master_init(void);
int continuous_reading(uint8_t device);
int calibration(uint8_t device);
int change_address(uint8_t * device, uint8_t new_i2c_7bits_address);
void initialise_adresses(void);
void affiche_distance_sur_led();
void init_sensors(void);

uint8_t tampon_commande_led[3];

#define I2C_SLAVE_ADDRESS 0x18
#define ADRESSE_RECEPTION_DATA 0x10
#define ADRESSE_COULEUR_LED 0x10
#define ADRESSE_MASQUE_LED_1 0x11
#define ADRESSE_MASQUE_LED_2 0x12

static const uint I2C_SLAVE_SDA_PIN = I2C1_SDA_PIN; 
static const uint I2C_SLAVE_SCL_PIN = I2C1_SCL_PIN;

// The slave implements a 256 byte memory. To write a series of bytes, the master first
// writes the memory address, followed by the data. The address is automatically incremented
// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
// sequentially from the current memory address.
static struct
{
    uint8_t mem[256];
    uint8_t mem_address;
    bool mem_address_written;
} context;

// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
// printing to stdio may interfere with interrupt handling.
static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
    switch (event) {
    case I2C_SLAVE_RECEIVE: // master has written some data
        if (!context.mem_address_written) {
            // writes always start with the memory address
            context.mem_address = i2c_read_byte(i2c);
            context.mem_address_written = true;
        } else {
            // save into memory
            context.mem[context.mem_address] = i2c_read_byte(i2c);
            context.mem_address++;
        }
        break;
    case I2C_SLAVE_REQUEST: // master is requesting data
        // load from memory
        i2c_write_byte(i2c, context.mem[context.mem_address]);
        context.mem_address++;
        break;
    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
        context.mem_address_written = false;
        break;
    default:
        break;
    }
}

void i2c_set_slave_mode_perso(i2c_inst_t *i2c, uint8_t addr) {
    i2c->hw->enable = 0;
    
    //while( !(i2c->hw->enable_status & 0x1) );
    
    i2c->hw->sar = addr;
    i2c->hw->con = 0;
    
    i2c->hw->enable = 1;
}

static void setup_slave() {
    gpio_init(I2C_SLAVE_SDA_PIN);
    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SLAVE_SDA_PIN);

    gpio_init(I2C_SLAVE_SCL_PIN);
    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SLAVE_SCL_PIN);

    i2c_slave_init(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
}


void blink(void){
    const uint LED_PIN = PICO_DEFAULT_LED_PIN;
    while(1){
        printf("couleur_Led:%2x\n", tampon_commande_led[0]);
        printf("masque_Led:%3x\n", (tampon_commande_led[1]<<8) | tampon_commande_led[2]);

        for(uint8_t capteur=0; capteur<12; capteur++){
            printf(">distance%x:%d\n", capteur, context.mem[capteur]);
        }
        
        gpio_put(LED_PIN, !gpio_get(LED_PIN));
        sleep_ms(20);

    }

}


void main(void)
{
    int status;
    uint8_t distance_capteur_cm[12];

    const uint LED_PIN = PICO_DEFAULT_LED_PIN;
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 1);
    
    tampon_commande_led[0]=0;
    tampon_commande_led[1]=0;
    tampon_commande_led[2]=0;
    
    uint8_t VL53L1X_device = 0x29;

    stdio_init_all();
    i2c_master_init();
    Selection_capteur_init();
    //Selection_capteur_select(1);

    ws2812_init();

    printf("End waiting\n");
    setup_slave();

    //Tests();

    multicore_launch_core1(blink);

    initialise_adresses();

    uint8_t capteur_courant=0;
    while(1){
        // Lecture des capteurs
        if(capteur_pret(capteur_courant)){
            uint8_t distance_cm;
            if(capteur_lire_distance_cm(capteur_courant, &distance_cm)){
                distance_capteur_cm[capteur_courant]= distance_cm;
            }
        }
        capteur_courant++;
        if(capteur_courant > 11){
            capteur_courant = 0;
        }

        // Affichage des distances sur les LEDs.
        affiche_distance_sur_led(distance_capteur_cm);

        // Envoie des valeurs des capteurs
        for(uint8_t capteur=0; capteur<12; capteur++){
            context.mem[capteur] = distance_capteur_cm[capteur];
        }

        // Reception des données à afficher sur les capteurs
        // Si nous avons reçu une nouvelle commande
        if(tampon_commande_led[0] != context.mem[0x10] ||
                tampon_commande_led[1] != context.mem[0x11] ||
                tampon_commande_led[2] != context.mem[0x12] ){
            
            tampon_commande_led[0] = context.mem[0x10];
            tampon_commande_led[1] = context.mem[0x11];
            tampon_commande_led[2] = context.mem[0x12];

            uint8_t couleur = tampon_commande_led[0];
            uint16_t masque_led = (tampon_commande_led[1] << 8) | tampon_commande_led[2];
            
            reset_affichage_led();
            for(uint8_t led=0; led < 12; led++){
                if((masque_led >> led) & 0x01){
                    affiche_couleur_sur_led(couleur, led);
                }
            }
            ws2812_affiche_buffer();
        }
    }
}


void i2c_master_init(void){
    //stdio_init_all();
    i2c_init(i2c0, 100 * 1000);

    printf("Initialisation des broches\n");
    for(int i=0; i++; i<=28){
        if(gpio_get_function(i) == GPIO_FUNC_I2C){
            printf("Pin I2C : %d\n", i);
            gpio_set_function(i, GPIO_FUNC_NULL);
        }
    }

    printf("%d and %d for I2C\n", I2C0_SDA_PIN, I2C0_SCL_PIN);
    gpio_set_function(I2C0_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C0_SCL_PIN, GPIO_FUNC_I2C);

}