#include <simgear/debug/logstream.hxx>

#include "Math.hpp"
#include "Surface.hpp"
#include "Rotorpart.hpp"
#include "Ground.hpp"
#include "Rotor.hpp"

#include STL_IOSTREAM
#include STL_IOMANIP

SG_USING_STD(setprecision);

#ifdef TEST_DEBUG
#include <stdio.h>
#endif
#include <string.h>
#include <iostream>
#include <sstream>



namespace yasim {

const float pi=3.14159;

static inline float sqr(float a) { return a * a; }

Rotor::Rotor()
{
    int i;
    _alpha0=-.05;
    _alpha0factor=1;
    _alphamin=-.1;
    _alphamax= .1;
    _alphaoutput[0][0]=0;
    _alphaoutput[1][0]=0;
    _alphaoutput[2][0]=0;
    _alphaoutput[3][0]=0;
    _alphaoutput[4][0]=0;
    _alphaoutput[5][0]=0;
    _alphaoutput[6][0]=0;
    _alphaoutput[7][0]=0;
    _base[0] = _base[1] = _base[2] = 0;
    _ccw=0;
    _delta=1;
    _delta3=0;
    _diameter =10;
    _dynamic=1;
    _engineon=0;
    _force_at_pitch_a=0;
    _forward[0]=1;
    _forward[1]=_forward[2]=0;
    _max_pitch=13./180*pi;
    _maxcyclicail=10./180*pi;
    _maxcyclicele=10./180*pi;
    _maxteeterdamp=0;
    _mincyclicail=-10./180*pi;
    _mincyclicele=-10./180*pi;
    _min_pitch=-.5/180*pi;
    _cyclicele=0;
    _cyclicail=0;
    _name[0] = 0;
    _normal[0] = _normal[1] = 0;
    _normal[2] = 1;
    _normal_with_yaw_roll[0]= _normal_with_yaw_roll[1]=0;
    _normal_with_yaw_roll[2]=1;
    _number_of_blades=4;
    _omega=_omegan=_omegarel=_omegarelneu=0;
    _ddt_omega=0;
    _pitch_a=0;
    _pitch_b=0;
    _power_at_pitch_0=0;
    _power_at_pitch_b=0;
    _no_torque=0;
    _rel_blade_center=.7;
    _rel_len_hinge=0.01;
    _rellenteeterhinge=0.01;
    _rotor_rpm=442;
    _sim_blades=0;
    _teeterdamp=0.00001;
    _translift=0.05;
    _weight_per_blade=42;
    _translift_ve=20;
    _translift_maxfactor=1.3;
    _ground_effect_constant=0.1;
    _vortex_state_lift_factor=0.4;
    _vortex_state_c1=0.1;
    _vortex_state_c2=0;
    _vortex_state_c3=0;
    _vortex_state_e1=1;
    _vortex_state_e2=1;
    _vortex_state_e3=1;
    _vortex_state=0;
    _lift_factor=1;
    _liftcoef=0.1;
    _dragcoef0=0.1;
    _dragcoef1=0.1;
    _twist=0; 
    _number_of_segments=1;
    _number_of_parts=4;
    _rel_len_where_incidence_is_measured=0.7;
    _torque_of_inertia=1;
    _torque=0;
    _chord=0.3;
    _taper=1;
    _airfoil_incidence_no_lift=0;
    _rel_len_blade_start=0;
    _airfoil_lift_coefficient=0;
    _airfoil_drag_coefficient0=0;
    _airfoil_drag_coefficient1=0;
    for(i=0; i<2; i++)
        _global_ground[i] =  0;
    _global_ground[2] = 1;
    _global_ground[3] = -1e3;
    _incidence_stall_zero_speed=18*pi/180.;
    _incidence_stall_half_sonic_speed=14*pi/180.;
    _lift_factor_stall=0.28;
    _stall_change_over=2*pi/180.;
    _drag_factor_stall=8;
    _stall_sum=1;
    _stall_v2sum=1;
    _collective=0;
    _yaw=_roll=0;
    for (int k=0;k<4;k++)
        for (i=0;i<3;i++)
            _groundeffectpos[k][i]=0;
    _ground_effect_altitude=1;
    _cyclic_factor=1;
    _lift_factor=_f_ge=_f_vs=_f_tl=1;
    _rotor_correction_factor=.65;
}

Rotor::~Rotor()
{
    int i;
    for(i=0; i<_rotorparts.size(); i++) {
        Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
        delete r;
    }
}

void Rotor::inititeration(float dt,float omegarel,float ddt_omegarel,float *rot)
{
    _stall_sum=0;
    _stall_v2sum=0;
    _omegarel=omegarel;
    _omega=_omegan*_omegarel; 
    _ddt_omega=_omegan*ddt_omegarel;
    int i;
    for(i=0; i<_rotorparts.size(); i++) {
        float s = Math::sin(2*pi*i/_number_of_parts);
        float c = Math::cos(2*pi*i/_number_of_parts);
        Rotorpart* r = (Rotorpart*)_rotorparts.get(i);
        r->setOmega(_omega);
        r->setDdtOmega(_ddt_omega);
        r->inititeration(dt,rot);
        r->setCyclic(_cyclicail*c+_cyclicele*s);
    }

    //calculate the normal of the rotor disc, for calcualtion of the downwash
    float side[3],help[3];
    Math::cross3(_normal,_forward,side);
    Math::mul3(Math::cos(_yaw)*Math::cos(_roll),_normal,_normal_with_yaw_roll);

    Math::mul3(Math::sin(_yaw),_forward,help);
    Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);

    Math::mul3(Math::sin(_roll),side,help);
    Math::add3(_normal_with_yaw_roll,help,_normal_with_yaw_roll);
}

float Rotor::calcStall(float incidence,float speed)
{
    float stall_incidence=_incidence_stall_zero_speed
        +(_incidence_stall_half_sonic_speed
        -_incidence_stall_zero_speed)*speed/(343./2);
    //missing: Temeperature dependency of sonic speed
    incidence = Math::abs(incidence);
    if (incidence > (90./180.*pi))
        incidence = pi-incidence;
    float stall = (incidence-stall_incidence)/_stall_change_over;
    stall = Math::clamp(stall,0,1);

    _stall_sum+=stall*speed*speed;
    _stall_v2sum+=speed*speed;

    return stall;
}

