//GRUPPE Ilia- David && Andreas Wolf 
//Datum 31.12.2005 23:59:59
//Blatt 4 Aufgabe 4.1 (a,b,c,d,!e)

//Aufgabe 4.1(b)
//
// Hier_drüber_schwingen = -(L1*cos(theta_1)+(L2*cos(theta_1+theta2)))
// wobei L1 = L2 = 1

#include "acrobot.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#define NVARS 4 
#define NREGELN 6
#define IRRELEVANT -1
#define NSCHRITTE 100
#define NKNOTEN 3

#define RECHTS 2
#define MITTE 1
#define LINKS 0

// ===================================================================
//                  V A R I A B L E N
// ===================================================================
typedef struct variable{
    int nknoten;
    double start, ende ,delta;
} variable;

variable varmenge[NVARS];
variable var_u;

// ===================================================================
//                  R E G E L N
// ===================================================================
typedef struct regel{
    int praemisse[NVARS];
    int konklusion;
} regel;

regel regelmenge[NREGELN];

// ===================================================================
//                  F U N K T I O N E N
// ===================================================================

//Berechnet die Zugehoerigkeitsfunktion (berechne_erfuellungsgrad_pro_variable)
double zugehoerigkeitsfunktion(int regel, int variable, double wert){
    int varindex;
    varindex=regelmenge[regel].praemisse[variable];
    if(varindex==IRRELEVANT) {return 0;}
    double delta;
    delta=varmenge[variable].delta;
    double xi;
    xi=varmenge[variable].start+delta*varindex;
                
    //Abbruchbedingung
    if(varindex<0 || varindex>varmenge[variable].nknoten){
        printf("\n Fehler in Funktion \"berechne_erfuellungsgrad_pro_variable\"!\n");
        return 0;
    }
    
    //links:
    if((wert>=(xi-delta)) && (wert<=xi)){
        return ((wert-(xi-delta))/delta);
    }
    //rechts:
    if((wert>xi) && (wert<=(xi+delta))){
        return ((xi+delta-wert)/delta);        
    }
    
    return 0;   
}

//Liefert das Minimum (UND-Verknüpfung) aller Zugehörigkeitsfunktionen
double erfuellung_pro_regel(int regel, double *zustand){
    int i;
    double min=1, temp;
    
    for(i=0; i<NVARS; i++){
        if(regelmenge[regel].praemisse[i] !=IRRELEVANT){
                temp=zugehoerigkeitsfunktion(regel, i, zustand[i]);
                if(temp<min){
                min=temp;
                }
        }
    }
    return min;
}

double fuzzy_steuerung(double *zustand){
    int i;
    double flaeche, zaehler=0, nenner=0, xi;
    
    //Formel s. Aufgabenblatt
    for(i=0; i<NREGELN; i++){
        flaeche=erfuellung_pro_regel(i, zustand);
        xi=var_u.start+var_u.delta*regelmenge[i].konklusion;
        zaehler+=flaeche*xi;
        nenner+=flaeche;
    }
    
    if(nenner==0){
        return 1;
    }
    return zaehler/nenner;    
}

