#include <simgear/debug/logstream.hxx>

#include "Math.hpp"
#include "Rotorpart.hpp"
#include "Rotor.hpp"
#include <stdio.h>
#include <string.h>
namespace yasim {
const float pi=3.14159;
float _help = 0;
Rotorpart::Rotorpart()
{
    _compiled=0;
    _cyclic=0;
    _collective=0;
    _rellenhinge=0;
    _shared_flap_hinge=false;
    _dt=0;
#define set3(x,a,b,c) x[0]=a;x[1]=b;x[2]=c;
    set3 (_speed,1,0,0);
    set3 (_directionofcentripetalforce,1,0,0);
    set3 (_directionofrotorpart,0,1,0);
    set3 (_direction_of_movement,1,0,0);
    set3 (_last_torque,0,0,0);
#undef set3
    _centripetalforce=1;
    _delta3=0.5;
    _cyclic=0;
    _collective=-1;
    _relamp=1;
    _mass=10;
    _incidence = 0;
    _alpha=0;
    _alphamin=-.1;
    _alphamax= .1;
    _alpha0=-.05;
    _alpha0factor=1;
    _alphaoutputbuf[0][0]=0;
    _alphaoutputbuf[1][0]=0;
    _alpha2type=0;
    _alphaalt=0;
    _lastrp=0;
    _nextrp=0;
    _oppositerp=0;
    _last90rp=0;
    _next90rp=0;
    _translift=0;
    _dynamic=100;
    _c2=0;
    _torque_max_force=0;
    _torque_no_force=0;
    _omega=0;
    _omegan=1;
    _ddt_omega=0;
    _phi=0;
    _len=1;
    _rotor=NULL;
    _twist=0; 
    _number_of_segments=1;
    _rel_len_where_incidence_is_measured=0.7;
    _diameter=10;
    _torque_of_inertia=0;
    _rel_len_blade_start=0;
    _torque=0;
    _rotor_correction_factor=0.6;
}

void Rotorpart::inititeration(float dt,float *rot)
{
    _dt=dt;
    _phi+=_omega*dt;
    while (_phi>(2*pi)) _phi-=2*pi;
    while (_phi<(0   )) _phi+=2*pi;
    float a=Math::dot3(rot,_normal);
    if(a>0)
        _alphaalt=_alpha*Math::cos(a)
        +_next90rp->getrealAlpha()*Math::sin(a);
    else
        _alphaalt=_alpha*Math::cos(a)
        +_last90rp->getrealAlpha()*Math::sin(-a);
    //calculate the rotation of the fuselage, determine
    //the part in the same direction as alpha
    //and add it ro _alphaalt
    //alpha is rotation about "normal cross dirofzentf"

    float dir[3];
    Math::cross3(_directionofcentripetalforce,_normal,dir);
    a=Math::dot3(rot,dir);
    _alphaalt -= a;
    _alphaalt= Math::clamp(_alphaalt,_alphamin,_alphamax);
}

void Rotorpart::setRotor(Rotor *rotor)
{
    _rotor=rotor;
}

void Rotorpart::setParameter(char *parametername, float value)
{
#define p(a) if (strcmp(parametername,#a)==0) _##a = value; else

    p(twist)
        p(number_of_segments)
        p(rel_len_where_incidence_is_measured)
        p(rel_len_blade_start)
        p(rotor_correction_factor)
        SG_LOG(SG_INPUT, SG_ALERT,
            "internal error in parameter set up for rotorpart: '"
            << parametername <<"'" << endl);
#undef p
}

void Rotorpart::setTorque(float torque_max_force,float torque_no_force)
{
    _torque_max_force=torque_max_force;
    _torque_no_force=torque_no_force;
}

void Rotorpart::setTorqueOfInertia(float toi)
{
    _torque_of_inertia=toi;
}

void Rotorpart::setWeight(float value)
{
    _mass=value;
}

float Rotorpart::getWeight(void)
{
    return(_mass/.453); //_mass is in kg, returns pounds 
}

void Rotorpart::setPosition(float* p)
{
    int i;
    for(i=0; i<3; i++) _pos[i] = p[i];
}

float Rotorpart::getIncidence()
{
    return(_incidence);
}

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

void Rotorpart::setPositionForceAttac(float* p)
{
    int i;
    for(i=0; i<3; i++) _posforceattac[i] = p[i];
}

void Rotorpart::getPositionForceAttac(float* out)
{
    int i;
    for(i=0; i<3; i++) out[i] = _posforceattac[i];
}

void Rotorpart::setSpeed(float* p)
{
    int i;
    for(i=0; i<3; i++) _speed[i] = p[i];
    Math::unit3(_speed,_direction_of_movement); 
}

void Rotorpart::setDirectionofZentipetalforce(float* p)
{
    int i;
    for(i=0; i<3; i++) _directionofcentripetalforce[i] = p[i];
}

void Rotorpart::setDirectionofRotorPart(float* p)
{
    int i;
    for(i=0; i<3; i++) _directionofrotorpart[i] = p[i];
}

void Rotorpart::setOmega(float value)
{
    _omega=value;
}

void Rotorpart::setPhi(float value)
{
    _phi=value;
}

void Rotorpart::setOmegaN(float value)
{
    _omegan=value;
}

void Rotorpart::setDdtOmega(float value)
{
    _ddt_omega=value;
}

void Rotorpart::setZentipetalForce(float f)
{
    _centripetalforce=f;
} 


void Rotorpart::setDelta3(float f)
{
    _delta3=f;
} 

void Rotorpart::setRelamp(float f)
{
    _relamp=f;
} 

void Rotorpart::setTranslift(float f)
{
    _translift=f;
}

void Rotorpart::setDynamic(float f)
{
    _dynamic=f;
}

void Rotorpart::setRelLenHinge(float f)
{
    _rellenhinge=f;
}

void Rotorpart::setSharedFlapHinge(bool s)
{
    _shared_flap_hinge=s;
}

void Rotorpart::setC2(float f)
{
    _c2=f;
}

void Rotorpart::setAlpha0(float f)
{
    if (f>-0.01) f=-0.01; //half a degree bending 
    _alpha0=f;
}

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

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

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

void Rotorpart::setDiameter(float f)
{
    _diameter=f;
}

float Rotorpart::getPhi()
{
    return(_phi);
}

float Rotorpart::getAlpha(int i)
{
    i=i&1;

    if (i==0)
        return _alpha*180/pi;//in Grad = 1
    else
    {
        if (_alpha2type==1) //yaw or roll
            return (getAlpha(0)-_oppositerp->getAlpha(0))/2;
        else //collective
            return (getAlpha(0)+_oppositerp->getAlpha(0)+
            _next90rp->getAlpha(0)+_last90rp->getAlpha(0))/4;
    }
}
float Rotorpart::getrealAlpha(void)
{
    return _alpha;
}

void Rotorpart::setAlphaoutput(char *text,int i)
{
    strncpy(_alphaoutputbuf[i>0],text,255);
    if (i>0) _alpha2type=i;
}

char* Rotorpart::getAlphaoutput(int i)
{
    return _alphaoutputbuf[i&1];
}

void Rotorpart::setLen(float value)
{
    _len=value;
}

void Rotorpart::setNormal(float* p)
{
    int i;
    for(i=0; i<3; i++) _normal[i] = p[i];
}

void Rotorpart::getNormal(float* out)
{
    int i;
    for(i=0; i<3; i++) out[i] = _normal[i];
}

void Rotorpart::setCollective(float pos)
{
    _collective = pos;
}

void Rotorpart::setCyclic(float pos)
{
    _cyclic = pos;
}

void Rotorpart::setlastnextrp(Rotorpart*lastrp,Rotorpart*nextrp,
    Rotorpart *oppositerp,Rotorpart*last90rp,Rotorpart*next90rp)
{
    _lastrp=lastrp;
    _nextrp=nextrp;
    _oppositerp=oppositerp;
    _last90rp=last90rp;
    _next90rp=next90rp;
}

void Rotorpart::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;
}

