#include <curses.h>
#include <math.h>
#include "balance.h"

#define pi M_PI

#define NB               0
#define NM               1
#define NS               2
#define ZE               3
#define PS               4 
#define PM               5
#define PB               6

#define NRANGES 7

static double b_alpha_l[NRANGES],b_alpha_u[NRANGES];
static double b_alphaDot_l[NRANGES],b_alphaDot_u[NRANGES];
static int    b_rule_alpha[NRANGES];
static int    b_rule_alphaDot[NRANGES];
static int    b_rule_a[NRANGES];
static char  *b_bez[NRANGES];

/*************************************************************************/
void balance_init(void)
{
    int i;

    /* Grenzen fuer Alpha */
    b_alpha_l[NB] = -30.0;   b_alpha_u[NB] =  30.0;
    b_alpha_l[NM] =   0.0;   b_alpha_u[NM] =  60.0;
    b_alpha_l[NS] =  30.0;   b_alpha_u[NS] =  90.0;
    b_alpha_l[ZE] =  60.0;   b_alpha_u[ZE] = 120.0;
    b_alpha_l[PS] =  90.0;   b_alpha_u[PS] = 150.0;
    b_alpha_l[PM] = 120.0;   b_alpha_u[PM] = 180.0;
    b_alpha_l[PB] = 150.0;   b_alpha_u[PB] = 210.0;

    for(i=0;i<NRANGES;i++){
	b_alpha_l[i] = b_alpha_l[i]*pi/180.0;
	b_alpha_u[i] = b_alpha_u[i]*pi/180.0;
    }

    /* Grenzen fuer alphaDot */
    b_alphaDot_l[NB] = -5.0;   b_alphaDot_u[NB] = -1.0;
    b_alphaDot_l[NM] = -3.0;   b_alphaDot_u[NM] = -1.0;
    b_alphaDot_l[NS] = -2.0;   b_alphaDot_u[NS] =  1.0;
    b_alphaDot_l[ZE] = -0.5;   b_alphaDot_u[ZE] =  0.5;
    b_alphaDot_l[PS] = -1.0;   b_alphaDot_u[PS] =  2.0;
    b_alphaDot_l[PM] =  1.0;   b_alphaDot_u[PM] =  3.0;
    b_alphaDot_l[PB] =  1.0;   b_alphaDot_u[PB] =  5.0;

    /* Regeln definieren */
    b_rule_alpha[0]=PM; b_rule_alphaDot[0]=ZE; b_rule_a[0]=PM;
    b_rule_alpha[1]=PS; b_rule_alphaDot[1]=PS; b_rule_a[1]=PS;
    b_rule_alpha[2]=PS; b_rule_alphaDot[2]=NS; b_rule_a[2]=ZE;
    b_rule_alpha[3]=NM; b_rule_alphaDot[3]=ZE; b_rule_a[3]=NM;
    b_rule_alpha[4]=NS; b_rule_alphaDot[4]=NS; b_rule_a[4]=NS;
    b_rule_alpha[5]=NS; b_rule_alphaDot[5]=PS; b_rule_a[5]=ZE;
    b_rule_alpha[6]=ZE; b_rule_alphaDot[6]=ZE; b_rule_a[6]=ZE;

    /* Bezeichnungen fuer Indizes */
    b_bez[NB] = "NB";
    b_bez[NM] = "NM";
    b_bez[NS] = "NS";
    b_bez[ZE] = "ZE";
    b_bez[PS] = "PS";
    b_bez[PM] = "PM";
    b_bez[PB] = "PB";
}

/*************************************************************************/
double balance(double alpha, double alphaDot)
{
    double b_alpha[NRANGES];
    double b_alphaDot[NRANGES];
    double b_Fa[NRANGES];
    double max,mid;
    int i;

    /* Gewichte von alpha ausrechnen */
    for(i=0;i<NRANGES;i++){
	if((b_alpha_u[i] > alpha) && (alpha > b_alpha_l[i])){
	    mid = (b_alpha_u[i]+b_alpha_l[i])/2.0;
	    if(alpha>mid){
		/* linke Haelfte */
		b_alpha[i] = 1.0-(alpha-mid)/(b_alpha_u[i]-mid);
	    }else{
		/* rechte Haelfte */
		b_alpha[i] = (alpha-b_alpha_l[i])/(mid-b_alpha_l[i]);
	    }
	}else{
	    b_alpha[i] = 0.0;
	}
    }

    /* Gewichte von alphaDot ausrechnen */
    for(i=0;i<NRANGES;i++){
	if((b_alphaDot_u[i] > alphaDot) && (alphaDot > b_alphaDot_l[i])){
	    mid = (b_alphaDot_u[i]+b_alphaDot_l[i])/2.0;
	    if(alphaDot>mid){
		/* rechte Haelfte */
		b_alphaDot[i] = 1.0-(alphaDot-mid)/(b_alphaDot_u[i]-mid);
	    }else{
		/* linke Haelfte */
		b_alphaDot[i] = (alphaDot-b_alphaDot_l[i])/(mid-b_alphaDot_l[i]);
	    }
	}else{
	    b_alphaDot[i] = 0.0;
	}
    }

    /* Regeln anwenden */
    for(i=0;i<NRANGES;i++){
	b_Fa[i] = 0.0;
    }
    for(i=0;i<NRANGES;i++){
	b_Fa[b_rule_a[i]] = Max(b_Fa[b_rule_a[i]],
				Min(b_alpha[b_rule_alpha[i]],
				    b_alphaDot[b_rule_alphaDot[i]]));
    }

    /* Mittelwert ausrechnen */
    max=0.0;
    for(i=0;i<NRANGES;i++){
	max =max + (i-(NRANGES/2))*b_Fa[i];
    }

    /* alpha ausgeben */
    gotoxy(58,3); printw("alpha:");
    for(i=0;i<NRANGES;i++){
	gotoxy(58,4+i);
	printw("%s: %5.3f",b_bez[i],b_alpha[i]);
    };
    
    /* alphaDot ausgeben */
    gotoxy(70,3); printw("alphaDot:");
    for(i=0;i<NRANGES;i++){
	gotoxy(70,4+i);
	printw("%s: %5.3f",b_bez[i],b_alphaDot[i]);
    };
    
    /* b_Fa ausgeben */
    gotoxy(65,13); printw("b_Fa:");
    for(i=0;i<NRANGES;i++){
	gotoxy(65,14+i);
	printw("%s: %5.3f",b_bez[i],b_Fa[i]);
    };
    
    /* max ausgeben */
    gotoxy(65,22);
    printw("max=%6.3f",max);

    refresh();

    return -max;
}