// **********************************************************************
// *                M A I N                                             * 
// **********************************************************************
int main(){
    int i, j, z;
    double s[NVARS], ss[NVARS], u;
    double t=0;
     FILE *stream;
    
    //Initialisierung:
    for(i=0; i<NVARS; i++){
        s[i]=0;
    }
        
// ===================================================================
//                  D e f i n i t i o n   d e r   V a r i a b l e n 
// ===================================================================
    
    //Ausgangsvariable u:
    var_u.start=-1;
    var_u.ende=1;
    var_u.nknoten=3; //u hat die Mengen "nach hinten" (-1), "mitte" (0) und "nach vorne" (1);
    var_u.delta=1;
    
     //Die Grenzen entsprechen den Vorgaben des Aufgabenblattes
    varmenge[0].start=varmenge[1].start=-M_PI;
    varmenge[0].ende=varmenge[1].ende=M_PI;
    varmenge[2].start=-4*M_PI;
    varmenge[2].ende=4*M_PI;
    varmenge[3].start=-9*M_PI;
    varmenge[3].ende=9*M_PI; 
     
    varmenge[0].delta=varmenge[1].delta=M_PI;
    varmenge[2].delta=4*M_PI;
    varmenge[3].delta=9*M_PI; 
    for(i=0; i<NVARS; i++){
        varmenge[i].nknoten=NKNOTEN; //alle Variablen in 3 Mengen unterteilt: N, ZE, P
       // varmenge[i].delta=((-(varmenge[i].start)+varmenge[i].ende)/(varmenge[i].nknoten-1)); //delta errechnet sich für alle gleich
    }

// ===================================================================
//                  D e f i n i t i o n   d e r   R e g e l n
// ===================================================================
 
   //TESTREGELN für NKNOTEN = 3*****************************************
   //WENN theta_1 = LINKS UND theta_1_punkt = LINKS DANN KRAFT nach RECHTS...
   //Oberkörper ist links und er schwingt nach links
    regelmenge[0].praemisse[0]=LINKS;     
    regelmenge[0].praemisse[1]=IRRELEVANT;
    regelmenge[0].praemisse[2]=LINKS;        
    regelmenge[0].praemisse[3]=IRRELEVANT;     
    regelmenge[0].konklusion=RECHTS; 
    
    //Oberkörper ist rechts und schwingt nach rechts
    regelmenge[1].praemisse[0]=RECHTS;
    regelmenge[1].praemisse[1]=IRRELEVANT;
    regelmenge[1].praemisse[2]=RECHTS;
    regelmenge[1].praemisse[3]=IRRELEVANT;
    regelmenge[1].konklusion=LINKS;
    
    //Oberkörper hängt mittig und schwingt nach rechts
    regelmenge[2].praemisse[0]=MITTE;
    regelmenge[2].praemisse[1]=IRRELEVANT;
    regelmenge[2].praemisse[2]=RECHTS;
    regelmenge[2].praemisse[3]=IRRELEVANT;
    regelmenge[2].konklusion=LINKS;
    
    //Oberkörper hängt mittig und schwingt nach links
    regelmenge[3].praemisse[0]=MITTE;
    regelmenge[3].praemisse[1]=IRRELEVANT;
    regelmenge[3].praemisse[2]=LINKS;
    regelmenge[3].praemisse[3]=IRRELEVANT;
    regelmenge[3].konklusion=RECHTS;
    
    //Oberkörper mittig, Füße links und schwingt nicht
    regelmenge[4].praemisse[0]=MITTE;   
    regelmenge[4].praemisse[1]=LINKS;   
    regelmenge[4].praemisse[2]=IRRELEVANT;    
    regelmenge[4].praemisse[3]=LINKS;     
    regelmenge[4].konklusion=LINKS;  
    
    //Oberkörper mittig, Füße rechts und schwingt nicht
    regelmenge[5].praemisse[0]=MITTE;
    regelmenge[5].praemisse[1]=RECHTS;
    regelmenge[5].praemisse[2]=IRRELEVANT;
    regelmenge[5].praemisse[3]=RECHTS;
    regelmenge[5].konklusion=RECHTS;

    //n Schritte simulieren, und testen, ob Ziel erreicht
    stream=fopen("egal.txt","w+");
    z=0;
    for(i=0; i<NSCHRITTE; i++){
        //Abbruchbedingung:
       double a ;
       a = -(cos(s[0])+(cos(s[0]+s[2])));
       if(a >= 1) {
            printf("Das Ziel wurde in %d erreicht.\n", z);
            
            system("PAUSE");
            return 0;    
        }
        z++;
        u=fuzzy_steuerung(s);
        step(s, u, ss);
        printf("s=%.2f %.2f %.2f %.2f >> u=%.2f >> ss=%.2f %.2f %.2f %.2f \n", s[0], s[1], s[2], s[3], u, ss[0], ss[1], ss[2], ss[3]);
        t += 0.2;    
        fprintf(stream, "%.2f %.2f %.2f %.2f %.2f %.1f\n", s[0], s[1], s[2], s[3], u, t);
        // Neuer Zustand --> Alter Zustand
             for (j=0;j<NVARS;j++) s[j]=ss[j];
    }
    fclose(stream);

    printf("Das Ziel wurde nach %d Schritten nicht erreicht.\n", NSCHRITTE);
                
    system("PAUSE");
    return 0;
}