#include <stdio.h>
#include <stdint.h>
#include <esp32-c3_regs.h>
#include <calibrate_adc.c>

volatile uint32_t* GPIO_ENABLE_W1TS_REG =   (volatile uint32_t*) 0x60004024;// GPIO Enable Set Register (Verwendung wird Reference Manual empfohlen, fehlt aber in der esp32-c3_regs.h)

//----------GLOBALE KONSTANTEN (Bei Bedarf beliebig anpassen!)----------//
#define LEDS 16                                                             // 16 physische LEDS
#define PIXEL_PER_LED 3                                                     // 3 Pixel pro LED
#define PIXEL_ARRAY_SIZE LEDS * PIXEL_PER_LED                               // 16 * 3 = 48 Pixel
uint8_t pixels[PIXEL_ARRAY_SIZE];                                           // Globales Array fuer 48 Pixel mit je 8 Bit = 384 Bit
#define MIN_DISTANCE 5                                                      // Minimale Sensordistanz
#define MAX_DISTANCE 50                                                     // Maximale Sensordistanz
#define TOTAL_MEASUREMENTS_FOR_AVERAGE 10                                   // Anzahl Messungen für einen Mittelwert
#define MAX_DISTANCE_DEVIATION_PERCENT 20                                   // +/- Maximale Abweichung in Prozent

//----------INITIALISIERUNG UND GLOBALE FUNKTIONEN----------//
void gpio_init()
{
    *GPIO_ENABLE_W1TS_REG |= (0b1 << 0);                                    // Enable GPIO0 als Output (Trigger)
    *GPIO_ENABLE_W1TS_REG |= (0b1 << 2);                                    // Enable GPIO2 als Output (LED)
    *GPIO_ENABLE_W1TS_REG |= (0b1 << 8);                                    // Enable GPIO8 als Output (LED)
    *IO_MUX_GPIO1_REG |= (0b1 << 9);  	                                    // Enable GPIO1 als Input (Echo)
    *IO_MUX_GPIO3_REG |= (0b1 << 9);                                        // Enable GPIO3 als Input (Poti)
}

void adc_init()
{
    *APB_SARADC_CTRL_REG |= (0b1 << 0);                                     // ADC per Software Variante auswählen
    *APB_SARADC_CTRL_REG |= (0b1 << 1);                                     // ADC per Software starten
    *APB_SARADC_ONETIME_SAMPLE_REG |= (0b11 << 23);                         // Dämpfung auf 12dB
    *APB_SARADC_ONETIME_SAMPLE_REG &= ~(0b1111 << 25);                      // Vorige Kanalauswahl löschen
    *APB_SARADC_ONETIME_SAMPLE_REG |= (3 << 25);                            // Kanal 3 auswählen
    *APB_SARADC_ONETIME_SAMPLE_REG |= (0b1 << 31);                          // Enable OneTimeSampling ADC1
    *APB_SARADC_INT_ENA_REG |= (0b1 << 31);                                 // Enable Done-Interrupt ADC1
    adc_calibrate(1);                                                       // Kalibriere ADC1
}

void timer_init()
{
    *TIMG_T0CONFIG_REG &= ~(0b1 << 9);                                      // APB_CLK nutzen
    *TIMG_T0CONFIG_REG |= (80 << 13);	                                    // Divider setzen (f = 80MHz / 80 = 1MHz ==> T = 1us)
	*TIMG_T0CONFIG_REG |= (0b1 << 30);                                      // Increase Counter
}

void reset_timer()
{
    *TIMG_T0LOADLO_REG |= (0b0 <<0);                                        // Startzeit = 0 (untere 32 Bit)
    *TIMG_T0LOADHI_REG |= (0b0 <<0);                                        // Startzeit = 0 (obere 22 Bit)
    *TIMG_T0LOAD_REG |= (0b0 <<0);                                          // Startzeit laden
}

