#include <bits/stdc++.h>
using namespace std;

struct vector3d{
   double x;
   double y;
   double z;
};

struct vector4d{
   double x;
   double y;
   double z;
   double w;
};

class QuadController{

    private:

    class controller {
        private:

        float Kp;
        float Ki;
        float Kd;

        float error=0;
        float previousError=0;
        float integral=0;
        float derivative=0;

        int last_time=0;

        public:

        float current=0;
        float target=0;
        float output=0;

        controller(float Kp,float Ki,float Kd) : Kp(Kp), Ki(Ki), Kd(Kd) {}

        void calc(){
            int current_time = 0;//[GET_TIME]();
            int dt=0;
            if (last_time!=0)
                dt = current_time - last_time;
            last_time=current_time;
            error = (target - current)/3.1415/2; //normalize: 2pi error => 1
            integral = integral + error*dt;
            derivative = (error - previousError)/dt;
            output = (Kp*error + Ki*integral + Kd*derivative);
            previousError = error;
        }
    };

    static constexpr  float steerConst=0.5;

    static constexpr  float rotKp=500;
    static constexpr  float rotKi=0;
    static constexpr float rotKd=100;

    static constexpr float posKp=100;
    static constexpr float posKi=0;
    static constexpr float posKd=80;

    //                              R  P  Y  X  Y  Z
    static constexpr int coeff[4][6]= {{  1, 1,-1, 1,-1, 1}, //FR
                                   { -1, 1, 1, 1, 1, 1}, //FL
                                   {  1,-1, 1,-1,-1, 1}, //BR
                                   { -1,-1,-1,-1, 1, 1}};//BL

    static controller rollCtl;
    static controller pitchCtl;
    static controller yawCtl;
    static controller XCtl;
    static controller YCtl;
    static controller ZCtl;

    public:
    QuadController(){
        rollCtl = controller(rotKp,rotKi,rotKd);
        pitchCtl = controller(rotKp,rotKi,rotKd);
        yawCtl = controller(rotKp,rotKi,rotKd);
        XCtl = controller(posKp,posKi,posKd);
        YCtl = controller(posKp,posKi,posKd);
        ZCtl = controller(posKp,posKi,posKd);
    }

    /**
     * calculate motor speeds by using an PID controllor on the inputs.
     *
     * This sum is the arithmetic sum, not some other kind of sum that only
     * mathematicians have heard of.
     *
     * @param rot current orientation, Vector containing Euler-angles (x=roll,y=pitch,z=yaw, 0 to 2pi)
     * @param input user steering input (x=roll,y=pitch,z=yaw,w=power, -1 to 1)
     * @param pos current position, Vector
     * @param targetPos position to be moved to, ignored if obmitted or NULL
     *
     * @return array of motor speeds.
     */
    int (&run(vector3d* rot,vector4d* input,vector3d* pos=NULL,vector3d* targetPos=NULL))[4]{

        rollCtl.target=steerConst*input->x;
        pitchCtl.target=steerConst*input->y;
        yawCtl.target=steerConst*input->z;

        rollCtl.current=rot->x;
        pitchCtl.current=rot->y;
        yawCtl.current=rot->z;

        rollCtl.calc();
        pitchCtl.calc();
        yawCtl.calc();

        if (targetPos!=NULL){
            XCtl.target=targetPos->x;
            YCtl.target=targetPos->y;
            ZCtl.target=targetPos->z;

            XCtl.current=pos->x;
            YCtl.current=pos->y;
            ZCtl.current=pos->z;

            XCtl.calc();
            YCtl.calc();
            ZCtl.calc();
        }
        static int output[]={0,0,0,0};

        for (int i=0;i<4;i++){
            output[i]=50        +  coeff[i][0]*rollCtl.output + coeff[i][1]*pitchCtl.output + coeff[i][2]*yawCtl.output;
            output[i]=output[i] +  coeff[i][3]*XCtl.output    + coeff[i][4]*YCtl.output     + coeff[i][5]*ZCtl.output;
            output[i]=output[i]*input->w;

            if (output[i]<0)  output[i]=0;
            if (output[i]>100)output[i]=100;
        }
        return output;
    }
};