float Rotor::getLiftCoef(float incidence,float speed)
{
    float stall=calcStall(incidence,speed);
    /* the next shold look like this, but this is the inner loop of
           the rotor simulation. For small angles (and we hav only small
           angles) the first order approximation works well
    float c1=  Math::sin(incidence-_airfoil_incidence_no_lift)*_liftcoef;
    for c2 we would need higher order, because in stall the angle can be large
    */
    float i2;
    if (incidence > (pi/2))
        i2 = incidence-pi;
    else if (incidence <-(pi/2))
        i2 = (incidence+pi);
    else 
        i2 = incidence;
    float c1=  (i2-_airfoil_incidence_no_lift)*_liftcoef;
    if (stall > 0)
    {
    float c2=  Math::sin(2*(incidence-_airfoil_incidence_no_lift))
        *_liftcoef*_lift_factor_stall;
    return (1-stall)*c1 + stall *c2;
    }
    else
        return c1;
}

float Rotor::getDragCoef(float incidence,float speed)
{
    float stall=calcStall(incidence,speed);
    float c1= (Math::abs(Math::sin(incidence-_airfoil_incidence_no_lift))
        *_dragcoef1+_dragcoef0);
    float c2= c1*_drag_factor_stall;
    return (1-stall)*c1 + stall *c2;
}

