#include <QDebug>
#include "interpolator.h"
#include <cmath>
#include <QFile>
#include <QTextStream>
#include <qtplchandler.h>
#include "global_defines.h"
#include "Slope.h"
#include "Logger.h"

#ifdef VISU_EXTERN
char * FILENAME_BIN_X = "out_x.bin";
char * FILENAME_BIN_Y = "out_y.bin";
#else
char * FILENAME_BIN_X = "/tmp/out_x.bin";
char * FILENAME_BIN_Y = "/tmp/out_y.bin";
#endif


Interpolator::Interpolator(double posX, double posY, FahrprofilData *dataX, FahrprofilData *dataY, WidgetData &widgetData):
    m_position_X(posX),
    m_position_Y(posY),
    m_factor_old(0),
    s_wurf_y(0),
    m_zwichenpause(0),
    m_factorVx(0.75),
    m_beschleunigungswegX(0),
    m_beschleunigungswegY(0),
    fbd_X(dataX),
    fbd_Y(dataY),
    m_widgetData(widgetData)
{
}

double factor_t = 2.0;

void Interpolator::calc(double h1, double h2, double offset_y, double offset_y2,
                        double n1, double n2, double s_max, double a_ymax,
                        double a_xmax, double pause, double zwischenpause)
{
    int outCount=0;
    if(h1>0.0001){
        m_zwichenpause=zwischenpause;
        // Dateiausgabe ///////////////////////
    #ifndef Q_WS_QWS
        QFile f("out.dat");
        f.open(QFile::WriteOnly);
        QTextStream out(&f);
    #else
    //    QFile f("/tmp/out.dat");
        ///////////////////////////////////////
    #endif

        //Berechnung Wuerfe nach vorn ///////////////////////////////////
        calcInit(h1, n1, s_max-m_position_X, a_ymax, a_xmax);

        double last_t=0;
        double last_t_y=0;
        int outCountLast=0;
        Slope slope;
        double pos_abs_x=0;
        double pos_abs_y=0;
        bool richtung_y=false;
        bool richtung_x=false;
        double offset_t=0;

        //Flugzeit berechnen
        double t_f = 2.0*sqrt(2.0*h1/g);
        int state=0;
        double offset_t_flug=0;
        double t0=0;
        double x = 0;
        double y = 0;
        int count=1;

        if(s_max>0.0001){
            unsigned int cycles=0;
            if(n1==0){
                cycles = pscd_x.nCycles+(pscd_x.T1+pscd_x.T2+pscd_x.T3)/0.001;
            }else{
                cycles = pscd_x.nCycles>pscd_y.nCycles ? pscd_x.nCycles: pscd_y.nCycles;
            }
            for(unsigned int nCycle=0; nCycle<=cycles; nCycle++, outCount++){
                if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                    return;
                double t=nCycle*0.001;
                switch(state){
                case 0://Start der Positionierungen, mit evtl. Verzoegerung in Y-Richtung
                {
                    richtung_y=false;
                    richtung_x=false;
                    if((pscd_x.T1+pscd_x.T2+pscd_x.T3)>(pscd_y.T1+pscd_y.T2+pscd_y.T3)){
                        t0=(pscd_x.T1+pscd_x.T2+pscd_x.T3)-(pscd_y.T1+pscd_y.T2+pscd_y.T3);
                    }
                    t0=(pscd_x.T1+pscd_x.T2+pscd_x.T3)*factor_t;
                    if(count>1)
                        t0=0;

                    if((t)>=t0){
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t-t0-offset_t);
                    }else{
                        scd_y.out_current_a=0;
                        scd_y.out_current_v=0;
                    }

                    //Ab hier beginnt die Flugkurve
                    if(t>=(pscd_y.T1+pscd_y.T2+pscd_y.T3+pscd_y.T4+offset_t+t0)){
                        offset_t_flug=t;
                        state=10;
                    }
                    break;
                }
                case 10:
                {
                    if(t>=t0){
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t-t0-offset_t);
                    }else{
                        scd_y.out_current_a=0;
                        scd_y.out_current_v=0;
                    }

                    x = scd_x.in_vn*(t-offset_t_flug);
                    y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y*(count-1);
                    m_widgetData.point.push_back(QPointF(x, y));

                    if(scd_y.out_stop){
                        if((s_wurf_y-offset_y)>0){
                            richtung_y=true;
                        }else{
                            richtung_y=false;
                        }

                        scd_y.in_s = fabs(s_wurf_y-offset_y);

                        slope.slopeCalcInit(pscd_y, scd_y);
                        offset_t=t;

                        state=20;
                    }
                    break;
                }
                case 20:
                {
                    slope.slopeCalcExe_for_time(pscd_y, scd_y, t-offset_t);

                    x = scd_x.in_vn*(t-offset_t_flug);
                    y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y*(count-1);
                    m_widgetData.point.push_back(QPointF(x, y));

                    if(scd_y.out_stop){
                        offset_t=t;
                        richtung_y=false;
                        state=30;
                    }
                    break;
                }
                case 30:
                {
                    if(t>=(offset_t_flug+t_f+m_zwichenpause)){
                        if(count<n1){
                            scd_y.in_s = s_wurf_y;
                            slope.slopeCalcInit(pscd_y, scd_y);
                            count++;
                            offset_t=t;
                            state=0;
                        }else{
                            offset_t=t;
                            state=50;
                        }
                        x=0;
                        y=offset_y*(count-1);
                        m_widgetData.point.push_back(QPointF(x, y));

                    }else{
                        if(t<=offset_t_flug+t_f){
                            x = scd_x.in_vn*(t-offset_t_flug);
                            y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y*(count-1);
                            m_widgetData.point.push_back(QPointF(x, y));
                        }else{
                            y=0;
                            m_widgetData.point.push_back(QPointF(x, y));
                        }
                    }
                    break;
                }
                case 40:
                {
                    x = scd_x.in_vn*(t-offset_t_flug);
                    y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y*(count-1);
                    m_widgetData.point.push_back(QPointF(x, y));
                    slope.slopeCalcExe_for_time(pscd_y, scd_y, t-offset_t);
                    if(scd_y.out_stop){
                        offset_t=t;
                        state=30;
                    }
                    break;
                }
                default:
                    break;
                }


                //Sollwerte fuer X berechnen
                slope.slopeCalcExe_for_time(pscd_x, scd_x, t);

                double v=0;
                double a=0;

                //Richungsabhaengige Sollgeschwindigeit und -beschleunigung ausgeben
                if(richtung_y){
                    if(!scd_y.out_stop){
                        pos_abs_y -= scd_y.out_delta_s;
                        v=-scd_y.out_current_v;
                        a=-scd_y.out_current_a;
                    }
                }else{
                    if(!scd_y.out_stop){
                        pos_abs_y += scd_y.out_delta_s;
                        v=scd_y.out_current_v;
                        a=scd_y.out_current_a;
                    }
                }
                if(scd_y.out_stop){
                    v=0;
                    a=0;
                }
                if(scd_x.out_stop){
                    scd_x.out_current_a=0;
                    scd_x.out_current_v=0;
                }

                //X-Achse Ausgabedaten schreiben
                if(!scd_x.out_stop){
                    pos_abs_x += scd_x.out_delta_s;
                }
                fbd_X->beschleunigung[outCount]  = scd_x.out_current_a;
                fbd_X->geschwindigkeit[outCount] = scd_x.out_current_v;
                fbd_X->position[outCount]        = m_position_X+pos_abs_x/1000.0;
                fbd_X->kraftgrenze[outCount]     = 0;

                //Ausgabedaten fuer Y-Achse schreiben
                fbd_Y->beschleunigung[outCount]  = a;
                fbd_Y->geschwindigkeit[outCount] = v;
                fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                fbd_Y->kraftgrenze[outCount]     = 0;

        #ifndef Q_WS_QWS
                //                       t,  s,  v,  a,  s,  v,  a
                out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                       .arg(t)
                       .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                       .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                       .arg(x).arg(y);
        #endif
                last_t=t;
                outCountLast=outCount;
            }

            outCountLast--;
            ////////////////////////////////////////////////////////////////////
            // Pause ///////////////////////////////////////////////////////////
            double last_t_pause=0;
            for(double t=0; t<=pause; t+=0.001, outCount++){
                if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                    return;

                fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                fbd_X->geschwindigkeit[outCount] = 0;
                fbd_X->beschleunigung[outCount]  = 0;
                fbd_X->kraftgrenze[outCount]     = 0;

                fbd_Y->position[outCount]        = fbd_Y->position[outCountLast];
                fbd_Y->beschleunigung[outCount]  = 0;
                fbd_Y->geschwindigkeit[outCount] = 0;
                fbd_Y->kraftgrenze[outCount]     = 0;
        #ifndef Q_WS_QWS
                //                       t,  s,  v,  a,  s,  v,  a
                out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                       .arg(last_t+t).arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                       .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                       .arg(x)
                       .arg(y);
        #endif
                last_t_pause=t;
            }
            ////////////////////////////////////////////////////////////////////
            // Wenn Y-Achsposition >0, dann auf Startposition zurück fahren
            if(((pos_abs_y/1000)-m_position_Y)>0.0005){
                double v_x_save = scd_y.in_vn;
                scd_y.in_vn = 5.0;
                scd_y.in_s = (pos_abs_y/1000);
                richtung_y=true;
                slope.slopeCalcInit(pscd_y, scd_y);
                for(double t=0; t<=pause; t+=0.001, outCount++){

                    if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                        return;

                    slope.slopeCalcExe_for_time(pscd_y, scd_y, t);

                    double v=0;
                    double a=0;
                    //Richungsabhaengige Sollgeschwindigeit und -beschleunigung ausgeben
                    if(richtung_y){
                        if(!scd_y.out_stop){
                            pos_abs_y -= scd_y.out_delta_s;
                            v=-scd_y.out_current_v;
                            a=-scd_y.out_current_a;
                        }
                    }else{
                        if(!scd_y.out_stop){
                            pos_abs_y += scd_y.out_delta_s;
                            v=scd_y.out_current_v;
                            a=scd_y.out_current_a;
                        }
                    }
                    if(scd_y.out_stop){
                        v=0;
                        a=0;
                    }

                    fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                    fbd_X->geschwindigkeit[outCount] = 0;
                    fbd_X->beschleunigung[outCount]  = 0;
                    fbd_X->kraftgrenze[outCount]     = 0;

                    fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                    fbd_Y->beschleunigung[outCount]  = a;
                    fbd_Y->geschwindigkeit[outCount] = v;
                    fbd_Y->kraftgrenze[outCount]     = 0;
        #ifndef Q_WS_QWS
                    //                       t,  s,  v,  a,  s,  v,  a
                    out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                           .arg(t+last_t+last_t_pause)
                           .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                           .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                           .arg(x).arg(y);
        #endif
                    last_t_y=t;
                    outCountLast=outCount;
                }
                scd_y.in_vn = v_x_save;
            }
            outCountLast--;
            if(h2>0){
                ////////////////////////////////////////////////////////////////////
                //Berechnungen Wurf zurueck ////////////////////////////////////////
                calcInit(h2, n2, s_max-m_position_X, a_ymax, a_xmax);
                t_f = 2.0*sqrt(2.0*h2/g);//(s_max - pscd_y.s1*3)/n/scd_x.in_vn;
                offset_t=0;
                count=1;
                state=0;
                for(double t=0.0; t<=(pscd_x.Treal+0.001); t+=0.001, outCount++){
                    if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                        return;

                    switch(state){
                    case 0://Start der Positionierungen, mit evtl. Verzoegerung in Y-Richtung
                    {
                        richtung_y=false;
                        richtung_x=true;
                        if((pscd_x.T1+pscd_x.T2+pscd_x.T3)>(pscd_y.T1+pscd_y.T2+pscd_y.T3)){
                            t0=(pscd_x.T1+pscd_x.T2+pscd_x.T3)-(pscd_y.T1+pscd_y.T2+pscd_y.T3);
                        }

                        t0=(pscd_x.T1+pscd_x.T2+pscd_x.T3)*factor_t;
                        if(count>1)
                            t0=0;

                        if((t)>=t0){
                            slope.slopeCalcExe_for_time(pscd_y, scd_y, t-t0-offset_t);
                        }else{
                            scd_y.out_current_a=0;
                            scd_y.out_current_v=0;
                        }

                        //Ab hier beginnt die Flugkurve
                        if(t>=(pscd_y.T1+pscd_y.T2+pscd_y.T3+pscd_y.T4+offset_t+t0)){
                            offset_t_flug=t;
                            state=10;
                        }
                        break;
                    }
                    case 10:
                    {
                        if(t>=t0)
                            slope.slopeCalcExe_for_time(pscd_y, scd_y, t-t0-offset_t);

                        x = scd_x.in_vn*(t-offset_t_flug);
                        y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y2*(count-1);

                        if(scd_y.out_stop){
                            if((s_wurf_y-offset_y2)>0){
                                richtung_y=true;
                            }else{
                                richtung_y=false;
                            }
                            scd_y.in_s = fabs(s_wurf_y-offset_y2);

                            slope.slopeCalcInit(pscd_y, scd_y);
                            offset_t=t;
                            state=20;
                        }
                        break;
                    }
                    case 20:
                    {
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t-offset_t);

                        x = scd_x.in_vn*(t-offset_t_flug);
                        y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y2*(count-1);

                        if(scd_y.out_stop){
                            offset_t=t;
                            richtung_y=false;
                            state=30;
                        }
                        break;
                    }
                    case 30:
                    {
                        if(t>=(offset_t_flug+t_f+m_zwichenpause)){
                            if(count<n2){
                                scd_y.in_s = s_wurf_y;
                                slope.slopeCalcInit(pscd_y, scd_y);
                                count++;
                                offset_t=t;
                                state=0;
                            }else{
                                offset_t=t;
                                state=50;
                            }
                            x=0;
                            y=offset_y2*(count-1);
                        }else{
                            if(t<=offset_t_flug+t_f){
                                x = scd_x.in_vn*(t-offset_t_flug);
                                y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y2*(count-1);
                            }else{
                                y=0;
                            }
                        }
                        break;
                    }
                    case 40:
                    {
                        x = scd_x.in_vn*(t-offset_t_flug);
                        y = -0.5*g*(t-offset_t_flug)*(t-offset_t_flug)+scd_y.in_vn*(t-offset_t_flug)+offset_y2*(count-1);
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t-offset_t);
                        if(scd_y.out_stop){
                            offset_t=t;
                            state=30;
                        }
                        break;
                    }
                    default:
                        break;
                    }
                    //Sollwerte fuer X berechnen
                    slope.slopeCalcExe_for_time(pscd_x, scd_x, t);

                    double v=0;
                    double a=0;

                    //Richungsabhaengige Sollgeschwindigeit und -beschleunigung ausgeben
                    if(!richtung_y){
                        if(!scd_y.out_stop){
                            pos_abs_y += scd_y.out_delta_s;
                            v=scd_y.out_current_v;
                            a=scd_y.out_current_a;
                        }
                    }else{
                        if(!scd_y.out_stop){
                            pos_abs_y -= scd_y.out_delta_s;
                            v=-scd_y.out_current_v;
                            a=-scd_y.out_current_a;
                        }
                    }
                    if(scd_y.out_stop){
                        v=0;
                        a=0;
                    }

                    //X-Achse Ausgabedaten schreiben
                    if(!richtung_x){
                        if(!scd_x.out_stop){
                            pos_abs_x += scd_x.out_delta_s;
                        }
                    }else{
                        if(!scd_x.out_stop){
                            pos_abs_x -= scd_x.out_delta_s;
                            scd_x.out_current_a = -scd_x.out_current_a;
                            scd_x.out_current_v = -scd_x.out_current_v;
                        }
                    }

                    if(scd_x.out_stop){
                        scd_x.out_current_a=0;
                        scd_x.out_current_v=0;
                    }

                    fbd_X->beschleunigung[outCount]  = scd_x.out_current_a;
                    fbd_X->geschwindigkeit[outCount] = scd_x.out_current_v;
                    fbd_X->position[outCount]        = m_position_X+pos_abs_x/1000.0;
                    fbd_X->kraftgrenze[outCount]     = 0;

                    //Ausgabedaten fuer Y-Achse schreiben
                    fbd_Y->beschleunigung[outCount]  = a;
                    fbd_Y->geschwindigkeit[outCount] = v;
                    fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                    fbd_Y->kraftgrenze[outCount]     = 0;

        #ifndef Q_WS_QWS
                    //                       t,  s,  v,  a,  s,  v,  a
                    out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                           .arg(t+last_t_pause+last_t+last_t_y)
                           .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                           .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                           .arg(x).arg(y);
        #endif
                }
            }
        }else{
            //double t=0;
            double t1=0;
            double t2=0;
            double last_t_pause=0;
            double last_t_n1=0;
            double last_t2=0;
            for(int i=0; i<n1; ++i){
                scd_y.in_s=s_wurf_y;
                slope.slopeCalcInit(pscd_y, scd_y);
                t1=pscd_y.Tcalc;
                richtung_y=false;
                for(unsigned int nCycle=0; nCycle<=pscd_y.nCycles; nCycle++, outCount++){
                    double t=nCycle*0.001;
                    slope.slopeCalcExe_for_time(pscd_y, scd_y, t);
                    double v=0;
                    double a=0;
                    if(richtung_y){
                        if(!scd_y.out_stop){
                            pos_abs_y -= scd_y.out_delta_s;
                            v=-scd_y.out_current_v;
                            a=-scd_y.out_current_a;
                        }
                    }else{
                        if(!scd_y.out_stop){
                            pos_abs_y += scd_y.out_delta_s;
                            v=scd_y.out_current_v;
                            a=scd_y.out_current_a;
                        }
                    }
                    if(scd_y.out_stop){
                        v=0;
                        a=0;
                    }

                    fbd_X->beschleunigung[outCount]  = 0;
                    fbd_X->geschwindigkeit[outCount] = 0;
                    fbd_X->position[outCount]        = m_position_X;
                    fbd_X->kraftgrenze[outCount]     = 0;

                    //Ausgabedaten fuer Y-Achse schreiben
                    fbd_Y->beschleunigung[outCount]  = a;
                    fbd_Y->geschwindigkeit[outCount] = v;
                    fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                    fbd_Y->kraftgrenze[outCount]     = 0;
    #ifndef Q_WS_QWS
                    //                       t,  s,  v,  a,  s,  v,  a
                    out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                           .arg(t+(t1+t2)*i+m_zwichenpause*i)
                           .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                           .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                           .arg(x).arg(y);
    #endif
                    outCountLast=outCount;
                    last_t=t;
                }

                if((s_wurf_y-offset_y)>0){
                    richtung_y=true;
                }else{
                    richtung_y=false;
                }

                scd_y.in_s = fabs(s_wurf_y-offset_y);

                slope.slopeCalcInit(pscd_y, scd_y);
                t2=pscd_y.Tcalc;
                richtung_y=true;
                for(unsigned int nCycle=0; nCycle<=pscd_y.nCycles; nCycle++, outCount++){
                    double t=nCycle*0.001;
                    slope.slopeCalcExe_for_time(pscd_y, scd_y, t);
                    double v=0;
                    double a=0;

                    if(richtung_y){
                        if(!scd_y.out_stop){
                            pos_abs_y -= scd_y.out_delta_s;
                            v=-scd_y.out_current_v;
                            a=-scd_y.out_current_a;
                        }
                    }else{
                        if(!scd_y.out_stop){
                            pos_abs_y += scd_y.out_delta_s;
                            v=scd_y.out_current_v;
                            a=scd_y.out_current_a;
                        }
                    }
                    if(scd_y.out_stop){
                        v=0;
                        a=0;
                    }

                    fbd_X->beschleunigung[outCount]  = 0;
                    fbd_X->geschwindigkeit[outCount] = 0;
                    fbd_X->position[outCount]        = m_position_X;
                    fbd_X->kraftgrenze[outCount]     = 0;

                    //Ausgabedaten fuer Y-Achse schreiben
                    fbd_Y->beschleunigung[outCount]  = a;
                    fbd_Y->geschwindigkeit[outCount] = v;
                    fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                    fbd_Y->kraftgrenze[outCount]     = 0;
    #ifndef Q_WS_QWS
                    //                       t,  s,  v,  a,  s,  v,  a
                    out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                           .arg(t+last_t+(t1+t2)*i+m_zwichenpause*i)
                           .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                           .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                           .arg(x).arg(y);
    #endif
                    outCountLast=outCount;
                    last_t2=t;
                }
                ////////////////////////////////////////////////////////////////////
                // Zwischen - Pause ///////////////////////////////////////////////////////////
                last_t_pause=0;
                if(i<n1-1){
                    outCountLast--;
                    for(double t=0; t<=m_zwichenpause; t+=0.001, outCount++){
                        if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                            return;

                        fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                        fbd_X->geschwindigkeit[outCount] = 0;
                        fbd_X->beschleunigung[outCount]  = 0;
                        fbd_X->kraftgrenze[outCount]     = 0;

                        fbd_Y->position[outCount]        = fbd_Y->position[outCountLast];
                        fbd_Y->beschleunigung[outCount]  = 0;
                        fbd_Y->geschwindigkeit[outCount] = 0;
                        fbd_Y->kraftgrenze[outCount]     = 0;
    #ifndef Q_WS_QWS
                        //                       t,  s,  v,  a,  s,  v,  a
                        out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                               .arg(last_t+last_t2+t+(t1+t2)*i+m_zwichenpause*i).arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                               .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                               .arg(x)
                               .arg(y);
    #endif
                        last_t_pause=t;
                    }
                }
                last_t_n1=last_t_pause+last_t2+last_t+(t1+t2)*i+m_zwichenpause*i;
            }
            t1=0;
            t2=0;
            double last_t3=0;
            last_t_pause=0;
            if(h2>0.0001){
                outCountLast--;
                for(double tt=0; tt<=pause; tt+=0.001, outCount++){
                    if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                        return;

                    fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                    fbd_X->geschwindigkeit[outCount] = 0;
                    fbd_X->beschleunigung[outCount]  = 0;
                    fbd_X->kraftgrenze[outCount]     = 0;

                    fbd_Y->position[outCount]        = fbd_Y->position[outCountLast];
                    fbd_Y->beschleunigung[outCount]  = 0;
                    fbd_Y->geschwindigkeit[outCount] = 0;
                    fbd_Y->kraftgrenze[outCount]     = 0;
    #ifndef Q_WS_QWS
                    //                       t,  s,  v,  a,  s,  v,  a
                    out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                           .arg(last_t_n1+tt).arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                           .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                           .arg(x)
                           .arg(y);
    #endif
                    last_t_pause=tt;
                }
                last_t_n1 +=last_t_pause;
                ////////////////////////////////////////////////////////////////////
                // Wenn Y-Achsposition >0, dann auf Startposition zurück fahren
                if(((pos_abs_y/1000))>0.0005){
                    double v_x_save = scd_y.in_vn;
                    scd_y.in_vn= 5.0;
                    scd_y.in_s = (pos_abs_y/1000);
                    richtung_y=true;
                    slope.slopeCalcInit(pscd_y, scd_y);
                    for(double tt=0; tt<=pscd_y.Tcalc; tt+=0.001, outCount++){

                        if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                            return;

                        slope.slopeCalcExe_for_time(pscd_y, scd_y, tt);

                        double v=0;
                        double a=0;
                        //Richungsabhaengige Sollgeschwindigeit und -beschleunigung ausgeben
                        if(richtung_y){
                            if(!scd_y.out_stop){
                                pos_abs_y -= scd_y.out_delta_s;
                                v=-scd_y.out_current_v;
                                a=-scd_y.out_current_a;
                            }
                        }else{
                            if(!scd_y.out_stop){
                                pos_abs_y += scd_y.out_delta_s;
                                v=scd_y.out_current_v;
                                a=scd_y.out_current_a;
                            }
                        }
                        if(scd_y.out_stop){
                            v=0;
                            a=0;
                        }

                        fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                        fbd_X->geschwindigkeit[outCount] = 0;
                        fbd_X->beschleunigung[outCount]  = 0;
                        fbd_X->kraftgrenze[outCount]     = 0;

                        fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                        fbd_Y->beschleunigung[outCount]  = a;
                        fbd_Y->geschwindigkeit[outCount] = v;
                        fbd_Y->kraftgrenze[outCount]     = 0;
            #ifndef Q_WS_QWS
                        //                       t,  s,  v,  a,  s,  v,  a
                        out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                               .arg(tt+last_t_n1)
                               .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                               .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                               .arg(x).arg(y);
            #endif
                        last_t3=tt;
                        outCountLast=outCount;
                    }
                    scd_y.in_vn = v_x_save;
                }
                last_t_n1 += last_t3;
                last_t=0;
                calcInit(h2, n2, s_max-m_position_X, a_ymax, a_xmax);
                for(int i=0; i<n2; ++i){
                    scd_y.in_s=s_wurf_y;
                    slope.slopeCalcInit(pscd_y, scd_y);
                    t1=pscd_y.Tcalc;
                    richtung_y=false;
                    for(unsigned int nCycle=0; nCycle<=pscd_y.nCycles; nCycle++, outCount++){
                        double t=nCycle*0.001;
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t);
                        double v=0;
                        double a=0;
                        if(richtung_y){
                            if(!scd_y.out_stop){
                                pos_abs_y -= scd_y.out_delta_s;
                                v=-scd_y.out_current_v;
                                a=-scd_y.out_current_a;
                            }
                        }else{
                            if(!scd_y.out_stop){
                                pos_abs_y += scd_y.out_delta_s;
                                v=scd_y.out_current_v;
                                a=scd_y.out_current_a;
                            }
                        }
                        if(scd_y.out_stop){
                            v=0;
                            a=0;
                        }

                        fbd_X->beschleunigung[outCount]  = 0;
                        fbd_X->geschwindigkeit[outCount] = 0;
                        fbd_X->position[outCount]        = m_position_X;
                        fbd_X->kraftgrenze[outCount]     = 0;

                        //Ausgabedaten fuer Y-Achse schreiben
                        fbd_Y->beschleunigung[outCount]  = a;
                        fbd_Y->geschwindigkeit[outCount] = v;
                        fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                        fbd_Y->kraftgrenze[outCount]     = 0;
        #ifndef Q_WS_QWS
                        //                       t,  s,  v,  a,  s,  v,  a
                        out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                               .arg(t+last_t_n1+(t1+t2)*i+m_zwichenpause*i)
                               .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                               .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                               .arg(x).arg(y);
        #endif
                        outCountLast=outCount;
                        last_t=t;
                    }

                    if((s_wurf_y-offset_y2)>0){
                        richtung_y=true;
                    }else{
                        richtung_y=false;
                    }

                    scd_y.in_s = fabs(s_wurf_y-offset_y2);

                    slope.slopeCalcInit(pscd_y, scd_y);
                    t2=pscd_y.Tcalc;
                    richtung_y=true;
                    for(unsigned int nCycle=0; nCycle<=pscd_y.nCycles; nCycle++, outCount++){
                        double t=nCycle*0.001;
                        slope.slopeCalcExe_for_time(pscd_y, scd_y, t);
                        double v=0;
                        double a=0;

                        if(richtung_y){
                            if(!scd_y.out_stop){
                                pos_abs_y -= scd_y.out_delta_s;
                                v=-scd_y.out_current_v;
                                a=-scd_y.out_current_a;
                            }
                        }else{
                            if(!scd_y.out_stop){
                                pos_abs_y += scd_y.out_delta_s;
                                v=scd_y.out_current_v;
                                a=scd_y.out_current_a;
                            }
                        }
                        if(scd_y.out_stop){
                            v=0;
                            a=0;
                        }

                        fbd_X->beschleunigung[outCount]  = 0;
                        fbd_X->geschwindigkeit[outCount] = 0;
                        fbd_X->position[outCount]        = m_position_X;
                        fbd_X->kraftgrenze[outCount]     = 0;

                        //Ausgabedaten fuer Y-Achse schreiben
                        fbd_Y->beschleunigung[outCount]  = a;
                        fbd_Y->geschwindigkeit[outCount] = v;
                        fbd_Y->position[outCount]        = m_position_Y+pos_abs_y/1000.0;
                        fbd_Y->kraftgrenze[outCount]     = 0;
        #ifndef Q_WS_QWS
                        //                       t,  s,  v,  a,  s,  v,  a
                        out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                               .arg(t+last_t+last_t_n1+(t1+t2)*i+m_zwichenpause*i)
                               .arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                               .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                               .arg(x).arg(y);
        #endif
                        outCountLast=outCount;
                    }
                    ////////////////////////////////////////////////////////////////////
                    // Pause ///////////////////////////////////////////////////////////
                    if(i<n2-1){
                        outCountLast--;
                        for(double t=0; t<=m_zwichenpause; t+=0.001, outCount++){
                            if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                                return;

                            fbd_X->position[outCount]        = fbd_X->position[outCountLast];
                            fbd_X->geschwindigkeit[outCount] = 0;
                            fbd_X->beschleunigung[outCount]  = 0;
                            fbd_X->kraftgrenze[outCount]     = 0;

                            fbd_Y->position[outCount]        = fbd_Y->position[outCountLast];
                            fbd_Y->beschleunigung[outCount]  = 0;
                            fbd_Y->geschwindigkeit[outCount] = 0;
                            fbd_Y->kraftgrenze[outCount]     = 0;
        #ifndef Q_WS_QWS
                            //                       t,  s,  v,  a,  s,  v,  a
                            out << QObject::trUtf8("%1, %2, %3, %4, %5, %6, %7, %8, %9\n")
                                   .arg(t1+t2+t+last_t_n1+(t1+t2)*i+m_zwichenpause*i).arg(fbd_X->position[outCount]).arg(fbd_X->geschwindigkeit[outCount]).arg(fbd_X->beschleunigung[outCount])
                                   .arg(fbd_Y->position[outCount]).arg(fbd_Y->geschwindigkeit[outCount]).arg(fbd_Y->beschleunigung[outCount])
                                   .arg(x)
                                   .arg(y);
        #endif
                            last_t_pause=t;
                        }
                    }
                }
            }
        }
    }else{
        fbd_X->position[outCount]        = m_position_X;
        fbd_Y->position[outCount]        = m_position_Y;
    }

    // Aufbereitung der X-Achsen Daten und Binärdatei schreiben fuer SPS
    {
        double lastPosition = fbd_X->position[outCount];
        for(int i=outCount; i<MAX_TABELLEN_STUETZPUNKTE; ++i){
            if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                return;

            fbd_X->beschleunigung[i]=0;
            fbd_X->geschwindigkeit[i]=0;
            fbd_X->position[i]=lastPosition;
            fbd_X->kraftgrenze[i] = 0;
        }

        fbd_X->uiVersion=1;
        fbd_X->uiAnzahlEintraege=max(outCount, 1);
        fbd_X->diWerteGueltig=1;

        QFile file(FILENAME_BIN_X);
        if(!file.open(QIODevice::WriteOnly)){
            LOG_ERROR_1() << QObject::trUtf8("Kann Datei: %1 nicht zum Schreiben öffnen!").arg(FILENAME_BIN_X);
            return;
        }

        QByteArray buffer(sizeof(*fbd_X), ' ');
        memcpy(buffer.data(), fbd_X, sizeof(*fbd_X));
        qint64 written = file.write(buffer.data(), buffer.size());
        if(written != sizeof(*fbd_X))
            return;

    }

    // Aufbereitung der Y-Achsen Daten und Binärdatei schreiben fuer SPS
    {
        double lastPosition = fbd_Y->position[outCount];
        for(int i=outCount; i<MAX_TABELLEN_STUETZPUNKTE; ++i){
            if(outCount>=MAX_TABELLEN_STUETZPUNKTE)
                return;

            fbd_Y->beschleunigung[i]=0;
            fbd_Y->geschwindigkeit[i]=0;
            fbd_Y->position[i]=lastPosition;
            fbd_Y->kraftgrenze[i] = 0;
        }

        fbd_Y->uiVersion=1;
        fbd_Y->uiAnzahlEintraege=max(outCount, 1);
        fbd_Y->diWerteGueltig=1;

        QFile file(FILENAME_BIN_Y);
        if(!file.open(QIODevice::WriteOnly)){
            LOG_ERROR_1() << QObject::trUtf8("Kann Datei: %1 nicht zum Schreiben öffnen!").arg(FILENAME_BIN_Y);
            return;
        }

        QByteArray buffer(sizeof(*fbd_Y), ' ');
        memcpy(buffer.data(), fbd_Y, sizeof(*fbd_Y));
        qint64 written = file.write(buffer, buffer.size());
        if(written != sizeof(*fbd_Y))
            return;
    }
}