void delay_ns(unsigned int ns)                                              // Delay in Nanosekunden (1 Takt = 43ns)
{
    asm volatile(
        //Berechnung der Taktanzahl
        "li t1, 43            \n\t"                                         // t1 = 43 (1 Takt = 43ns)
        "li t0, 21            \n\t"                                         // t0 = 43/2 = 21 (Rundung)
        "add t0, t0, %0       \n\t"                                         // t0 = 21 + xns (Rundung)
        "div t0, t0, t1       \n\t"                                         // t0 = (21 + xns) / 43 (Takte gerundet)
        "addi t0, t0, -5      \n\t"                                         // t0 = Takte - 5 (nop-Takte ohne Initialisierung)
        //NOP-Schleife
        "1:                   \n\t"
        "nop                  \n\t"
        "addi t0, t0, -1      \n\t"                                         // t0--
        "bnez t0, 1b          \n\t"                                         // while(t0 != 0) -> 1:
        //Parameter
        :                                                                   // Ausgabe: -
        : "r" (ns)                                                          // Eingabe: Nanosekunden in a0
        : "t0", "t1"                                                        // Clobber-Register
    );
}

//----------LEDS----------//
void write_one(int gpio) 
{
    *GPIO_OUT_W1TS_REG |= (0b1 << gpio);                                    // GPIOX setzen
    delay_ns(600);                                                          // 600ns warten
    *GPIO_OUT_W1TC_REG |= (0b1 << gpio);                                    // GPIOX ruecksetzen
    delay_ns(600);                                                          // 600ns warten
}

void write_zero(int gpio)
{
    *GPIO_OUT_W1TS_REG |= (0b1 << gpio);                                    // GPIOX setzen
    delay_ns(300);                                                          // 300ns warten
    *GPIO_OUT_W1TC_REG |= (0b1 << gpio);                                    // GPIOX ruecksetzen
    delay_ns(900);                                                          // 900ns warten
}

void display_pixelsArray_LEDstripe()
{
    for (int i = 0; i < PIXEL_ARRAY_SIZE; i++)                              // Fuer alle 48 Pixel
    {
        for (int k = 7; k >=0 ; k--)                                        // Fuer alle 8 Bit jedes Pixels
        {
        if (pixels[i] & (0b1 << k))                                         // Jedes Bit pruefen
        {
            write_one(2);                                                   // Bei 1: Schreib 1
        }
        else 
        {
            write_zero(2);                                                  // Bei 0: Schreib 0
        }
        }
    }
  delay_ns(50000);                                                          // 50us warten damit die LEDs die Daten uebernehmen (siehe Datenblatt)
}



void display_pixelsArray_onboardLED()
{
    for (int i = 0; i < PIXEL_PER_LED; i++)                                 // Fuer alle Pixel der Onboard-LED
    {
        for (int k = 7; k >=0 ; k--)                                        // Fuer alle 8 Bit jedes Pixels
        {
        if (pixels[i] & (0b1 << k))                                         // Jedes Bit pruefen
        {
            write_one(8);                                                   // Bei 1: Schreib 1
        }
        else 
        {
            write_zero(8);                                                  // Bei 0: Schreib 0
        }
        }
    }
  delay_ns(50000);                                                          // 50us warten damit die LEDs die Daten uebernehmen
}





//----------ULTRASCHALLSENSOR----------//
int read_distance_cm()
{
    int distanceLow32 = 0;
    reset_timer();                                                          // Timer auf 0s

    // Trigger
    *GPIO_OUT_W1TS_REG |= (0b1 << 0);                                       // TRIGGER setzen
    delay_ns(11000);                                                        // 11us warten
    *GPIO_OUT_W1TC_REG |= (0b1 << 0);                                       // TRIGGER ruecksetzen

    // Echo + Timer
    while (!(*GPIO_IN_REG &= (0b1 << 1))){}                                 // Warten auf ECHO
    *TIMG_T0CONFIG_REG |= (0b1 << 31);                                      // TIMER starten
    while (*GPIO_IN_REG &= (0b1 << 1)) {}                                   // Warten solange Echo-Signal anliegt
    *TIMG_T0CONFIG_REG &= ~(0b1 << 31);                                     // TIMER stoppen

    // Timer auslesen
    *TIMG_T0UPDATE_REG |= (0b1 << 0);                                       // Timerwertausgabe beauftragen
    while (*TIMG_T0UPDATE_REG &= (0b1 << 1)) {}                             // Warten bis Timerwert da ist
    distanceLow32 = *TIMG_T0LO_REG;                                         // untere 32 Bit des Timers auslesen
                                                                            //(obere 22 Bit werden nicht benötigt, da (2^32)/16 = 268s, Echo ist jedoch maximal 38ms lang)
    // Timerwert verarbeiten
    int distanceCm = distanceLow32 / 58;                                    // Umrechnung in cm (siehe Datenblatt des HC-SR04)

    if(distanceCm < 2 || distanceCm > 400)                                  // Bei Messwert ausserhalb des Messbereichs wird -1 zurückgegeben
    {
        return -1;
    }
    else                                                                    // Ist der Messwert plausibel, wird er zurückgegeben
    {
        return distanceCm;
    }
}