// Calculate the flapping angle, where zentripetal force and
//lift compensate each other
float Rotorpart::calculateAlpha(float* v_rel_air, float rho, 
    float incidence, float cyc, float alphaalt, float *torque,
    float *returnlift)
{
    float moment[3],v_local[3],v_local_scalar,lift_moment,v_flap[3],v_help[3];
    float ias;//nur f. dgb
    int i,n;
    for (i=0;i<3;i++)
        moment[i]=0;
    lift_moment=-_mass*_len*9.81; //*cos yaw * cos roll
    *torque=0;//
    if((_nextrp==NULL)||(_lastrp==NULL)||(_rotor==NULL)) 
        return 0.0;//not initialized. Can happen during startupt of flightgear
    if (returnlift!=NULL) *returnlift=0;
    float flap_omega=(_next90rp->getrealAlpha()-_last90rp->getrealAlpha())
        *_omega / pi;
    float local_width=_diameter*(1-_rel_len_blade_start)/2.
        /(float (_number_of_segments));
    for (n=0;n<_number_of_segments;n++)
    {
        float rel = (n+.5)/(float (_number_of_segments));
        float r= _diameter *0.5 *(rel*(1-_rel_len_blade_start)
            +_rel_len_blade_start);
        float local_incidence=incidence+_twist *rel -
            _twist *_rel_len_where_incidence_is_measured;
        float local_chord = _rotor->getChord()*rel+_rotor->getChord()
            *_rotor->getTaper()*(1-rel);
        float A = local_chord * local_width;
        //calculate the local air speed and the incidence to this speed;
        Math::mul3(_omega * r , _direction_of_movement , v_local);

        // add speed component due to flapping
        Math::mul3(flap_omega * r,_normal,v_flap);
        Math::add3(v_flap,v_local,v_local);
        Math::sub3(v_rel_air,v_local,v_local); 
        //v_local is now the total airspeed at the blade 
        //apparent missing: calculating the local_wind = v_rel_air at 
        //every point of the rotor. It differs due to aircraft-rotation
        //it is considered in v_flap

        //substract now the component of the air speed parallel to 
        //the blade;
        Math::mul3(Math::dot3(v_local,_directionofrotorpart),
           _directionofrotorpart,v_help);
        Math::sub3(v_local,v_help,v_local);

        //split into direction and magnitude
        v_local_scalar=Math::mag3(v_local);
        if (v_local_scalar!=0)
            //Math::unit3(v_local,v_help);
            Math::mul3(1/v_local_scalar,v_local,v_help);
        float incidence_of_airspeed = Math::asin(Math::clamp(
            Math::dot3(v_help,_normal),-1,1)) + local_incidence;
        ias = incidence_of_airspeed;

        //reduce the ias (Prantl factor)
        float prantl_factor=2/pi*Math::acos(Math::exp(
            -_rotor->getNumberOfBlades()/2.*(1-rel)
             *Math::sqrt(1+1/Math::sqr(Math::tan(
               pi/2-Math::abs(incidence_of_airspeed-local_incidence))))));
        incidence_of_airspeed = (incidence_of_airspeed+
            _rotor->getAirfoilIncidenceNoLift())*prantl_factor
            *_rotor_correction_factor-_rotor->getAirfoilIncidenceNoLift();
        ias = incidence_of_airspeed;
        float lift_wo_cyc = _rotor->getLiftCoef(incidence_of_airspeed
            -cyc*_rotor_correction_factor*prantl_factor,v_local_scalar)
            * v_local_scalar * v_local_scalar * A *rho *0.5;
        float lift_with_cyc = 
            _rotor->getLiftCoef(incidence_of_airspeed,v_local_scalar)
            * v_local_scalar * v_local_scalar *A *rho*0.5;
        float lift=lift_wo_cyc+_relamp*(lift_with_cyc-lift_wo_cyc);
        //take into account that the rotor is a resonant system where
        //the cyclic input hase increased result
        float drag = -_rotor->getDragCoef(incidence_of_airspeed,v_local_scalar) 
            * v_local_scalar * v_local_scalar * A *rho*0.5;
        float angle = incidence_of_airspeed - incidence; 
        //angle between blade movement caused by rotor-rotation and the
        //total movement of the blade

        /* 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
        lift_moment += r*(lift * Math::cos(angle) 
            - drag * Math::sin(angle));
        *torque     += r*(drag * Math::cos(angle) 
            + lift * Math::sin(angle));
            */
        lift_moment += r*(lift * (1-angle*angle) 
            - drag * angle);
        *torque     += r*(drag * (1-angle*angle) 
            + lift * angle);
        
        if (returnlift!=NULL) *returnlift+=lift;
    }
    //as above, use 1st order approximation
    //float alpha=Math::atan2(lift_moment,_centripetalforce * _len); 
    float alpha;
    if (_shared_flap_hinge)
    {
        float div=0;
        if (Math::abs(_alphaalt) >1e-6)
            div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0*(_alphaalt+_oppositerp->getAlphaAlt())/(2.0*_alphaalt));
        if (Math::abs(div)>1e-6)
            alpha=lift_moment/div;
        else if(Math::abs(_alphaalt+_oppositerp->getAlphaAlt())>1e-6)
        {
            float div=(_centripetalforce * _len - _mass * _len * 9.81 *0.5)*(_alphaalt+_oppositerp->getAlphaAlt());
            if (Math::abs(div)>1e-6)
                alpha=_oppositerp->getAlphaAlt()+lift_moment/div*_alphaalt;
        }
        else
            alpha=_alphaalt;
    }
    else
    {
        float div=(_centripetalforce * _len - _mass * _len * 9.81 /_alpha0);
        if (Math::abs(div)>1e-6)
            alpha=lift_moment/div;
        else
            alpha=_alphaalt;
    }
 
    return (alpha);
}

