#pragma once

namespace Work
{
    using namespace Eigen;

    namespace Model
    {
        template <typename WorkType, typename JointType, size_t DmsW, size_t DmsJ, typename ControlType = VectorXd, int ControlDim = 6>
        struct Base
        {
            // !!!!! required to use this class as pointers !!!!!
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

			virtual ~Base() {}
            virtual void update() {};
            virtual void draw() {}

            virtual WorkType estimateGravity()
            {
                // return dis.gravity;
                return WorkType::Zero();
            }
            virtual WorkType estimateFriction(const WorkType& vel)
            {
                // return dis.friction;
                return WorkType::Zero();
            }
            virtual WorkType estimateCoriolis()
            {
                // return dis.coriolis;
                return WorkType::Zero();
            }
            virtual WorkType estimateLimitation(const WorkType& acc)
            {
                return WorkType();
            }
            virtual WorkType fromAccelToForce(const WorkType& ddx)
            {
                WorkType force = Mn * ddx;
                return force;
            }
            virtual WorkType fromForceToAccel(const WorkType& force)
            {
                WorkType acc = M_inv * force;
                return acc;
            }
            virtual JointType fromWorkAccToJointAcc(const WorkType& ddx)
            {
                JointType ddq = Jacobian_inv * ddx;
                return ddq;
            }
            virtual WorkType fromJointVelToWorkVel(const JointType& dq)
            {
                WorkType dx = Jacobian * dq;
                return dx;
            }
            virtual JointType fromJointVelToNullSpaceDumping(const JointType& dq)
            {
                JointType dq_dump = Jacobian_null * dq;
                return dq_dump;
            }

            MatrixXd Jacobian_local {DmsW, DmsJ};
            MatrixXd Jacobian {DmsW, DmsJ};
            MatrixXd Jacobian_inv {DmsJ, DmsW};
            MatrixXd Jacobian_tra {DmsJ, DmsW};
            MatrixXd Jacobian_null {DmsJ, DmsJ};
            MatrixXd rotation {DmsW, DmsW};

            virtual void updateJacobian(const double& rad) = 0;

            MatrixXd M_inv {DmsW, DmsW};
            MatrixXd Mn {DmsW, DmsW};
            MatrixXd Mn_inv {DmsW, DmsW};

            // void setDisturbance(SimulateVariables::Disturbance<WorkType> d) { dis = d; }

            // void estimateAcceleration(const ControlVariables::Reference<WorkType>& ref)
            // {
            //     est.force = ref.force - (dis.force + dis.gravity + dis.friction + dis.coriolis);
            //     est.acc = fromForceToAccel(est.force);
            // }

            // void estimateResponse(const double& interval)
            // {
            //     est.vel = ddx_to_dx.get(est.acc, interval);
            //     est.pos = dx_to_x.get(est.vel, interval);
            // }

            // void setAccelerationResponse(const WorkType& acc) { est.acc = acc; }

            // const SimulateVariables::Estimation<WorkType>& getEstimatedResponse() const { return est; }
            // const WorkType& getEstimatedPosition() const { return est.pos; }
            // const WorkType& getEstimatedVelocity() const { return est.vel; }

            void selectCartesianCoordinate()
            {
                // TODO: variable template is better!!
            }

            void setControlSelectMatrix(const MatrixXd& mat)
            {
                mat_euler_to_min = mat;
                mat_min_to_euler = mat.transpose();
            }


            const MatrixXd& getControlVariableMinToEulerMatrix() { return mat_min_to_euler; }
            const MatrixXd& getControlVariableEulerToMinMatrix() { return mat_euler_to_min; }

            VectorXd getControlVariableEulerToQuat(const VectorXd& v) const
            {
                assert(v.size() == 6);

                Quaternionf q;
                q = AngleAxisf(v(3), Vector3f::UnitX()) *
                    AngleAxisf(v(4), Vector3f::UnitY()) *
                    AngleAxisf(v(5), Vector3f::UnitZ());

                VectorXd p {7};
                p << v(0), v(1), v(2), q.x(), q.y(), q.z(), q.w();

                return p;
            }

            VectorXd getControlVariableQuatToEuler(const VectorXd& v) const
            {
                assert(v.size() == 7);

                Quaternionf q(v(3), v(4), v(5), v(6));
                Vector3f e = q.toRotationMatrix().eulerAngles(0, 1, 2);
                VectorXd p {6};
                p << v(0), v(1), v(2), e(0), e(1), e(2);

                return p;
            }

            // void setPosition(const WorkType& pos) { dx_to_x.reset(pos); }

        private:

            // SimulateVariables::Disturbance<WorkType> dis;
            // SimulateVariables::Estimation<WorkType> est;
            // Calculus::Integral<WorkType, DmsW> dx_to_x;
            // Calculus::Integral<WorkType, DmsW> ddx_to_dx;

            MatrixXd mat_min_to_euler {DmsW, ControlDim};
            MatrixXd mat_euler_to_min {ControlDim, DmsW};
        };
    }
}
