#include <semaphore.h>
#include <wiringPi.h>
#include <sys/types.h>
#include <sys/wait.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include <pthread.h>

sem_t S1, S2, S3, S4, S5;

void *function1(){
    while(1){
        if(digitalRead(23)==1){
//printf("Funktion 1 wird ausgeführt\n");

sem_post(&S1);  // -> 1     Thread 2 kann starten
sem_wait(&S2);  // -> 0     Warte auf Thread 3
sem_wait(&S4);  // -> 0     Warte auf Thread 4

}
}
}


void *function2(double * dist){             // Thread 2 Funktion (Bewegungsmelder)
    struct timeval start, ende;
    long sec, usec;
    double time;

    while(1){                   // Endlosschleife
        sem_wait(&S1);              // <- 0  warte dass Thread 1 freigibt
        //printf("Funktion 2 wird ausgeführt\n");

        // Code war gegeben, aus Unterlagen
        digitalWrite(22,1);
        delay(10);
        digitalWrite(22,0);


        if(gettimeofday(&start,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }

        while(digitalRead(21) == 0){
            if (gettimeofday(&start,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }
        }

        while(digitalRead(21) == 1){
            if(gettimeofday(&ende,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }
        }

        sec=ende.tv_sec-start.tv_sec;
        usec=ende.tv_usec-start.tv_usec;    // Mikrosekunden -> 1/1000000stel

        // printf("sec:%ld\n", sec);
        printf("usec:%ld\n", usec);
        time = usec;
        time = (time / 1000000) + sec;
        printf("Time:%f\n", time);
        * dist = (time*34300)/2;        // Distanz in cm
        printf("Dist:%f\n", *dist);

        sem_post(&S3);              // -> 1  Thread 3 freigeben
        sem_post(&S5);              // -> 1  Thread 4 freigeben
    }

}


void *function3(double * dist){             // Thread 3 Funktion (LEDs)
    int i;
    int gpionr[3] = {0,2,3};
    while(1){
        for(i=0; i < 3; i++){
            digitalWrite(gpionr[i], 0);
        }

        sem_wait(&S3);              // <- 0 Warte das Thread 2 freigibt

        //printf("Funktion 3 wird ausgeführt\n");

        if(*dist < 10){             // Unter 10cm -> Rote LED
            digitalWrite(0, 1);
                printf("\nRote LED");
                printf("\n### BUZZER ###\n\n");
            delay(50);
        }else if(*dist > 10 && *dist < 20){ // Zwischen 10cm und 20Cm -> Gelbe LED
            digitalWrite(3, 1);
            printf("\nGelbe LED\n\n");
            delay(50);
        }else{                  // Bei mehr als 20cm -> Grüne LED
            digitalWrite(2, 1);
            printf("\nGrüne LED\n\n");
            delay(50);
        }
        sem_post(&S2);              // -> 1 Gebe Thread 1 wieder frei (50%) 1/2
    }
}


void *function4(double * dist){             // Thread 4 Funktion (Buzzer)
    while(1){
        sem_wait(&S5);              // -> 0     Warte das Thread 2 freigibt
        //printf("Funktion 4 wird ausgeführt\n");

        if(*dist < 10){             // Wenn Distanz kleiner als 10cm, wird ein Ton ausgegeben
            softToneWrite(25, 10);      // Ton ausgeben
            delay(300);         // Warten
            softToneWrite(25, 0);       // Ton auschalten
        }
    sem_post(&S4);                  // -> 1     gebe Thread 1 wieder frei (50%) 2/2
    }
}
// Main
int main(void){                     // Main Thread
    int sem_value;
    double _dist;
    sem_init(&S1, 0, 0); //0   sem_init(&Name/Adr. , Teilen zwischen Threads , Initialisierung(0))
    sem_init(&S2, 0, 0); //0
    sem_init(&S3, 0, 0); //0
    sem_init(&S4, 0, 0); //0
    sem_init(&S5, 0, 0); //0

    wiringPiSetup();                // WiringPiSetup
    int gpionr[5] = {0,2,3,22,25};          // Pin Belegung
    int i;

    for(i=0; i < 5; i++){               // Pinmode für alle pins auf Output stellen
        pinMode(gpionr[i], 1);
    }
    pullUpDnControl(21,PUD_OFF);            // Ultraschallsensor (Echo)
    pullUpDnControl(22,PUD_OFF);            // Ultraschallsensor (Trig)
    pullUpDnControl(23,PUD_OFF);            // Bewegungssensor
    softToneCreate(25);             // Buzzer soll genutzt werden

    // Threads erstellen und initialisieren
    pthread_t Thread1, Thread2, Thread3, Thread4;
    pthread_create (&Thread1, NULL, (void*) function1, NULL);
    pthread_create (&Thread2, NULL, (void*) function2, (void *) &_dist);
    pthread_create (&Thread3, NULL, (void*) function3, (void *) &_dist);
    pthread_create (&Thread4, NULL, (void*) function4, (void *) &_dist);

    sleep(15);                  // Schlafe 15 Sekunden, "Dauer wie lange das Programm läuft"

    // Threads beenden
    pthread_cancel(Thread1);
    pthread_cancel(Thread2);
    pthread_cancel(Thread3);
    pthread_cancel(Thread4);

    int gpioreset[6] = {0,2,3,21,22,23};
    for(i=0; i < 6; i++){               // Deaktivieren der LEDs und Sensoren
        digitalWrite(gpioreset[i], 0);
    }
    softToneWrite(25,0);                // Deaktivieren des Buzzers

    // Semaphoren schliessen
    sem_destroy(&S1);
    sem_destroy(&S2);
    sem_destroy(&S3);
    sem_destroy(&S4);
    sem_destroy(&S5);

    return 1;
}

// Funktionen

void *function1(){
    while(1){
        if(digitalRead(23)==1){
//printf("Funktion 1 wird ausgeführt\n");

sem_post(&S1);  // -> 1     Thread 2 kann starten
sem_wait(&S2);  // -> 0     Warte auf Thread 3
sem_wait(&S4);  // -> 0     Warte auf Thread 4

}
}
}


void *function2(double * dist){             // Thread 2 Funktion (Bewegungsmelder)
    struct timeval start, ende;
    long sec, usec;
    double time;

    while(1){                   // Endlosschleife
        sem_wait(&S1);              // <- 0  warte dass Thread 1 freigibt
        //printf("Funktion 2 wird ausgeführt\n");

        // Code war gegeben, aus Unterlagen
        digitalWrite(22,1);
        delay(10);
        digitalWrite(22,0);


        if(gettimeofday(&start,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }

        while(digitalRead(21) == 0){
            if (gettimeofday(&start,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }
        }

        while(digitalRead(21) == 1){
            if(gettimeofday(&ende,(struct timezone*)0)){
            printf("error\n");
            exit(1);
            }
        }

        sec=ende.tv_sec-start.tv_sec;
        usec=ende.tv_usec-start.tv_usec;    // Mikrosekunden -> 1/1000000stel

        // printf("sec:%ld\n", sec);
        printf("usec:%ld\n", usec);
        time = usec;
        time = (time / 1000000) + sec;
        printf("Time:%f\n", time);
        * dist = (time*34300)/2;        // Distanz in cm
        printf("Dist:%f\n", *dist);

        sem_post(&S3);              // -> 1  Thread 3 freigeben
        sem_post(&S5);              // -> 1  Thread 4 freigeben
    }

}


void *function3(double * dist){             // Thread 3 Funktion (LEDs)
    int i;
    int gpionr[3] = {0,2,3};
    while(1){
        for(i=0; i < 3; i++){
            digitalWrite(gpionr[i], 0);
        }

        sem_wait(&S3);              // <- 0 Warte das Thread 2 freigibt

        //printf("Funktion 3 wird ausgeführt\n");

        if(*dist < 10){             // Unter 10cm -> Rote LED
            digitalWrite(0, 1);
                printf("\nRote LED");
                printf("\n### BUZZER ###\n\n");
            delay(50);
        }else if(*dist > 10 && *dist < 20){ // Zwischen 10cm und 20Cm -> Gelbe LED
            digitalWrite(3, 1);
            printf("\nGelbe LED\n\n");
            delay(50);
        }else{                  // Bei mehr als 20cm -> Grüne LED
            digitalWrite(2, 1);
            printf("\nGrüne LED\n\n");
            delay(50);
        }
        sem_post(&S2);              // -> 1 Gebe Thread 1 wieder frei (50%) 1/2
    }
}


void *function4(double * dist){             // Thread 4 Funktion (Buzzer)
    while(1){
        sem_wait(&S5);              // -> 0     Warte das Thread 2 freigibt
        //printf("Funktion 4 wird ausgeführt\n");

        if(*dist < 10){             // Wenn Distanz kleiner als 10cm, wird ein Ton ausgegeben
            softToneWrite(25, 10);      // Ton ausgeben
            delay(300);         // Warten
            softToneWrite(25, 0);       // Ton auschalten
        }
    sem_post(&S4);                  // -> 1     gebe Thread 1 wieder frei (50%) 2/2
    }
}