// Calculate the aerodynamic force given a wind vector v (in the
// aircraft's "local" coordinates) and an air density rho.  Returns a
// torque about the Y axis, too.
void Rotorpart::calcForce(float* v, float rho,  float* out, float* torque,
    float* torque_scalar)
{
    if (_compiled!=1)
    {
        for (int i=0;i<3;i++)
            torque[i]=out[i]=0;
        *torque_scalar=0;
        return;
    }
    _centripetalforce=_mass*_len*_omega*_omega;
    float vrel[3],vreldir[3];
    Math::sub3(_speed,v,vrel);
    float scalar_torque=0;
    Math::unit3(vrel,vreldir);//direction of blade-movement rel. to air
    //Angle of blade which would produce no vertical force (where the 
    //effective incidence is zero)

    float cyc=_cyclic;
    float col=_collective;
    if (_shared_flap_hinge)
        _incidence=(col+cyc)-_delta3*0.5*(_alphaalt-_oppositerp->getAlphaAlt());
    else
        _incidence=(col+cyc)-_delta3*_alphaalt;
    //the incidence of the rotorblade due to control input reduced by the
    //delta3 effect, see README.YASIM
    //float beta=_relamp*cyc+col; 
    //the incidence of the rotorblade which is used for the calculation

    float alpha,factor; //alpha is the flapping angle
    //the new flapping angle will be the old flapping angle
    //+ factor *(alpha - "old flapping angle")
    alpha=calculateAlpha(v,rho,_incidence,cyc,0,&scalar_torque);
    alpha=Math::clamp(alpha,_alphamin,_alphamax);
    //the incidence is a function of alpha (if _delta* != 0)
    //Therefore missing: wrap this function in an integrator
    //(runge kutta e. g.)

    factor=_dt*_dynamic;
    if (factor>1) factor=1;

    float dirblade[3];
    Math::cross3(_normal,_directionofcentripetalforce,dirblade);
    float vblade=Math::abs(Math::dot3(dirblade,v));

    alpha=_alphaalt+(alpha-_alphaalt)*factor;
    _alpha=alpha;
    float meancosalpha=(1*Math::cos(_last90rp->getrealAlpha())
        +1*Math::cos(_next90rp->getrealAlpha())
        +1*Math::cos(_oppositerp->getrealAlpha())
        +1*Math::cos(alpha))/4;
    float schwenkfactor=1-(Math::cos(_lastrp->getrealAlpha())-meancosalpha)*_rotor->getNumberOfParts()/4;

    //missing: consideration of rellenhinge
    float xforce = Math::cos(alpha)*_centripetalforce;
    float zforce = schwenkfactor*Math::sin(alpha)*_centripetalforce;
    *torque_scalar=scalar_torque;
    scalar_torque+= 0*_ddt_omega*_torque_of_inertia;
    float thetorque = scalar_torque;
    int i;
    float f=_rotor->getCcw()?1:-1;
    for(i=0; i<3; i++) {
        _last_torque[i]=torque[i] = f*_normal[i]*thetorque;
        out[i] = _normal[i]*zforce*_rotor->getLiftFactor()
            +_directionofcentripetalforce[i]*xforce;
    }
}

