#include <stdio.h>
#include "pico/stdlib.h"
#include <time.h>

#define S0 2 
#define S1 3
#define S2 4 
#define S3 5
#define S_OUT 6
#define LED 8

absolute_time_t start_time, end_time;
uint64_t start_us, end_us,duration_R, duration_G, duration_B;
int R,G,B;
uint coul,val;
uint redMin = 52; // Red minimum value
uint redMax = 440; // Red maximum value
uint greenMin = 72; // Green minimum value
uint greenMax = 695; // Green maximum value
uint blueMin = 79; // Blue minimum value
uint blueMax = 783; // Blue maximum value

void irq_rise_and_fall(uint gpio,uint32_t events)
{
    if(coul==1)
    {
        if(events==8 && val ==0)
        {
            start_time=get_absolute_time();
            start_us = to_us_since_boot(start_time);
            //printf("début: %lld us\n\r", start_us);
            val=1;
        }
        if(events==4 && val == 1)
        {
            end_time=get_absolute_time();
            end_us = to_us_since_boot(end_time);
            //printf("fin: %lld us\n\r", end_us);
            duration_R = end_us-start_us;
            R = (int)((float)duration_R - (float)redMin) * (0 - 255) / ((float)redMax - (float)redMin) + 255;
            val =0;
            //printf("temps: %lld s\n\r", duration);
        }
    }
    if(coul ==2)
    {
        if(events==8)
        {
            start_time=get_absolute_time();
            start_us = to_us_since_boot(start_time);
            //printf("début: %lld us\n\r", start_us);
        }
        if(events==4)
        {
            end_time=get_absolute_time();
            end_us = to_us_since_boot(end_time);
            //printf("fin: %lld us\n\r", end_us);
            duration_G = end_us-start_us;
            G = (int)((float)duration_G - (float)greenMin) * (0 - 255) / ((float)greenMax - (float)greenMin) + 255;
            //printf("temps: %lld s\n\r", duration);
        }
    }
    if(coul ==3)
    {
        if(events==8)
        {
            start_time=get_absolute_time();
            start_us = to_us_since_boot(start_time);
            //printf("début: %lld us\n\r", start_us);
        }
        if(events==4)
        {
            end_time=get_absolute_time();
            end_us = to_us_since_boot(end_time);
            //printf("fin: %lld us\n\r", end_us);
            duration_B = end_us-start_us;
            B = (int)((float)duration_B - (float)blueMin) * (0 - 255) / ((float)blueMax - (float)blueMin) + 255;
            //printf("temps: %lld s\n\r", duration);
        }
    }
    //printf("event %d \n\r",events);
}

/*void irq_fall(uint gpio,uint32_t events)
{
    end_time=get_absolute_time();
    end_us = to_us_since_boot(end_time);
    printf("fall \n\r");
}*/
void init_color_sensor() {

    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);

    gpio_init(S0);
    gpio_set_dir(S0, GPIO_OUT);
    gpio_put(S0, 1);

    gpio_init(S1);
    gpio_set_dir(S1, GPIO_OUT);
    gpio_put(S1, 0);

    gpio_init(S2);
    gpio_set_dir(S2, GPIO_OUT);

    gpio_init(S3);
    gpio_set_dir(S3, GPIO_OUT);

    gpio_init(LED);
    gpio_set_dir(LED, GPIO_OUT);
    gpio_put(LED, 1);
   
    gpio_init(S_OUT);
    gpio_set_dir(S_OUT, GPIO_IN);
    gpio_set_irq_enabled_with_callback(S_OUT,GPIO_IRQ_EDGE_RISE|GPIO_IRQ_EDGE_FALL,true,&irq_rise_and_fall);
    //gpio_set_irq_enabled_with_callback(ECHO_PIN,GPIO_IRQ_EDGE_FALL,true,&irq_fall);
}

void measure_couleur() {

    //absolute_time_t start_time, end_time;
    //uint64_t duration=0 ;
    //float distance=0;
    end_us = 0;
    start_us = 0;
    // Envoie une impulsion de 10 microsecondes sur TRIG_PIN
    gpio_put(S2, 0);
    //sleep_us(10);
    gpio_put(S3, 0);
    coul = 1;
    sleep_ms(100);

    gpio_put(S2, 1);
    //sleep_us(10);
    gpio_put(S3, 1);
    coul = 2;
    sleep_ms(100);

    gpio_put(S2, 0);
    //sleep_us(10);
    gpio_put(S3, 1);
    coul = 3;
    sleep_ms(200);

    // Attend la réponse sur ECHO_PIN
    
    /*while (gpio_get(ECHO_PIN) == 0);
    start_time=get_absolute_time();
    start_us = to_us_since_boot(start_time);
    printf("début: %lld us\n\r", start_us);
    while(gpio_get(ECHO_PIN)==1);
    end_time=get_absolute_time();
    end_us = to_us_since_boot(end_time);*/
    //printf("début: %lld us\n\r", start_us);
    //printf("fin: %lld us\n\r", end_us);

    
    // Calcul de la distance en cm
    /*duration = end_us-start_us;
    printf("temps: %2.2f cm\n\r", duration);
    distance = (duration*0.0340)/2.0;
    return distance;*/
}

int main() {
    stdio_init_all();
    init_color_sensor();
    while (1) {
        measure_couleur();
        //printf("dur_R : %lld  dur_G : %lld  dur_B: %lld \n\r", duration_R,duration_G,duration_B);
        printf("R : %d  G : %d  B: %d \n\r", R,G,B);
        if(R > 100)
        {
            gpio_put(PICO_DEFAULT_LED_PIN, true);
        }
        else
        {
            gpio_put(PICO_DEFAULT_LED_PIN, false);
        }
        sleep_ms(1000);
    }
}