int Rotor::getValueforFGSet(int j,char *text,float *f)
{
    if (_name[0]==0) return 0;
    if (4>numRotorparts()) return 0; //compile first!
    if (j==0)
    {
        sprintf(text,"/rotors/%s/cone-deg", _name);
        *f=( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
            +((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
            +((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
            +((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
            )/4*180/pi;
    }
    else
        if (j==1)
        {
            sprintf(text,"/rotors/%s/roll-deg", _name);
            _roll = ( ((Rotorpart*)getRotorpart(0))->getrealAlpha()
                -((Rotorpart*)getRotorpart(2*(_number_of_parts>>2)))->getrealAlpha()
                )/2*(_ccw?-1:1);
            *f=_roll *180/pi;
        }
        else
            if (j==2)
            {
                sprintf(text,"/rotors/%s/yaw-deg", _name);
                _yaw=( ((Rotorpart*)getRotorpart(1*(_number_of_parts>>2)))->getrealAlpha()
                    -((Rotorpart*)getRotorpart(3*(_number_of_parts>>2)))->getrealAlpha()
                    )/2;
                *f=_yaw*180/pi;
            }
            else
                if (j==3)
                {
                    sprintf(text,"/rotors/%s/rpm", _name);
                    *f=_omega/2/pi*60;
                }
                else
                    if (j==4)
                    {
                        sprintf(text,"/rotors/%s/debug/debugfge",_name);
                        *f=_f_ge;
                    }
                    else if (j==5)
                    {
                        sprintf(text,"/rotors/%s/debug/debugfvs",_name);
                        *f=_f_vs;
                    }
                    else if (j==6)
                    {
                        sprintf(text,"/rotors/%s/debug/debugftl",_name);
                        *f=_f_tl;
                    }
                    else if (j==7)
                    {
                        sprintf(text,"/rotors/%s/debug/vortexstate",_name);
                        *f=_vortex_state;
                    }
                    else if (j==8)
                    {
                        sprintf(text,"/rotors/%s/stall",_name);
                        *f=getOverallStall();
                    }
                    else if (j==9)
                    {
                        sprintf(text,"/rotors/%s/torque",_name);
                        *f=-_torque;;
                    }
                    else
                    {
                        int b=(j-10)/3; 
                        if (b>=_number_of_blades) 
                        {
                            return 0;
                        }
                        int w=j%3;
                        sprintf(text,"/rotors/%s/blade[%i]/%s",
                            _name,b,
                            w==0?"position-deg":(w==1?"flap-deg":"incidence-deg"));
                        *f=((Rotorpart*)getRotorpart(0))->getPhi()*180/pi
                            +360*b/_number_of_blades*(_ccw?1:-1);
                        if (*f>360) *f-=360;
                        if (*f<0) *f+=360;
                        int k,l;
                        float rk,rl,p;
                        p=(*f/90);
                        k=int(p);
                        l=k+1;
                        rk=l-p;
                        rk=Math::clamp(rk,0,1);//Delete this
                        rl=1-rk;
                        if(w==2) {k+=2;l+=2;}
                        else
                            if(w==1) {k+=1;l+=1;}
                            k%=4;
                            l%=4;
                            if (w==1) *f=rk*((Rotorpart*) getRotorpart(k*(_number_of_parts>>2)))->getrealAlpha()*180/pi
                                +rl*((Rotorpart*) getRotorpart(l*(_number_of_parts>>2)))->getrealAlpha()*180/pi;
                            else if(w==2) *f=rk*((Rotorpart*)getRotorpart(k*(_number_of_parts>>2)))->getIncidence()*180/pi
                                +rl*((Rotorpart*)getRotorpart(l*(_number_of_parts>>2)))->getIncidence()*180/pi;
                    }
                    return j+1;
}

void Rotorgear::setEngineOn(int value)
{
    _engineon=value;
}

void Rotor::setAlpha0(float f)
{
    _alpha0=f;
}

void Rotor::setAlphamin(float f)
{
    _alphamin=f;
}

void Rotor::setAlphamax(float f)
{
    _alphamax=f;
}

void Rotor::setAlpha0factor(float f)
{
    _alpha0factor=f;
}

int Rotor::numRotorparts()
{
    return _rotorparts.size();
}

Rotorpart* Rotor::getRotorpart(int n)
{
    return ((Rotorpart*)_rotorparts.get(n));
}

int Rotorgear::getEngineon()
{
    return _engineon;
}

float Rotor::getTorqueOfInertia()
{
    return _torque_of_inertia;
}

void Rotor::setTorque(float f)
{
    _torque=f;
}

void Rotor::addTorque(float f)
{
    _torque+=f;
}

void Rotor::strncpy(char *dest,const char *src,int maxlen)
{
    int n=0;
    while(src[n]&&n<(maxlen-1))
    {
        dest[n]=src[n];
        n++;
    }
    dest[n]=0;
}

void Rotor::setNormal(float* normal)
{
    int i;
    float invsum,sqrsum=0;
    for(i=0; i<3; i++) { sqrsum+=normal[i]*normal[i];}
    if (sqrsum!=0)
        invsum=1/Math::sqrt(sqrsum);
    else
        invsum=1;
    for(i=0; i<3; i++) 
    {
        _normal_with_yaw_roll[i]=_normal[i] = normal[i]*invsum;
    }
}

void Rotor::setForward(float* forward)
{
    int i;
    float invsum,sqrsum=0;
    for(i=0; i<3; i++) { sqrsum+=forward[i]*forward[i];}
    if (sqrsum!=0)
        invsum=1/Math::sqrt(sqrsum);
    else
        invsum=1;
    for(i=0; i<3; i++) { _forward[i] = forward[i]*invsum; }
}

void Rotor::setForceAtPitchA(float force)
{
    _force_at_pitch_a=force; 
}

void Rotor::setPowerAtPitch0(float value)
{
    _power_at_pitch_0=value; 
}

void Rotor::setPowerAtPitchB(float value)
{
    _power_at_pitch_b=value; 
}

void Rotor::setPitchA(float value)
{
    _pitch_a=value/180*pi; 
}

void Rotor::setPitchB(float value)
{
    _pitch_b=value/180*pi; 
}

void Rotor::setBase(float* base)
{
    int i;
    for(i=0; i<3; i++) _base[i] = base[i];
}

void Rotor::setMaxCyclicail(float value)
{
    _maxcyclicail=value/180*pi;
}

void Rotor::setMaxCyclicele(float value)
{
    _maxcyclicele=value/180*pi;
}

void Rotor::setMinCyclicail(float value)
{
    _mincyclicail=value/180*pi;
}

void Rotor::setMinCyclicele(float value)
{
    _mincyclicele=value/180*pi;
}

void Rotor::setMinCollective(float value)
{
    _min_pitch=value/180*pi;
}

void Rotor::setMaxCollective(float value)
{
    _max_pitch=value/180*pi;
}

void Rotor::setDiameter(float value)
{
    _diameter=value;
}

void Rotor::setWeightPerBlade(float value)
{
    _weight_per_blade=value;
}

void Rotor::setNumberOfBlades(float value)
{
    _number_of_blades=int(value+.5);
}

void Rotor::setRelBladeCenter(float value)
{
    _rel_blade_center=value;
}

void Rotor::setDynamic(float value)
{
    _dynamic=value;
}

void Rotor::setDelta3(float value)
{
    _delta3=value;
}

void Rotor::setDelta(float value)
{
    _delta=value;
}

void Rotor::setTranslift(float value)
{
    _translift=value;
}

void Rotor::setC2(float value)
{
    _c2=value;
}

void Rotor::setStepspersecond(float steps)
{
    _stepspersecond=steps;
}

void Rotor::setRPM(float value)
{
    _rotor_rpm=value;
}

void Rotor::setRelLenHinge(float value)
{
    _rel_len_hinge=value;
}

void Rotor::setAlphaoutput(int i, const char *text)
{
    strncpy(_alphaoutput[i],text,255);
}

void Rotor::setName(const char *text)
{
    strncpy(_name,text,256);//256: some space needed for settings
}

void Rotor::setCcw(int ccw)
{	
    _ccw=ccw;
}

void Rotor::setNotorque(int value)
{
    _no_torque=value;
}

void Rotor::setRelLenTeeterHinge(float f)
{
    _rellenteeterhinge=f;
}

void Rotor::setTeeterdamp(float f)
{
    _teeterdamp=f;
}

void Rotor::setMaxteeterdamp(float f)
{
    _maxteeterdamp=f;
}

void Rotor::setGlobalGround(double *global_ground, float* global_vel)
{
    int i;
    for(i=0; i<4; i++) _global_ground[i] = global_ground[i];
}

void Rotor::setParameter(char *parametername, float value)
{
#define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
    p(translift_ve,1)
        p(translift_maxfactor,1)
        p(ground_effect_constant,1)
        p(vortex_state_lift_factor,1)
        p(vortex_state_c1,1)
        p(vortex_state_c2,1)
        p(vortex_state_c3,1)
        p(vortex_state_e1,1)
        p(vortex_state_e2,1)
        p(vortex_state_e3,1)
        p(twist,pi/180.)
        p(number_of_segments,1)
        p(number_of_parts,1)
        p(rel_len_where_incidence_is_measured,1)
        p(chord,1)
        p(taper,1)
        p(airfoil_incidence_no_lift,pi/180.)
        p(rel_len_blade_start,1)
        p(incidence_stall_zero_speed,pi/180.)
        p(incidence_stall_half_sonic_speed,pi/180.)
        p(lift_factor_stall,1)
        p(stall_change_over,pi/180.)
        p(drag_factor_stall,1)
        p(airfoil_lift_coefficient,1)
        p(airfoil_drag_coefficient0,1)
        p(airfoil_drag_coefficient1,1)
        p(cyclic_factor,1)
        p(rotor_correction_factor,1)
        SG_LOG(SG_INPUT, SG_ALERT,
            "internal error in parameter set up for rotor: '" << 
            parametername <<"'" << endl);
#undef p
}

float Rotor::getLiftFactor()
{
    return _lift_factor;
}

void Rotorgear::setRotorBrake(float lval)
{
    lval = Math::clamp(lval, 0, 1);
    _rotorbrake=lval;
}

void Rotor::setCollective(float lval)
{
    lval = Math::clamp(lval, -1, 1);
    int i;
    for(i=0; i<_rotorparts.size(); i++) {
        ((Rotorpart*)_rotorparts.get(i))->setCollective(lval);
    }
    _collective=_min_pitch+(lval+1)/2*(_max_pitch-_min_pitch);
}

void Rotor::setCyclicele(float lval,float rval)
{
    lval = Math::clamp(lval, -1, 1);
    _cyclicele=_mincyclicele+(lval+1)/2*(_maxcyclicele-_mincyclicele);
}

void Rotor::setCyclicail(float lval,float rval)
{
    lval = Math::clamp(lval, -1, 1);
    if (_ccw) lval *=-1;
    _cyclicail=-(_mincyclicail+(lval+1)/2*(_maxcyclicail-_mincyclicail));
}

void Rotor::getPosition(float* out)
{
    int i;
    for(i=0; i<3; i++) out[i] = _base[i];
}

void Rotor::calcLiftFactor(float* v, float rho, State *s)
{
    //calculates _lift_factor, which is a foactor for the lift of the rotor
    //due to
    //- ground effect (_f_ge)
    //- vortex state (_f_vs)
    //- translational lift (_f_tl)
    _f_ge=1;
    _f_tl=1;
    _f_vs=1;

    // Calculate ground effect
    _f_ge=1+_diameter/_ground_effect_altitude*_ground_effect_constant;

    // Now calculate translational lift
    float v_vert = Math::dot3(v,_normal);
    float help[3];
    Math::cross3(v,_normal,help);
    float v_horiz = Math::mag3(help);
    _f_tl = ((1-Math::pow(2.7183,-v_horiz/_translift_ve))
        *(_translift_maxfactor-1)+1)/_translift_maxfactor;

    _lift_factor = _f_ge*_f_tl*_f_vs;
}

void Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s)
{
    _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
        _groundeffectpos[0],_groundeffectpos[1],
        _groundeffectpos[2],_groundeffectpos[3]);
}

float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
        float *pos0,float *pos1,float *pos2,float *pos3,
        int iteration,float a0,float a1,float a2,float a3)
{
    float a[5];
    float *p[5],pos4[3];
    a[0]=a0;
    a[1]=a1;
    a[2]=a2;
    a[3]=a3;
    a[4]=-1;
    p[0]=pos0;
    p[1]=pos1;
    p[2]=pos2;
    p[3]=pos3;
    p[4]=pos4;
    Math::add3(p[0],p[2],p[4]);
    Math::mul3(0.5,p[4],p[4]);//the center
    
    float mina=100*_diameter;
    float suma=0;
    for (int i=0;i<5;i++)
    {
        if (a[i]==-1)//in the first iteration,(iteration==0) no height is
                     //passed to this function, these missing values are 
                     //marked by ==-1
        {
            double pt[3];
            s->posLocalToGlobal(p[i], pt);

            // Ask for the ground plane in the global coordinate system
            double global_ground[4];
            float global_vel[3];
            ground_cb->getGroundPlane(pt, global_ground, global_vel);
            // find h, the distance to the ground 
            // The ground plane transformed to the local frame.
            float ground[4];
            s->planeGlobalToLocal(global_ground, ground);

            a[i] = ground[3] - Math::dot3(p[i], ground);
            // Now a[i] is the distance from p[i] to ground
        }
        suma+=a[i];
        if (a[i]<mina)
            mina=a[i];
    }
    if (mina>=10*_diameter)
        return mina; //the ground effect will be zero
    
    //check if further recursion is neccessary
    //if the height does not differ more than 20%, than 
    //we can return then mean height, if not split
    //zhe square to four parts and calcualte the height
    //for each part
    //suma * 0.2 is the mean 
    //0.15 is the maximum allowed difference from the mean
    //to the height at the center
    if ((iteration>2)
       ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
        return suma*0.2;
    suma=0;
    float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3] 
    for (int i=0;i<4;i++)
    {
        Math::add3(p[i],p[(i+1)&3],pc[i]);
        Math::mul3(0.5,pc[i],pc[i]);
        double pt[3];
        s->posLocalToGlobal(pc[i], pt);

        // Ask for the ground plane in the global coordinate system
        double global_ground[4];
        float global_vel[3];
        ground_cb->getGroundPlane(pt, global_ground, global_vel);
        // find h, the distance to the ground 
        // The ground plane transformed to the local frame.
        float ground[4];
        s->planeGlobalToLocal(global_ground, ground);

        ac[i] = ground[3] - Math::dot3(p[i], ground);
        // Now ac[i] is the distance from pc[i] to ground
    }
    return 0.25*
        (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
            iteration+1,a[0],ac[0],a[4],ac[3])
        +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
            iteration+1,a[1],ac[0],a[4],ac[1])
        +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
            iteration+1,a[2],ac[1],a[4],ac[2])
        +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
            iteration+1,a[3],ac[2],a[4],ac[3])
        );
}