int calc_averageDistanceCorrected()
{
    int distancesArray[TOTAL_MEASUREMENTS_FOR_AVERAGE];                     // Array für Mittelwert
    int averageDistance = 0;                                                // Variable für Mittelwert
    int averageDistanceCorrected = 0;                                       // Variable für korrigierten Mittelwert (ohne Ausreisser)

    // Berechnung des Mittelwerts
    for(int i = 0; i < TOTAL_MEASUREMENTS_FOR_AVERAGE; i++)
    {
        distancesArray[i] = read_distance_cm();                             // Entfernungsmessung starten und Abstand in cm in Array auslesen
        if(distancesArray[i] == -1)                                         // Bei fehlerhaftem Messwert wird vorheriger Messwert verwendet
        {
            distancesArray[i] = distancesArray[i-1];
        }
        averageDistance += distancesArray[i];                               // Alle Messwerte aufsummieren
        //delay_ns(10000);
    }
    averageDistance = averageDistance / TOTAL_MEASUREMENTS_FOR_AVERAGE;     // Mittelwert bilden


    // Entfernung abweichender Werte und erneute Bildung des Mittelwerts
    int floorDistanceDeviation = averageDistance - (averageDistance * MAX_DISTANCE_DEVIATION_PERCENT / 100);// Untere maximale Abweichung
    int ceilDistanceDeviation = averageDistance + (averageDistance * MAX_DISTANCE_DEVIATION_PERCENT / 100); // Obere maximale Abweichung

    for(int i = 0; i < TOTAL_MEASUREMENTS_FOR_AVERAGE; i++)
    {
        if(distancesArray[i] < floorDistanceDeviation || distancesArray[i] > ceilDistanceDeviation) 
        {
        distancesArray[i] = averageDistance;                                // Falls Messwert ausserhalb der maximalen Abweichung, wird er auf Mittelwert gesetzt
        }
        averageDistanceCorrected += distancesArray[i];                      // Alle korrigierten Messwerte aufsummieren
    }
    averageDistanceCorrected = averageDistanceCorrected / TOTAL_MEASUREMENTS_FOR_AVERAGE;   // Mittelwert der korrigierten Messwerte bilden

    return averageDistanceCorrected;
}


//----------POTENTIOMETER----------//

int adc_read()
{
    *APB_SARADC_INT_CLR_REG |= (0b1 << 31);                                 // Done-Bit Clearen
    *APB_SARADC_ONETIME_SAMPLE_REG |= (0b1 << 29);                          // One-Time Sampling starten
    delay_ns(500000);                                                       // Warten bis Sampling abgeschlossen
    *APB_SARADC_ONETIME_SAMPLE_REG &= ~(0b1 << 29);                         // One-Time Sampling stoppen
    while(!(*APB_SARADC_INT_RAW_REG & (0b1 << 31) >> 31)){}                 // Warten bis Konvertierung abgeschlossen
  
    return (*APB_SARADC_1_DATA_STATUS_REG &= (0xFFF));                      // Digitalwert auslesen
}