void Interpolator::calcCheck(double h1, double s_max, double a_ymax, double a_xmax)
{
    calcInit(h1, 0, s_max-m_position_X, a_ymax, a_xmax);
}

void Interpolator::calc(InterpolatorData ipoData)
{
    calc(ipoData.h1,
         ipoData.h2,
         ipoData.offset_y,
         ipoData.offset_y2,
         ipoData.n1,
         ipoData.n2,
         ipoData.s_max,
         ipoData.a_ymax,
         ipoData.a_xmax,
         ipoData.pause,
         ipoData.zwischenpause);
}

void Interpolator::setPositionX(double pos)
{
    m_position_X=pos;
}

void Interpolator::setPositionY(double pos)
{
    m_position_Y=pos;
}

void Interpolator::setPositionxY(double posx, double posy)
{
    m_position_X=posx;
    m_position_Y=posy;
}

void Interpolator::setOverrideY(double val)
{
    m_factorVx=val;
}

double Interpolator::beschleunigunsWegX() const
{
    return m_beschleunigungswegX;
}

double Interpolator::beschleunigunsWegY() const
{
    return m_beschleunigungswegY;
}


void Interpolator::calcInit(double h, double n, double s_max, double a_max_y, double a_max_x)
{
    Slope slope;

    double t_beschl_y;
    double v=sqrt(2.0*g*h);
    double v_kor = sqrt(v*m_factorVx)*sqrt(v*m_factorVx);

    double s3;
    double s_beschl;
    double r;
    {
        t_beschl_y=v_kor/(2*a_max_y);
        r=a_max_y/t_beschl_y;

        double v1 = 0.5*r*t_beschl_y*t_beschl_y;
        double v2 = v1 + a_max_y * t_beschl_y;

        v_kor=v2 + 0.5*r*(t_beschl_y)*(t_beschl_y);

        double s1 = 1.0/6.0*r*t_beschl_y*t_beschl_y*t_beschl_y;
        double s2 = s1 + v1*t_beschl_y + 0.5*a_max_y*t_beschl_y*t_beschl_y;

        s3 =  s2 + v2*t_beschl_y + 0.5*a_max_y*t_beschl_y*t_beschl_y+1.0/6.0*r*t_beschl_y*t_beschl_y*t_beschl_y;
        s_beschl = s1 + s2 + s3;
    }

    s_wurf_y=s_beschl*2/*+t_beschl_y*v*/;
    scd_y.in_s=s_wurf_y;
    scd_y.in_cycle_time=0.001;
    scd_y.in_vs=0;
    scd_y.in_vn=v_kor;
    scd_y.in_amax=a_max_y;
    scd_y.in_amax2=a_max_y;
    scd_y.in_r=r;
    scd_y.in_r2=r;
    scd_y.in_ve=0.0;
    slope.slopeCalcInit(pscd_y, scd_y);
    m_beschleunigungswegY=pscd_y.phaseS[3];

    double factor=0;
    double v_x=0;
    double s_wurf=0;
    int count=0;
    double tx=0;
    double rx=0;
    double rx2=0;
    double s_x=0;
    if(n>1 && s_max>0.0001){
        double tw=2.0*sqrt(2.0*h/g);
        double a=a_max_x;
        double b=(n*a_max_x*tw);
        double c=(-s_max);
        double diskriminante=b*b-4*a*c;
        if(diskriminante<0)
            return;
        tx=(-b+sqrt(diskriminante))/(2.0*a);
        for(;;){
            v_x = a_max_x*(tx-factor);
            double s_y = n*(pscd_y.phaseT[4])*v_x +(m_zwichenpause)*v_x*(n-1);
            s_wurf = (s_max - s_y)/n;
            double v_x2 = s_wurf/v/2*g;
            double diff = fabs(v_x-v_x2);
            if(diff<0.001 || v_x<=0.0001 || count>50000){
                break;
            }else{
                factor += 0.00001;
            }
            count++;
        }
        tx=v_x/2.0/a_max_x;
        rx=a_max_x/tx;
        s_x=s_max;
        rx2=sqrt(a_max_x/tx)*3;
    }else if(n==0 && s_max>0.0001){
        v_x = s_max/v/2*g;
        tx=v_x/2.0/a_max_x;
        rx2=rx=a_max_x/tx;
        tx*=3;
        tx*=factor_t;
        s_x=a_max_x*tx*tx+tx*v_x;
        s_x=max(s_x, 0.1);
    }else if(n==1 && s_max>0.0001){
        v_x = s_max/v/2*g;
        tx=v_x/2.0/a_max_x;
        rx=a_max_x/tx;
        s_x=s_max;
        rx2=sqrt(a_max_x/tx)*3;
    }
//    double v_x = a_max_x*tx;
//    double s_y = n*(pscd_y.phaseT[4])*v_x;
//    double s_wurf = (s_max - s_y)/n;

    if(s_max>0.0001){
        scd_x.in_s=s_x;
        scd_x.in_cycle_time=0.001;
        scd_x.in_vs=0;
        scd_x.in_vn=v_x;//a_max_x*tx;
        scd_x.in_amax=a_max_x;
        scd_x.in_amax2=a_max_x;
        scd_x.in_r=rx;
        scd_x.in_r2=rx2;
        scd_x.in_ve=0.0;
        slope.slopeCalcInit(pscd_x, scd_x);
        m_beschleunigungswegX=pscd_x.phaseS[3];
    }
}