void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
{
    float pos2rotor[3],tmp[3];
    Math::sub3(_base,pos,pos2rotor);
    float dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
    //calculate incidence at 0.7r;
    float inc = _collective+_twist *0.7 
                - _twist*_rel_len_where_incidence_is_measured;
    if (inc < 0) 
        dist *=-1;
    if (dist<0) // we are not in the downwash region
    {
        downwash[0]=downwash[1]=downwash[2]=0.;
        return;
    }

    //calculate the mean downwash speed directly beneath the rotor disk
    float v1bar = Math::sin(inc) *_omega * 0.35 * _diameter * 0.8; 
    //0.35 * d = 0.7 *r, a good position to calcualte the mean downwashd
    //0.8 the slip of the rotor.

    //calculate the time the wind needed from thr rotor to here
    if (v1bar< 1) v1bar = 1;
    float time=dist/v1bar;

    //calculate the pos2rotor, where the rotor was, "time" ago
    Math::mul3(time,v_heli,tmp);
    Math::sub3(pos2rotor,tmp,pos2rotor);

    //and again calculate dist
    dist=Math::dot3(pos2rotor,_normal_with_yaw_roll);
    //missing the normal is offen not pointing to the normal of the rotor 
    //disk. Rotate the normal by yaw and tilt angle

    if (inc < 0) 
        dist *=-1;
    if (dist<0) // we are not in the downwash region
    {
        downwash[0]=downwash[1]=downwash[2]=0.;
        return;
    }
    //of course this could be done in a runge kutta integrator, but it's such
    //a approximation that I beleave, it would'nt be more realistic

    //calculate the dist to the rotor-axis
    Math::cross3(pos2rotor,_normal_with_yaw_roll,tmp);
    float r= Math::mag3(tmp);
    //calculate incidence at r;
    float rel_r = r *2 /_diameter;
    float inc_r = _collective+_twist * r /_diameter * 2 
        - _twist*_rel_len_where_incidence_is_measured;

    //calculate the downwash speed directly beneath the rotor disk
    float v1=0;
    if (rel_r<1)
        v1 = Math::sin(inc_r) *_omega * r * 0.8; 

    //calcualte the downwash speed in a distance "dist" to the rotor disc,
    //for large dist. The speed is assumed do follow a gausian distribution 
    //with sigma increasing with dist^2:
    //sigma is assumed to be half of the rotor diameter directly beneath the
    //disc and is assumed to the rotor diameter at dist = (diameter * sqrt(2))

    float sigma=_diameter/2 + dist * dist / _diameter /4.;
    float v2 = v1bar*_diameter/ (Math::sqrt(2 * pi) * sigma) 
        * Math::pow(2.7183,-.5*r*r/(sigma*sigma))*_diameter/2/sigma;

    //calculate the weight of the two downwash velocities.
    //Directly beneath the disc it is v1, far away it is v2
    float g = Math::pow(2.7183,-2*dist/_diameter); 
    //at dist = rotor radius it is assumed to be 1/e * v1 + (1-1/e)* v2

    float v = g * v1 + (1-g) * v2;
    Math::mul3(-v,_normal_with_yaw_roll,downwash);
    //the downwash is calculated in the opposite direction of the normal
}