void fill_pixelsArray(int distance, int maxBrightness)                      // Bestimmung der Helligkeitswerte jedes Pixels und eintragen in pixels-Array
{
    int range = MAX_DISTANCE - MIN_DISTANCE;

    int Red_Value = ((distance * 100 / MAX_DISTANCE) * maxBrightness / 100);    // Roten Farbwert je nach Distanz mit maximaler Helligkeit berechnen
    int Green_Value = maxBrightness - ((distance * 100 / MAX_DISTANCE) * maxBrightness / 100); // Grünen Farbwert je nach Distanz mit maximaler Helligkeit berechnen
    int Blue_Value = 0;                                                     // Blaue Pixel bleiben aus


    for (int i = 0; i < PIXEL_ARRAY_SIZE; i++) 
    {
        pixels[i] = 0;                                                      // Initialisiere alle Pixel auf 0
    }

    if (distance > MAX_DISTANCE)                                            // Bei d > MAX_DISTANCE bleiben alle Pixel aus
    {
        return;
    } 
    else if (distance <= MIN_DISTANCE)                                      // Bei d <= MIN_DISTANCE werden alle roten Pixel maximal hell
    {  
        for (int i = 0; i < PIXEL_ARRAY_SIZE; i+=3) 
        {
            pixels[i] = 0;
            pixels[i + 1] = maxBrightness;
            pixels[i + 2] = 0;
        }
    }
    else                                                                    // Bei MIN_DISTANCE < d <= MAX_DISTANCE: Berechnung der Pixel der aktiven LEDs
    {
        int scaledDistance = (distance - MIN_DISTANCE) * maxBrightness / range;                     // Skaliere distance auf 0 bis maxBrightness
        int fullActivePixels = (maxBrightness - scaledDistance) * LEDS / maxBrightness;             // Anzahl vollständiger LEDs
        int partActivePixelsBrightness = (maxBrightness - scaledDistance) * LEDS % maxBrightness;   // Wert der letzten LED

        for (int i = 0; i < fullActivePixels * PIXEL_PER_LED; i += 3)       // Farbwerte aller Pixel der vollständigen LEDs setzen
        {
            pixels[i] = Red_Value;
            pixels[i + 1] = Green_Value;
            pixels[i + 2] = Blue_Value;
        }

        if (fullActivePixels < LEDS)                                        // Sind nicht alle LEDs vollständig aktiv, wird die letzte LED gedimmt
        {
            int partActivePixelsBrightnessValue;                            // Variable für gedimmte Helligkeit der letzten LED
            if (partActivePixelsBrightness > maxBrightness)                 // Abfangen zu großer Helligkeitswerte
            {
                partActivePixelsBrightnessValue = maxBrightness;
            }
            else                                                            // Helligkeitswert der letzten LED setzen
            {
                partActivePixelsBrightnessValue = partActivePixelsBrightness;
            }

            Red_Value = ((distance * 100 / MAX_DISTANCE) * partActivePixelsBrightnessValue / 100); // Roten Farbwert je nach Distanz mit gedimmter Helligkeit berechnen
            Green_Value = partActivePixelsBrightnessValue - ((distance * 100 / MAX_DISTANCE) * partActivePixelsBrightnessValue / 100); // Grünen Farbwert je nach Distanz mit gedimmter Helligkeit berechnen
            Blue_Value = 0;                                                 // Blaue Pixel bleiben aus
            pixels[fullActivePixels * PIXEL_PER_LED] = Red_Value;       
            pixels[fullActivePixels * PIXEL_PER_LED + 1] = Green_Value;     // Farbwerte der letzten LED setzen
            pixels[fullActivePixels * PIXEL_PER_LED + 2] = Blue_Value;
        }
    }
}

//----------MAIN----------//
void app_main(void)
{
    gpio_init();
    adc_init();
    timer_init();

    while (1) 
    {
        int adcValue = adc_read();                                          // ADC Wert (0...4095)
        int maxPixelBrightness = (adcValue * 255) / 4095;                   // Helligkeitswert der Pixel (0...255)
        int averageDistanceCorrected = calc_averageDistanceCorrected();     // Korigierter Mittelwert der Distanz
        fill_pixelsArray(averageDistanceCorrected, maxPixelBrightness);     // Pixels-Array mit Helligkeitswerten belegen
    
        display_pixelsArray_LEDstripe();                                    // Pixels-Array auf LED-Streifen ausgeben
        display_pixelsArray_onboardLED();                                   // LED-Farbwerte auf Onboard-LED ausgeben
        delay_ns(100000);                                                   // 100us warten, um Programm zu entschleunigen
        
        
        for (int i = 0; i < PIXEL_ARRAY_SIZE; i++)                          // DEBUGGING: Ausgabe aller Pixelwerte nach LEDs gruppiert
        {
            printf("%d ", pixels[i]);
            if ((i + 1) % PIXEL_PER_LED == 0) printf("| "); // Gruppierung in LEDs
        }
        printf("\n");
        
    }
}

