#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "math.h"
#include "iostream"
#include <chrono>
#include <thread>

QBrush greenBrush(Qt::green);
QBrush blueBrush(Qt::blue);
QPen BlackPen(Qt::black);
QPen BluePen(Qt::blue);
QPen CyanPen(Qt::cyan);
QPen GreenPen(Qt::green);
QPen GreyPen(Qt::gray);
QPen RedPen(Qt::red);

std::chrono::steady_clock::time_point beginplot;
std::chrono::steady_clock::time_point endplot;
double timeplot=0;

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    scene = new QGraphicsScene(this);
    ui->graphicsView->setScene(scene);
    ui->graphicsView->scale(25,25);

    BlackPen.setWidth(0);
    BluePen.setWidth(0);
    CyanPen.setWidth(0);
    GreenPen.setWidth(0);
    GreyPen.setWidth(0);
    RedPen.setWidth(0);

    //QBrush greenBrush(Qt::green);
    //QBrush blueBrush(Qt::blue);
    //QPen outlinePen(Qt::black);
    //outlinePen.setWidth(2);

    //Examples:
    //rectangle = scene->addRect(100, 0, 80, 100, outlinePen, blueBrush);
    // addEllipse(x,y,w,h,pen,brush)
    //ellipse = scene->addEllipse(0, 0, 1, 1, outlinePen, greenBrush);
    //text = scene->addText("bogotobogo.com", QFont("Arial", 20) );
    // movable text
    //text->setFlag(QGraphicsItem::ItemIsMovable);
    //line = scene->addLine(0,0,100,100, outlinePen);
}

MainWindow::~MainWindow()
{
    delete ui;
}


void MainWindow::on_pushButton_pressed()
{
    MOTION M;

    // A motion;
    M.PaX=0;
    M.PaY=0;
    M.PaZ=0;
    M.PbX=50;
    M.PbY=0;
    M.PbZ=0;
    M.Vo=0;
    M.Vel=12;
    M.Al=5;
    M.CurveType=0;
    MotionVec.push_back(M);

    // A second motion;
    M.PaX=50;
    M.PaY=0;
    M.PaZ=0;
    M.PbX=100;
    M.PbY=0;
    M.PbZ=0;
    M.Vo=0;
    M.Vel=5;
    M.Al=4;
    M.CurveType=0;
    MotionVec.push_back(M);

    // A thirth motion;
    M.PaX=150;
    M.PaY=0;
    M.PaZ=0;
    M.PbX=200;
    M.PbY=0;
    M.PbZ=0;
    M.Vo=0;
    M.Vel=10;
    M.Al=2;
    M.CurveType=0;
    MotionVec.push_back(M);

    bool stop=0;
    int calculations=0;
    double stopwatch=0;
    beginplot = std::chrono::steady_clock::now();

    // Preprocess, we have to know the motion total time Ttot.
    for(int i=0; i<MotionVec.size(); i++){
        MotionVec.at(i)=MotionScurveCmd(MotionVec.at(i)); // Singleshot, get the total path time Ttot.
        Print("t",MotionVec.at(i).Ttot);

        stopwatch=0;
        stop=0;

        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

        while (!stop){

            std::this_thread::sleep_for(std::chrono::nanoseconds(900000)); //ahead of servo-thread time
            std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

            stopwatch = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin).count()*0.000000001;
            if(stopwatch<=MotionVec.at(i).Ttot){
                MotionVec.at(i).t = stopwatch;
                MotionVec.at(i)=MotionScurveCmd(MotionVec.at(i));
                Print("Px",MotionVec.at(i).Px);
                Print("PY",MotionVec.at(i).Py);
                Print("Pz",MotionVec.at(i).Pz);
                Print("V ",MotionVec.at(i).V);
                Print("A ",MotionVec.at(i).A);
                Print("Stopwatch",stopwatch);
                Print("Calcuations",calculations++);

            } else {
                stop=true;
            }
        }
    }

    //Example output:
    //std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() << "[µs]" << std::endl;
    //std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end - begin).count() << "[ns]" << std::endl;
    //std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::milliseconds> (end - begin).count() <<"[ms]" << std::endl;
}