void Rotor::compile()
{
    // Have we already been compiled?
    if(_rotorparts.size() != 0) return;

    //rotor is divided into _number_of_parts parts
    //each part is calcualted at _number_of_segments points

    //clamp to 4..256
    //and make it a factor of 4
    _number_of_parts=(int(Math::clamp(_number_of_parts,4,256))>>2)<<2;

    _dynamic=_dynamic*(1/                          //inverse of the time
        ( (60/_rotor_rpm)/4         //for rotating 90 deg
        +(60/_rotor_rpm)/(2*_number_of_blades) //+ meantime a rotorblade 
                                               //will pass a given point 
        ));
    float directions[5][3];
    //pointing forward, right, ... the 5th is ony for calculation
    directions[0][0]=_forward[0];
    directions[0][1]=_forward[1];
    directions[0][2]=_forward[2];
    int i;
    for (i=1;i<5;i++)
    {
        if (!_ccw)
            Math::cross3(directions[i-1],_normal,directions[i]);
        else
            Math::cross3(_normal,directions[i-1],directions[i]);
        Math::unit3(directions[i],directions[i]);
    }
    Math::set3(directions[4],directions[0]);
    // now directions[0] is perpendicular to the _normal.and has a length
    // of 1. if _forward is already normalized and perpendicular to the 
    // normal, directions[0] will be the same
    for (i=0;i<4;i++)
    {
        Math::mul3(_diameter*0.7,directions[i],_groundeffectpos[i]);
        Math::add3(_base,_groundeffectpos[i],_groundeffectpos[i]);
    }
    float rotorpartmass = _weight_per_blade*_number_of_blades/_number_of_parts*.453;
    //was pounds -> now kg

    _torque_of_inertia = 1/12. * ( _number_of_parts * rotorpartmass) * _diameter 
        * _diameter * _rel_blade_center * _rel_blade_center /(0.5*0.5);
    float speed=_rotor_rpm/60*_diameter*_rel_blade_center*pi;
    float lentocenter=_diameter*_rel_blade_center*0.5;
    float lentoforceattac=_diameter*_rel_len_hinge*0.5;
    float zentforce=rotorpartmass*speed*speed/lentocenter;
    float pitchaforce=_force_at_pitch_a/_number_of_parts*.453*9.81;
    // was pounds of force, now N, devided by _number_of_parts
    //(so its now per rotorpart)

    float torque0=0,torquemax=0,torqueb=0;
    float omega=_rotor_rpm/60*2*pi;
    _omegan=omega;
    float omega0=omega*Math::sqrt(1/(1-_rel_len_hinge));
    _delta*=pitchaforce/(_pitch_a*omega*lentocenter*2*rotorpartmass);

    float phi=Math::atan2(2*omega*_delta,omega0*omega0-omega*omega);
    float relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
        +4*_delta*_delta*omega*omega)))*_cyclic_factor;
    if (!_no_torque)
    {
        torque0=_power_at_pitch_0/_number_of_parts*1000/omega;  
        // f*r=p/w ; p=f*s/t;  r=s/t/w ; r*w*t = s
        torqueb=_power_at_pitch_b/_number_of_parts*1000/omega;
        torquemax=_power_at_pitch_b/_number_of_parts*1000/omega/_pitch_b*_max_pitch;

        if(_ccw)
        {
            torque0*=-1;
            torquemax*=-1;
            torqueb*=-1;
        }
    }

    Rotorpart* rps[256];
    for (i=0;i<_number_of_parts;i++)
    {
        float lpos[3],lforceattac[3],lspeed[3],dirzentforce[3];
        float s = Math::sin(2*pi*i/_number_of_parts);
        float c = Math::cos(2*pi*i/_number_of_parts);
        float direction[3],nextdirection[3],help[3];
        Math::mul3(c ,directions[0],help);
        Math::mul3(s ,directions[1],direction);
        Math::add3(help,direction,direction);

        Math::mul3(c ,directions[1],help);
        Math::mul3(s ,directions[2],nextdirection);
        Math::add3(help,nextdirection,nextdirection);

        Math::mul3(lentocenter,direction,lpos);
        Math::add3(lpos,_base,lpos);
        Math::mul3(lentoforceattac,nextdirection,lforceattac);
        //nextdirection: +90deg (gyro)!!!

        Math::add3(lforceattac,_base,lforceattac);
        Math::mul3(speed,nextdirection,lspeed);
        Math::mul3(1,nextdirection,dirzentforce);


        float maxcyclic=(i&1)?_maxcyclicele:_maxcyclicail;
        float mincyclic=(i&1)?_mincyclicele:_mincyclicail;

        Rotorpart* rp=rps[i]=newRotorpart(lpos, lforceattac, _normal,
            lspeed,dirzentforce,zentforce,pitchaforce, _max_pitch,_min_pitch,
            mincyclic,maxcyclic,_delta3,rotorpartmass,_translift,
            _rel_len_hinge,lentocenter);
        int k = i*4/_number_of_parts;
        rp->setAlphaoutput(_alphaoutput[k&1?k:(_ccw?k^2:k)],0);
        rp->setAlphaoutput(_alphaoutput[4+(k&1?k:(_ccw?k^2:k))],1+(k>1));
        _rotorparts.add(rp);
        rp->setTorque(torquemax,torque0);
        rp->setRelamp(relamp);
        rp->setDirectionofRotorPart(direction);
        rp->setTorqueOfInertia(_torque_of_inertia/_number_of_parts);
    }
    for (i=0;i<_number_of_parts;i++)
    {
        rps[i]->setlastnextrp(rps[(i-1+_number_of_parts)%_number_of_parts],
            rps[(i+1)%_number_of_parts],
            rps[(i+_number_of_parts/2)%_number_of_parts],
            rps[(i-_number_of_parts/4+_number_of_parts)%_number_of_parts],
            rps[(i+_number_of_parts/4)%_number_of_parts]);
    }
    for (i=0;i<_number_of_parts;i++)
    {
        rps[i]->setCompiled();
    }
    float lift[4],torque[4], v_wind[3];
    v_wind[0]=v_wind[1]=v_wind[2]=0;
    rps[0]->setOmega(_omegan);

    if (_airfoil_lift_coefficient==0)
    {
        //calculate the lift and drag coefficients now
        _dragcoef0=1;
        _dragcoef1=1;
        _liftcoef=1;
        rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
            &(torque[0]),&(lift[0])); //max_pitch a
        _liftcoef = pitchaforce/lift[0];
        _dragcoef0=1;
        _dragcoef1=0;
        rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[0]),&(lift[0])); 
        //0 degree, c0

        _dragcoef0=0;
        _dragcoef1=1;
        rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,&(torque[1]),&(lift[1]));
        //0 degree, c1

        _dragcoef0=1;
        _dragcoef1=0;
        rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[2]),&(lift[2])); 
        //picth b, c0

        _dragcoef0=0;
        _dragcoef1=1;
        rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,&(torque[3]),&(lift[3])); 
        //picth b, c1

        if (torque[0]==0)
        {
            _dragcoef1=torque0/torque[1];
            _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
        }
        else
        {
            _dragcoef1=(torque0/torque[0]-torqueb/torque[2])
                /(torque[1]/torque[0]-torque[3]/torque[2]);
            _dragcoef0=(torqueb-_dragcoef1*torque[3])/torque[2];
        }
    }
    else
    {
        _liftcoef=_airfoil_lift_coefficient/_number_of_parts*_number_of_blades;
        _dragcoef0=_airfoil_drag_coefficient0/_number_of_parts*_number_of_blades*_c2;
        _dragcoef1=_airfoil_drag_coefficient1/_number_of_parts*_number_of_blades*_c2;
    }

    //Check
    rps[0]->calculateAlpha(v_wind,rho_null,_pitch_a,0,0,
        &(torque[0]),&(lift[0])); //pitch a
    rps[0]->calculateAlpha(v_wind,rho_null,_pitch_b,0,0,
        &(torque[1]),&(lift[1])); //pitch b
    rps[0]->calculateAlpha(v_wind,rho_null,0,0,0,
        &(torque[3]),&(lift[3])); //pitch 0
    SG_LOG(SG_GENERAL, SG_INFO,
        "Rotor: coefficients for airfoil:" << endl << setprecision(6)
        << " drag0: " << _dragcoef0*_number_of_parts/_number_of_blades/_c2
        << " drag1: " << _dragcoef1*_number_of_parts/_number_of_blades/_c2
        << " lift: " << _liftcoef*_number_of_parts/_number_of_blades
        << endl
        << "at 10 deg:" << endl
        << "drag: " << (Math::sin(10./180*pi)*_dragcoef1+_dragcoef0)
            *_number_of_parts/_number_of_blades/_c2
        << " lift: " << Math::sin(10./180*pi)*_liftcoef*_number_of_parts/_number_of_blades
        << endl
        << "Some results (Pitch [degree], Power [kW], Lift [N])" << endl
        << 0.0f << "deg " << Math::abs(torque[3]*_number_of_parts*_omegan/1000) << "kW "
            << lift[3]*_number_of_parts << endl
        << _pitch_a*180/pi << "deg " << Math::abs(torque[0]*_number_of_parts*_omegan/1000) 
            << "kW " << lift[0]*_number_of_parts << endl
        << _pitch_b*180/pi << "deg " << Math::abs(torque[1]*_number_of_parts*_omegan/1000) 
            << "kW " << lift[1]*_number_of_parts << endl << endl );

    //first calculation of relamp is wrong
    //it used pitchaforce, but this was unknown and
    //on the default value
    _delta*=lift[0]/pitchaforce;
    relamp=(omega*omega/(2*_delta*Math::sqrt(sqr(omega0*omega0-omega*omega)
        +4*_delta*_delta*omega*omega)))*_cyclic_factor;
    for (i=0;i<_number_of_parts;i++)
    {
        rps[i]->setRelamp(relamp);
    }
    rps[0]->setOmega(0);
    writeInfo();
}
std::ostream &  operator<<(std::ostream & out, /*const*/ Rotor& r)
{
#define i(x) << #x << ":" << r.x << endl
#define iv(x) << #x << ":" << r.x[0] << ";" << r.x[1] << ";" <<r.x[2] << ";" << endl
    out << "Writing Info on Rotor " 
    i(_name)
    i(_torque)
    i(_omega) i(_omegan) i(_omegarel) i(_ddt_omega) i(_omegarelneu)
    i (_chord)
    i( _taper)
    i( _airfoil_incidence_no_lift)
    i( _collective)
    i( _airfoil_lift_coefficient)
    i( _airfoil_drag_coefficient0)
    i( _airfoil_drag_coefficient1)
    i( _ccw)
    i( _number_of_segments)
    i( _number_of_parts)
    iv( _base)
    iv( _groundeffectpos[0])iv( _groundeffectpos[1])iv( _groundeffectpos[2])iv( _groundeffectpos[3])
    i( _ground_effect_altitude)
    iv( _normal)
    iv( _normal_with_yaw_roll)
    iv( _forward)
    i( _diameter)
    i( _number_of_blades)
    i( _weight_per_blade)
    i( _rel_blade_center)
    i( _min_pitch)
    i( _max_pitch)
    i( _force_at_pitch_a)
    i( _pitch_a)
    i( _power_at_pitch_0)
    i( _power_at_pitch_b)
    i( _no_torque)
    i( _sim_blades)
    i( _pitch_b)
    i( _rotor_rpm)
    i( _rel_len_hinge)
    i( _maxcyclicail)
    i( _maxcyclicele)
    i( _mincyclicail)
    i( _mincyclicele)
    i( _delta3)
    i( _delta)
    i( _dynamic)
    i( _translift)
    i( _c2)
    i( _stepspersecond)
    i( _engineon)
    i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
    i( _teeterdamp) i(_maxteeterdamp)
    i( _rellenteeterhinge)
    i( _translift_ve)
    i( _translift_maxfactor)
    i( _ground_effect_constant)
    i( _vortex_state_lift_factor)
    i( _vortex_state_c1)
    i( _vortex_state_c2)
    i( _vortex_state_c3)
    i( _vortex_state_e1)
    i( _vortex_state_e2)
    i( _vortex_state_e3)
    i( _lift_factor) i(_f_ge) i(_f_vs) i(_f_tl)
    i( _vortex_state)
    i( _liftcoef)
    i( _dragcoef0)
    i( _dragcoef1)
    i( _twist) //outer incidence = inner inner incidence + _twist
    i( _rel_len_where_incidence_is_measured)
    i( _torque_of_inertia)
    i( _rel_len_blade_start)
    i( _incidence_stall_zero_speed)
    i( _incidence_stall_half_sonic_speed)
    i( _lift_factor_stall)
    i( _stall_change_over)
    i( _drag_factor_stall)
    i( _stall_sum)
    i( _stall_v2sum)
    i( _yaw)
    i( _roll)
    i( _cyclicail)
    i( _cyclicele)
    i( _cyclic_factor) <<endl;
    int j;
    for(j=0; j<r._rotorparts.size(); j++) {
        out << *((Rotorpart*)r._rotorparts.get(j));
    }
    out <<endl << endl;
#undef i
#undef iv
    return out;
}
void Rotor:: writeInfo()
{
#ifdef TEST_DEBUG
    std::ostringstream buffer;
    buffer << *this;
    FILE*f=fopen("c:\\fgmsvc\\bat\\log.txt","at");
    if (!f) f=fopen("c:\\fgmsvc\\bat\\log.txt","wt");
    if (f)
    {
        fprintf(f,"%s",(const char *)buffer.str().c_str());
        fclose (f);
    }
#endif
}
Rotorpart* Rotor::newRotorpart(float* pos, float *posforceattac, float *normal,
    float* speed,float *dirzentforce, float zentforce,float maxpitchforce,
    float maxpitch, float minpitch, float mincyclic,float maxcyclic,
    float delta3,float mass,float translift,float rellenhinge,float len)
{
    Rotorpart *r = new Rotorpart();
    r->setPosition(pos);
    r->setNormal(normal);
    r->setZentipetalForce(zentforce);
    r->setPositionForceAttac(posforceattac);
    r->setSpeed(speed);
    r->setDirectionofZentipetalforce(dirzentforce);
    r->setMaxpitch(maxpitch);
    r->setMinpitch(minpitch);
    r->setMaxcyclic(maxcyclic);
    r->setMincyclic(mincyclic);
    r->setDelta3(delta3);
    r->setDynamic(_dynamic);
    r->setTranslift(_translift);
    r->setC2(_c2);
    r->setWeight(mass);
    r->setRelLenHinge(rellenhinge);
    r->setOmegaN(_omegan);
    r->setAlpha0(_alpha0);
    r->setAlphamin(_alphamin);
    r->setAlphamax(_alphamax);
    r->setAlpha0factor(_alpha0factor);
    r->setLen(len);
    r->setDiameter(_diameter);
    r->setRotor(this);
#define p(a) r->setParameter(#a,_##a);
    p(twist)
    p(number_of_segments)
    p(rel_len_where_incidence_is_measured)
    p(rel_len_blade_start)
    p(rotor_correction_factor)
#undef p
    return r;
}