void Rotorpart::getAccelTorque(float relaccel,float *t)
{
    int i;
    float f=_rotor->getCcw()?1:-1;
    for(i=0; i<3; i++) {
        t[i]=f*-1* _normal[i]*relaccel*_omegan* _torque_of_inertia;// *_omeagan ?
        _rotor->addTorque(-relaccel*_omegan* _torque_of_inertia);
    }
}
std::ostream &  operator<<(std::ostream & out, const Rotorpart& rp)
{
#define i(x) << #x << ":" << rp.x << endl
#define iv(x) << #x << ":" << rp.x[0] << ";" << rp.x[1] << ";" <<rp.x[2] << ";" << endl
    out << "Writing Info on Rotorpart " << endl
        i( _dt)
        iv( _last_torque)
        i( _compiled)
        iv( _pos)    // position in local coords
        iv( _posforceattac)    // position in local coords
        iv( _normal) //direcetion of the rotation axis
        i( _torque_max_force)
        i( _torque_no_force)
        iv( _speed)
        iv( _direction_of_movement)
        iv( _directionofcentripetalforce)
        iv( _directionofrotorpart)
        i( _centripetalforce)
        i( _cyclic)
        i( _collective)
        i( _delta3)
        i( _dynamic)
        i( _translift)
        i( _c2)
        i( _mass)
        i( _alpha)
        i( _alphaalt)
        i( _alphamin) i(_alphamax) i(_alpha0) i(_alpha0factor)
        i( _rellenhinge)
        i( _relamp)
        i( _omega) i(_omegan) i(_ddt_omega)
        i( _phi)
        i( _len)
        i( _incidence)
        i( _twist) //outer incidence = inner inner incidence + _twist
        i( _number_of_segments)
        i( _rel_len_where_incidence_is_measured)
        i( _rel_len_blade_start)
        i( _diameter)
        i( _torque_of_inertia)
        i( _torque) 
        i (_alphaoutputbuf[0])
        i (_alphaoutputbuf[1])
        i( _alpha2type)
        i(_rotor_correction_factor)
    << endl;
#undef i
#undef iv
    return out;  
}
}; // namespace yasim