//! Return point at current motioncommand.
//! Coded in C-style to ensure halcompile --compile compatible
MOTION MainWindow::MotionScurveCmd(MOTION M){

    //! -- --- -- Input -- --- --

    double t=M.t;                                   // Return xyz at time point t.
    double r=0;                                     // Path ratio r at time point t.
    double PaX=M.PaX;                               // Path start point.
    double PaY=M.PaY;
    double PaZ=M.PaZ;
    double PbX=M.PbX;                               // Path end point.
    double PbY=M.PbY;
    double PbZ=M.PbZ;
    double Ltot = sqrt(pow(PaX-PbX,2)+pow(PaY-PbY,2)+pow(PaZ-PbZ,2));

    double Vel = M.Vel;                       // mm/sec
    double Al = M.Al;                               // Lineair acceleration, mm/sec^2
    double Vo = M.Vo;                               // Start velocity (starting at concave period).

    int CurveType = M.CurveType;                    // CurveType = 0 => S-curve at both sides.
    // CurveType = 1 => No S-curve at left side.
    // CurveType = 2 => No S-curve at right side.
    // CurveType = 3 => No S-curve at both sides.
    //! -- --- -- Function -- --- --
    //!

    double As = Al*2;                               // Maximum acceleration encountered at the S-curve inflection point, Note at page 3 of document, mm/sec2
    double T  = Vel/Al;                          // Total Acc, dcc time.

    //double A  = 0;                                  // Current acceleration.
    double Jm = 2*As/T;                             // Max Jerk for profile, 2*As/T <= Jm.
    //double V  = 0;                                  // Current velocity.

    double Vh = 0;                                  // Vel at start of convex period.
    double Sa = 0;                                  // S(t) Distance at concave period, changed from S to Sa.
    double Sb = 0;                                  // S(t) Distance at convex period, changed from S to Sb.
    double Sc = 0;                                  // Distance of atspeed period.

    //! Acc, Dcc, AtSpeed lenghts can be calculated as normal linear curve.

    double L1 = Vo*T + 0.5*Al*(T*T);                // Orginal formula : S=Vo*t + 0.5*Acc*t^2
    double L2 = Ltot-(2*L1);
    double L3 = L1;


    double T1 = T;                                  // acc time.
    double T2 = L2/Vel;                          // Time at full speed.
    double T3 = T;                                  // dcc time.
    double Ttot = T1+T2+T3;                         // Total traject time, if acc and dcc is active.

    // Safety function, you can turn this off to see difference in graph.
    if(L2<0){ // If we have a motion that can not reach a full speed, we have to act.
        L1-=Ltot/2;
        L2=0;
        L3-=Ltot/2;
        Ltot=L1+L2+L3;

        T1=Ttot/2;
        T2=0;
        T3=Ttot/2;
        Ttot=T1+T2+T3;

        Jm = 2*As/T1; //update jerk.

        //in this case
        CurveType=0;
    }

    // Set the no Scurve left side bit.
    if(CurveType==1){
        L2+=L1;
        L1=0;
        Ltot=L1+L2+L3;

        T2=L2/Vel; //repaired.
        T1=0;
        Ttot=T1+T2+T3;
    }

    // Set the no Scurve right side bit.
    if(CurveType==2){
        L2+=L3;
        L3=0;
        Ltot=L1+L2+L3;

        T2=L2/Vel; //repaired.
        T3=0;
        Ttot=T1+T2+T3;
    }

    // Set the no Scurve bit on both sides.
    if(CurveType==3){
        L1=0;
        L2=Ltot;
        L3=0;
        Ltot=L1+L2+L3;

        Ttot=Ltot/Vel;
        T1=0;
        T2=Ttot;
        T3=0;
        Ttot=T1+T2+T3;
    }

    M.Ttot=Ttot; // At this stage we now total traject time of this line.

    // Debug :
    //        std::cout << "L1  : " << L1 << std::endl;
    //        std::cout << "L2  : " << L2 << std::endl;
    //        std::cout << "L3  : " << L3 << std::endl;
    //        std::cout << "Ltot: " << Ltot << std::endl;

    //        std::cout << "T1   : " << T1 << std::endl;
    //        std::cout << "T2   : " << T2 << std::endl;
    //        std::cout << "T3   : " << T3 << std::endl;
    //        std::cout << "Ttot : " << Ttot << std::endl;

    //    for(double t=0; t<=Ttot; t+=0.005){ //for testing.
    endplot = std::chrono::steady_clock::now();
    timeplot = std::chrono::duration_cast<std::chrono::nanoseconds>(endplot - beginplot).count()*0.000000001;

    if(t<T1){                                               //! Acc period.
        if(t<=T1/2){                                        // Acc concave period.
            Sa = Vo*t + Jm*(t*t*t)/6;
            M.V = Vo + Jm*(t*t)/2;
            M.A = Jm*t;

            r=Sa/Ltot;                                      // Path ratio
            M.Px= PaX+( r*(PbX-PaX));                         // Interpolate path point.
            M.Py= PaY+( r*(PbY-PaY));
            M.Pz= PaZ+( r*(PbZ-PaZ));

            //Print("A",A); Print("V",V);
            //Print("Acc convave t",t); Print("Px",Px); Print("Py",Py); Print("Pz",Pz); Print("-- End --",0);

            ellipse = scene->addEllipse(timeplot, M.V*-1, 0.1, 0.1, CyanPen); // Be aware graphicsview is inverted on y-axis.
            ellipse = scene->addEllipse(timeplot, M.A*-1, 0.1, 0.1, GreenPen);

            return M;
        }
        if(t>T1/2){                                         //! Acc convex period.
            double th=T1/2;                                 // th=half acc,dcc period.
            Sa = Vo*th + Jm*(th*th*th)/6;
            Vh = Vo + Jm*(th*th)/2;                         // Velocity at end of concave period

            double tt=t-(T1/2);                             // Convex starttime period=0, so tt=t-convave period
            Sb = Vh*tt + As*(tt*tt)/2 -Jm*(tt*tt*tt)/6;
            M.V = Vh + As*tt -Jm*(tt*tt)/2;
            M.A = As - (Jm*tt);

            r=(Sa+Sb)/Ltot;                                 // Concave period + part of convex period
            M.Px= PaX+( r*(PbX-PaX));
            M.Py= PaY+( r*(PbY-PaY));
            M.Pz= PaZ+( r*(PbZ-PaZ));

            //Print("A",A); Print("V",V);
            //Print("Acc convex t",t); Print("Px",Px); Print("Py",Py); Print("Pz",Pz); Print("-- End --",0);

            ellipse = scene->addEllipse(timeplot, M.V*-1, 0.1, 0.1, BluePen);
            ellipse = scene->addEllipse(timeplot, M.A*-1, 0.1, 0.1, GreenPen);

            return M;
        }
    }

    if(t>=T1 && t<=T1+T2){                                  //! Atspeed period.
        Sc = (t-T1)*Vel;                                 // Time=Dist/Vel
        M.V = Vo+Vel; //repaired.
        M.A = 0;

        r= (L1+Sc)/Ltot;
        M.Px= PaX+( r*(PbX-PaX));
        M.Py= PaY+( r*(PbY-PaY));
        M.Pz= PaZ+( r*(PbZ-PaZ));

        //Print("A",A); Print("V",V);
        //Print("Atspeed t",t); Print("Px",Px); Print("Py",Py); Print("Pz",Pz); Print("-- End --",0);

        ellipse = scene->addEllipse(timeplot, M.V*-1, 0.1, 0.1, GreyPen);
        ellipse = scene->addEllipse(timeplot, M.A*-1, 0.1, 0.1, GreenPen);

        return M;
    }

    if((t>T1+T2)){                         //! Dcc period
        if(t<=(T1+T2+(T3/2))){                              // Dcc concave period.
            double th=T3/2;                                 // th=half acc,dcc period.
            Sa = Vo*th + Jm*(th*th*th)/6;
            Vh = Vo + Jm*(th*th)/2;                         // Velocity at end of concave period

            double tempSb = Vh*th + As*(th*th)/2 -Jm*(th*th*th)/6;  // Convex path lenght of acc period.

            double tt= th - (t-(T1+T2));
            Sb = tempSb - ( Vh*tt + As*(tt*tt)/2 -Jm*(tt*tt*tt)/6 );

            M.V = Vh + As*tt -Jm*(tt*tt)/2;
            M.A = -abs(As - (Jm*tt));

            r= (L1+L2+Sb)/Ltot;
            M.Px= PaX+( r*(PbX-PaX));
            M.Py= PaY+( r*(PbY-PaY));
            M.Pz= PaZ+( r*(PbZ-PaZ));

            //Print("A",A); Print("V",V);
            //Print("Dcc convave t",t); Print("Px",Px); Print("Py",Py); Print("Pz",Pz); Print("-- End --",0);

            ellipse = scene->addEllipse(timeplot, M.V*-1, 0.1, 0.1, BluePen);
            ellipse = scene->addEllipse(timeplot, M.A*-1, 0.1, 0.1, GreenPen);

            return M;
        }
        if(t>(T1+T2+(T3/2))){                               // Dcc convex period.

            double th=T3/2;                                 // Th=half acc,dcc period.
            double tempSa = Vo*th + Jm*(th*th*th)/6;

            Vh = Vo + Jm*(th*th)/2;
            Sb = Vh*th + As*(th*th)/2 -Jm*(th*th*th)/6;

            double tt= th- (t-(T1+T2+th));

            Sa = tempSa - ( Vo*tt + Jm*(tt*tt*tt)/6 );
            M.V = Vo + Jm*(tt*tt)/2;
            M.A = -abs(Jm*tt);

            r= (L1+L2+Sb+Sa)/Ltot;
            M.Px= PaX+( r*(PbX-PaX));
            M.Py= PaY+( r*(PbY-PaY));
            M.Pz= PaZ+( r*(PbZ-PaZ));

            //Print("A",A); Print("V",V);
            //Print("Dcc convex t",t); Print("Px",Px); Print("Py",Py); Print("Pz",Pz); Print("-- End --",0);

            ellipse = scene->addEllipse(timeplot, M.V*-1, 0.1, 0.1, CyanPen);
            ellipse = scene->addEllipse(timeplot, M.A*-1, 0.1, 0.1, GreenPen);

            return M;
        }
    }
    //} // For testing.


}