void Rotor::interp(float* v1, float* v2, float frac, float* out)
{
    out[0] = v1[0] + frac*(v2[0]-v1[0]);
    out[1] = v1[1] + frac*(v2[1]-v1[1]);
    out[2] = v1[2] + frac*(v2[2]-v1[2]);
}

void Rotorgear::initRotorIteration(float *lrot,float dt)
{
    int i;
    float omegarel;
    if (!_rotors.size()) return;
    Rotor* r0 = (Rotor*)_rotors.get(0);
    omegarel= r0->getOmegaRelNeu();
    for(i=0; i<_rotors.size(); i++) {
        Rotor* r = (Rotor*)_rotors.get(i);
        r->inititeration(dt,omegarel,0,lrot);
    }
}

void Rotorgear::calcForces(float* torqueOut)
{
    int i,j;
    torqueOut[0]=torqueOut[1]=torqueOut[2]=0;
    // check,<if the engine can handle the torque of the rotors. 
    // If not reduce the torque to the fueselage and change rotational 
    // speed of the rotors instead
    if (_rotors.size())
    {
        float omegarel,omegan;
        Rotor* r0 = (Rotor*)_rotors.get(0);
        omegarel= r0->getOmegaRel();

        float total_torque_of_inertia=0;
        float total_torque=0;
        for(i=0; i<_rotors.size(); i++) {
            Rotor* r = (Rotor*)_rotors.get(i);
            omegan=r->getOmegan();
            total_torque_of_inertia+=r->getTorqueOfInertia()*omegan*omegan;
            //FIXME: this is constant, so this can be done in compile

            total_torque+=r->getTorque()*omegan;
        }
        float max_torque_of_engine=0;
        if (_engineon)
        {
            max_torque_of_engine=_max_power_engine;
            float df=1-omegarel;
            df/=_engine_prop_factor;
            df = Math::clamp(df, 0, 1);
            max_torque_of_engine = df * _max_power_engine;
        }
        total_torque*=-1;
        _ddt_omegarel=0;
        float rel_torque_engine=1;
        if (total_torque<=0)
            rel_torque_engine=0;
        else
            if (max_torque_of_engine>0)
                rel_torque_engine=1/max_torque_of_engine*total_torque;
            else
                rel_torque_engine=0;

        //add the rotor brake and the gear fritcion
        float dt=0.1f;
        if (r0->_rotorparts.size()) dt=((Rotorpart*)r0->_rotorparts.get(0))->getDt();

        float rotor_brake_torque;
        rotor_brake_torque=_rotorbrake*_max_power_rotor_brake+_rotorgear_friction;
        //clamp it to the value you need to stop the rotor
        //to avod accelerate the rotor to neagtive rpm:
        rotor_brake_torque=Math::clamp(rotor_brake_torque,0,
            total_torque_of_inertia/dt*omegarel);
        max_torque_of_engine-=rotor_brake_torque;

        //change the rotation of the rotors 
        if ((max_torque_of_engine<total_torque) //decreasing rotation
            ||((max_torque_of_engine>total_torque)&&(omegarel<1))
            //increasing rotation due to engine
            ||(total_torque<0) ) //increasing rotation due to autorotation
        {
            _ddt_omegarel=(max_torque_of_engine-total_torque)/total_torque_of_inertia;
            if(max_torque_of_engine>total_torque) 
            {
                //check if the acceleration is due to the engine. If yes,
                //the engine self limits the accel.
                float lim1=-total_torque/total_torque_of_inertia; 
                //accel. by autorotation
                
                if (lim1<_engine_accel_limit) lim1=_engine_accel_limit; 
                //if the accel by autorotation greater than the max. engine
                //accel, then this is the limit, if not: the engine is the limit
                if (_ddt_omegarel>lim1) _ddt_omegarel=lim1;
            }
            if (_ddt_omegarel>5.5)_ddt_omegarel=5.5; 
            //clamp it to avoid overflow. Should never be reached
            if (_ddt_omegarel<-5.5)_ddt_omegarel=-5.5;

            if (_max_power_engine<0.001) {omegarel=1;_ddt_omegarel=0;}
            //for debug: negative or no maxpower will result 
            //in permanet 100% rotation

            omegarel+=dt*_ddt_omegarel;

            if (omegarel>2.5) omegarel=2.5; 
            //clamp it to avoid overflow. Should never be reached
            if (omegarel<-.5) omegarel=-.5;

            r0->setOmegaRelNeu(omegarel);
            //calculate the torque, which is needed to accelerate the rotors.
            //Add this additional torque to the body
            for(j=0; j<_rotors.size(); j++) {
                Rotor* r = (Rotor*)_rotors.get(j);
                for(i=0; i<r->_rotorparts.size(); i++) {
                    float torque_scalar=0;
                    Rotorpart* rp = (Rotorpart*)r->_rotorparts.get(i);
                    float torque[3];
                    rp->getAccelTorque(_ddt_omegarel,torque);
                    Math::add3(torque,torqueOut,torqueOut);
                }
            }
        }
        _total_torque_on_engine=total_torque+_ddt_omegarel*total_torque_of_inertia;
    }
}