/*
void delay_us(unsigned int xus)             // Delay in Mikrosekunden
{
  asm volatile
  (
    //----Berechnung der Takteanzahl = xus / 43ns----
    "li t1, 43                      \n\t"   // t1 = 43
    "li t2, 1000                    \n\t"   // t2 = 1000
    "mul t2, %0, t2                 \n\t"   // t2 = us * 1000 -> Zeit in ns
    "div t2, t2, t1                 \n\t"   // t2 = (us * 1000) / 43 -> Takte
    "addi t2, t2, -6                \n\t"   // t2 = Takte - 6 -> nop-Takte ohne Initialisierung
    "addi t0, t2, 0                 \n\t"   // Lade die Anzahl der Takte in t0
    //----NOP-Schleife----
    "1:                             \n\t"
    "nop                            \n\t"
    "addi t0, t0, -1                \n\t"   // t0--
    "bnez t0, 1b                    \n\t"   // while(t0 != 0) -> 1:
    //----Parameter----
    :                                       // Ausgabe:
    : "r" (xus)                             // Eingabe: Mikrosekunden in a0
    : "t0", "t1", "t2"                      // Clobber-Register
  );
}

void write_zero()
{
  asm volatile
  (
    // 1 schreiben
    "li t1, 4					    \n\t"   // t1 = 2
    "lw t2, %0					    \n\t"	// t2 = GPIO_OUT_REG
    "or t3, t2, t1				    \n\t"	// GPIO2 setzen
    "sw t3, %0					    \n\t"	// GPIO2 = 1 rausschreiben

    // 300ns warten
    delay_ns(300);

    // 0 schreiben
    "li t1, 4					    \n\t"   // t1 = 2 == 0b0000 0010
    "lw t2, %0					    \n\t"	// t2 = GPIO_OUT_REG
    "not t1, t1					    \n\t"	// ~0b1111 1101
    "and t3, t2, t1				    \n\t"	// Verunden
    "sw t3, %0					    \n\t"	// GPIO2 = 0 rausschreiben

    // 950ns warten
    delay_ns(950);

    :
    : "m" (*GPIO_OUT_REG)
    : "t0", "t1", "t2", "t3"
  );
}

void write_one()
{
  asm volatile
  (
    // 1 schreiben
    "li t1, 4					    \n\t"	//2 laden
    "lw t2, %0					    \n\t"	//GPIO_OUT Adresse laden
    "or t3, t2, t1				    \n\t"	//Bit 2 setzen(GPIO2)
    "sw t3, %0				        \n\t"	//Adresse rausschreiben

    // 96 Takte = 600ns warten
    "addi t0, zero, 14              \n\t"   // t3 = 96 (NOPs für 600ns)
    "1:                             \n\t"   // NOP-Schleife für 600ns
    "nop                            \n\t"   // NOP
    "addi t0, t0, -1                \n\t"   // Decrement t3
    "bnez t0, 1b                    \n\t"   // Wiederhole, solange t3 != 0

    // 0 schreiben
    "li t1, 4					    \n\t"   //2 laden
    "lw t2, %0					    \n\t"   //GPIO_OUT Adresse laden
    "not t1, t1					    \n\t"	// Tilde
    "and t3, t2, t1				    \n\t"	// Verunden
    "sw t3, %0				        \n\t"	//Adresse rausschreiben

    // 104 Takte = 650ns warten
    "addi t0, zero, 15              \n\t"   // t3 = 104 (NOPs für 650ns)
    "2:                             \n\t"   // NOP-Schleife für 650ns
    "nop                            \n\t"   // NOP
    "addi t0, t0, -1                \n\t"   // Decrement t3
    "bnez t0, 2b"          		            // Wiederhole, solange t3 != 0

    :
    : "m" (*GPIO_OUT_REG)
    : "t0", "t1", "t2", "t3"
  );
}
*/