void MainWindow::Print(std::string string, double value){
    string.append(": ");
    std::cout<<string<<value<<std::endl;
}



void MainWindow::Test(){

    std::cout << "" << std::endl;
    std::cout << "test ok" << std::endl;

    double T  = 4;      // Total Acc time.
    double t  = T/2;    // A period time, concave or convex.
    double ti = t;      // A period time, used for invert time at deacceleration.
    double As = 2;      // mm/sec2, Maximum acceleration encountered at the S-curve inflection point.
    double Ar = As/2;   // Acceleration as normal lineair transition curve, Note at page 3 of document. As = 2*Ar.
    double A;           // Current acceleration.
    double Jm = 2*As/T; // Max Jerk for profile, 2*As/T <= Jm.
    double V;           // Current velocity.
    double Vo = 0;      // Vel at start of concave period.
    double Vh;          // Vel at start of convex period.
    double Sa;          // S(t) Distance at concave period, changed from S to Sa.
    double Sb;          // S(t) Distance at convex period, changed from S to Sb.
    double St;          // Total distance of acc path.


    //! ----------------- ---- -- Concave period, figur 5.1. -- ---- ----------------------------
    //! up

    for(double t=0; t<=T/2; t+=0.5){
        Sa = Vo*t + Jm*(t*t*t)/6;
        V = Vo + Jm*(t*t)/2;
        A = Jm*t;
        std::cout << "----- Results acc concave period -----"           << std::endl;
        std::cout << "period   t     :" << t                            << std::endl;
        std::cout << "distance S     :" << Sa                           << std::endl;
        std::cout << "velocity V     :" << V                            << std::endl;
        std::cout << "acceleration A :" << A                            << std::endl;
    }

    // Initialize Vh.
    Vh=V;

    //! ----------------- ---- -- Convex period, figur 5.1. -- ---- ----------------------------
    //! down

    for(double t=0; t<=T/2; t+=0.5){
        Sb = Vh*t + As*(t*t)/2 -Jm*(t*t*t)/6;
        V = Vh + As*t -Jm*(t*t)/2;
        A = As - (Jm*t);
        std::cout << ""                                                 << std::endl;
        std::cout << "---- Results acc convex period ----"              << std::endl;
        std::cout << "period   t     :" << t                            << std::endl;
        std::cout << "distance S     :" << Sb                           << std::endl;
        std::cout << "velocity V     :" << V                            << std::endl;
        std::cout << "acceleration A :" << A                            << std::endl;
    }

    //! The DeAcceleration start here !!
    //! Try to mirror the acc into dcc.
    //! We mirror the time t.
    //! We mirror the distance Sa and Sb.
    //!
    //!
    //!
    //! ----------------- ---- -- Convex period, figur 5.1. -- ---- ----------------------------
    //! down

    double temp=Sb;
    for(double t=0; t<=T/2; t+=0.5){
        Sb = temp - ( Vh*(ti-t) + As*((ti-t)*(ti-t))/2 -Jm*((ti-t)*(ti-t)*(ti-t))/6 );
        V = Vh + As*(ti-t) -Jm*((ti-t)*(ti-t))/2;
        A = -abs(As - (Jm*(ti-t)));
        std::cout << ""                                                 << std::endl;
        std::cout << "---- Results dcc convex period ----"              << std::endl;
        std::cout << "period   t     :" << t                            << std::endl;
        std::cout << "distance S     :" << Sb                           << std::endl;
        std::cout << "velocity V     :" << V                            << std::endl;
        std::cout << "acceleration A :" << A                            << std::endl;
    }

    //! ----------------- ---- -- Concave period, figur 5.1. -- ---- ----------------------------
    //! up

    temp = Sa;
    for(double t=0; t<=T/2; t+=0.5){
        Sa = temp - Vo*(ti-t) + Jm*((ti-t)*(ti-t)*(ti-t))/6;
        V = Vo + Jm*((ti-t)*(ti-t))/2;
        A = -abs(Jm*(ti-t));
        std::cout << "----- Results dcc concave period -----"           << std::endl;
        std::cout << "period   t     :" << t                            << std::endl;
        std::cout << "distance S     :" << Sa                           << std::endl;
        std::cout << "velocity V     :" << V                            << std::endl;
        std::cout << "acceleration A :" << A                            << std::endl;
    }
}