void Rotorgear::addRotor(Rotor* rotor)
{
    _rotors.add(rotor);
    _in_use = 1;
}

void Rotorgear::compile()
{
    float wgt = 0;
    for(int j=0; j<_rotors.size(); j++) {
        Rotor* r = (Rotor*)_rotors.get(j);
        r->compile();
    }
}

void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
{
    float tmp[3];
    downwash[0]=downwash[1]=downwash[2]=0;
    for(int i=0; i<_rotors.size(); i++) {
        Rotor* ro = (Rotor*)_rotors.get(i);
        ro->getDownWash(pos,v_heli,tmp);
        Math::add3(downwash,tmp,downwash);    //  + downwash
    }
}

void Rotorgear::setParameter(char *parametername, float value)
{
#define p(a,b) if (strcmp(parametername,#a)==0) _##a = (value * (b)); else
        p(max_power_engine,1000)
        p(engine_prop_factor,1)
        p(yasimdragfactor,1)
        p(yasimliftfactor,1)
        p(max_power_rotor_brake,1000)
        p(rotorgear_friction,1000)
        p(engine_accel_limit,0.01)
        SG_LOG(SG_INPUT, SG_ALERT,
            "internal error in parameter set up for rotorgear: '"
            << parametername <<"'" << endl);
#undef p
}
int Rotorgear::getValueforFGSet(int j,char *text,float *f)
{
    if (j==0)
    {
        sprintf(text,"/rotors/gear/total-torque");
        *f=_total_torque_on_engine;
    } else return 0;
    return j+1;
}
Rotorgear::Rotorgear()
{
    _in_use=0;
    _engineon=0;
    _rotorbrake=0;
    _max_power_rotor_brake=1;
    _rotorgear_friction=1;
    _max_power_engine=1000*450;
    _engine_prop_factor=0.05f;
    _yasimdragfactor=1;
    _yasimliftfactor=1;
    _ddt_omegarel=0;
    _engine_accel_limit=0.05f;
    _total_torque_on_engine=0;
}

Rotorgear::~Rotorgear()
{
    for(int i=0; i<_rotors.size(); i++)
        delete (Rotor*)_rotors.get(i);
}

}; // namespace yasim