//
// qtpendel-data.cc
//
//  - Hubert Feyrer <hubert@feyrer.de>
// 

#include <stdio.h>
#include <assert.h>

#include "qtpendel-data.h"

#define Max(x,y) (((x)>(y))?(x):(y))
#define Min(x,y) (((x)<(y))?(x):(y))    

/*************************************************************************/
QtPendelData::QtPendelData()
{
	int i;

	/* Regeln: wenn ... und ... dann ... */
	static Rule myrule[] = { // HACK!
		Rule( PM, ZE, PM ),
		Rule( PS, PS, PM ),
		Rule( PS, NS, ZE ),
		Rule( NM, ZE, NM ),
		Rule( NS, NS, NM ),
		Rule( NS, PS, ZE ),
		Rule( ZE, ZE, ZE ),
	};
	rule = myrule;
	nrules = sizeof(myrule) / sizeof(myrule[0]);
	
	static Bound myalpha_bounds[] = {
		{ -30.0,  30.0 },           /* /\             */
		{   0.0,  60.0 },           /*   /\           */
		{  30.0,  90.0 },           /*     /\         */
		{  60.0, 120.0 },           /*       /\       */
		{  90.0, 150.0 },           /*         /\     */
		{ 120.0, 180.0 },           /*           /\   */
		{ 150.0, 210.0 }            /*             /\ */
	};
	assert((sizeof(myalpha_bounds) / sizeof(myalpha_bounds[0])) == NRANGES);
	alpha.bounds = myalpha_bounds; // HACK!
	alpha.maxbounds.lower = 0;
	alpha.maxbounds.upper = 0;
	for(i=0; i < NRANGES; i++) {
		alpha.bounds[i].lower = alpha.bounds[i].lower / 180.0 * pi;
		alpha.bounds[i].upper = alpha.bounds[i].upper / 180.0 * pi;
		
		alpha.maxbounds.lower = Min(alpha.maxbounds.lower,
					    alpha.bounds[i].lower);
		alpha.maxbounds.upper = Max(alpha.maxbounds.upper,
					    alpha.bounds[i].upper);
	}

	static Bound myalphaDot_bounds[] = {
		{ -5.0, -1.0 },             /* /\             */
		{ -3.0, -1.0 },             /*   /\           */
		{ -2.0,  1.0 },             /*     /\         */
		{ -0.5,  0.5 },             /*       /\       */
		{ -1.0,  2.0 },             /*         /\     */
		{  1.0,  3.0 },             /*           /\   */
		{  1.0,  5.0 },             /*             /\ */
	};
	assert(sizeof(myalphaDot_bounds)/sizeof(myalphaDot_bounds[0]) == NRANGES);
	alphaDot.bounds = myalphaDot_bounds; // HACK!
	alphaDot.maxbounds.lower = 0;
	alphaDot.maxbounds.upper = 0;
	for(i=0; i < NRANGES; i++) {
		alphaDot.maxbounds.lower = Min(alphaDot.bounds[i].lower,
					       alphaDot.maxbounds.lower);
		alphaDot.maxbounds.upper = Max(alphaDot.bounds[i].upper,
					       alphaDot.maxbounds.upper);
	}

	static Bound mya_bounds[] = {
		{ -7.0, -5.0 },             /* /\             */
		{ -5.0, -3.0 },             /*   /\           */
		{ -3.0, -1.0 },             /*     /\         */
		{ -1.0,  1.0 },             /*       /\       */
		{  1.0,  3.0 },             /*         /\     */
		{  3.0,  5.0 },             /*           /\   */
		{  5.0,  7.0 },             /*             /\ */		
	};
	assert(sizeof(mya_bounds)/sizeof(mya_bounds[0]) == NRANGES);
	a.bounds = mya_bounds; // HACK!
	a.maxbounds.lower = 0;
	a.maxbounds.upper = 0;
	for(i=0; i < NRANGES; i++) {
		a.maxbounds.lower = Min(a.maxbounds.lower,
					a.bounds[i].lower);
		a.maxbounds.upper = Max(a.maxbounds.upper,
					a.bounds[i].upper);
	}

	alpha.initialvalue = 75.0*(pi/180.0);	// Anfangswinkel
	alphaDot.initialvalue = 0.0;		// Anfangsgeschw.
	a.initialvalue = 15.0;			// Anfangsbeschl.
	xpos=0.0;
}

/*************************************************************************/
double QtPendelData::balance()
{
//    double res_alpha[NRANGES];
//    double res_alphaDot[NRANGES];
//    double res_a[NRANGES];
    double max,mid;
    int i;

    /* Gewichte von alpha ausrechnen */
    for(i=0; i<NRANGES; i++){
        if ((alpha.bounds[i].upper > alpha.currentvalue)
	    && (alpha.currentvalue > alpha.bounds[i].lower )){
		mid = (alpha.bounds[i].upper + alpha.bounds[i].lower)/2.0;
		if(alpha.currentvalue > mid){
			/* rechte Haelfte */
			alpha.rangevalues[i] = 1.0 - (alpha.currentvalue - mid)
				/ (alpha.bounds[i].upper-mid);
		}else{
			/* linke Haelfte */
			alpha.rangevalues[i] = (alpha.currentvalue - alpha.bounds[i].lower)
				/ (mid-alpha.bounds[i].lower); 
		}
        }else{
		alpha.rangevalues[i] = 0.0;
        }
    }

    /* Gewichte von alphaDot ausrechnen */
    for(i=0; i<NRANGES; i++){
	    if((alphaDot.bounds[i].upper > alphaDot.currentvalue)
	       && (alphaDot.currentvalue > alphaDot.bounds[i].lower)){
		    mid = (alphaDot.bounds[i].upper + alphaDot.bounds[i].lower)/2.0;
		    if(alphaDot.currentvalue > mid){
			    /* rechte Haelfte */
			    alphaDot.rangevalues[i] = 1.0-(alphaDot.currentvalue - mid)
				    / (alphaDot.bounds[i].upper - mid);
		    }else{
			    /* linke Haelfte */
			    alphaDot.rangevalues[i] = (alphaDot.currentvalue - alphaDot.bounds[i].lower)
				    / (mid - alphaDot.bounds[i].lower);
		    }
	    }else{
		    alphaDot.rangevalues[i] = 0.0;
	    }
    }

    /* Regeln anwenden */
    for(i=0; i<NRANGES; i++){
	    a.rangevalues[i] = 0.0;
    }
    for(i=0; i<nrules; i++){
	    a.rangevalues[rule[i].a] = Max(a.rangevalues[rule[i].a],
					   Min(alpha.rangevalues[rule[i].alpha],
					       alphaDot.rangevalues[rule[i].alphaDot]));
    }

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

    a.currentvalue = -max;
    return -max;
}

/*************************************************************************/
void QtPendelData::init(void) // und reset
{
	alpha.currentvalue = alpha.initialvalue;
	alphaDot.currentvalue = alphaDot.initialvalue;
	a.currentvalue = a.initialvalue;

	Fg = m * g;
}

/*************************************************************************/
void QtPendelData::step(void)
{
	/* f2: Ausgleichen der Pendelbewegung */
	double ah = balance(/* alpha, alphaDot */);
	Fa = m * ah * a.initialvalue;

	/* f1: Pendel-Verhalten nach Ch. Ziegaus */
	Fres       = sqrt(Fg*Fg + Fa*Fa);
	rho        = alpha.currentvalue - acos(Fa / Fres);
	Fz         = sin(rho) * Fres;
	deltaAlpha = Fz / (2.0 * m * l) * deltaT*deltaT + alphaDot.currentvalue * deltaT;
	alphaDot.currentvalue   = Fz / (m * l) * deltaT + alphaDot.currentvalue;
	alpha.currentvalue      = alpha.currentvalue + deltaAlpha;
	xpos        = xpos - 0.5 * ah * deltaT*deltaT  * 10000;       // letzter Faktor: Skalierung
	
	// Sanity checks
	if (alpha.currentvalue < 0)
		alpha.currentvalue = 0;
	if (alpha.currentvalue > pi)
		alpha.currentvalue = pi;
}

/*************************************************************************/
void QtPendelData::bumpleft(void)
{
	alphaDot.currentvalue = alphaDot.currentvalue - bumper_alphaDot;
	alpha.currentvalue    = alpha.currentvalue    - bumper_alpha;
}

/*************************************************************************/
void QtPendelData::bumpright(void)
{
	alphaDot.currentvalue = alphaDot.currentvalue + bumper_alphaDot;
	alpha.currentvalue    = alpha.currentvalue    + bumper_alpha;
}

/*************************************************************************/
int QtPendelData::isOk(void)
{
	if (alpha.currentvalue > 0.0 && alpha.currentvalue < pi) {
		return 1;
	} else {
		return 0;
	}
}