#pragma once

#include <Arduino.h>
#include <ArduinoEigen.h>
#include "WorkSpaceModel.h"

namespace Work
{
    using namespace Eigen;

    namespace Model
    {
        // right hand coord
        // +X: right, +Y: forward, +Z: up

        template <typename WorkType, typename JointType, size_t DmsW, size_t DmsJ>
        struct Omni : public Base<WorkType, JointType, DmsW, DmsJ>
        {
            // !!!!! required to use this class as pointers !!!!!
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            const double g = 9.80665; // gravitational acceleraation [m/s^2]

            const double R = 0.125; // wheel radius [m]
            // const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel)
            const double L = 0.3625;//1.0;//0.435; // from center to wheel [m]
            // const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
            const double Mp = 10.0; // mass of platform [kg]
            const double Ml = 10.0; // mass of load [kg]
            // const double Fwz = (Mp + Ml) * g; // z axis force limit of each wheel [Nm]
            const double Iw = 0.1; // inertia of wheel (include motor)
            const double Ip = 0.5; // inertia of platform center (around Z axis)
            const double H = 0.5; // load height (center of gravity) [m]
            // const double L = T + W;
            // const double A = (Mp + Ml) * R * R / 8.0;
            // const double B = Ip * R * R / (16.0 * L * L);
            // const double AA = Mp * R * R / 2.0;
            // const double BB = Ip * R * R / (4.0 * W * W);
            // const double CC = (AA + BB) / (R * R);

            const double wheel_thickness = 0.05;

            // const....
            MatrixXd I     {DmsJ, DmsJ};
            MatrixXd I_inv {DmsJ, DmsJ};
            MatrixXd In    {DmsJ, DmsJ};
            MatrixXd C     {DmsJ, 2};
            MatrixXd C_inv {2, DmsJ};

            Omni()
            : Model::Base<WorkType, JointType, DmsW, DmsJ>()
            {
                // C <<
                //    Ml * H * T,  Ml * H *W,
                //   -Ml * H * T,  Ml * H *W,
                //   -Ml * H * T, -Ml * H *W,
                //    Ml * H * T, -Ml * H *W;

                // C_inv = PseudoInverse(C);

                // I <<
                //     AA + BB,     -AA,  BB,
                //         -AA, 2. * AA, -AA,
                //          BB,     -AA,  CC;

                // I_inv = I.inverse();

                this->Jacobian_inv <<
                    -sin(PI * 1. / 4.), cos(PI * 1. / 4.), L,
                    -sin(PI * 3. / 4.), cos(PI * 3. / 4.), L,
                    -sin(PI * 5. / 4.), cos(PI * 5. / 4.), L,
                    -sin(PI * 7. / 4.), cos(PI * 7. / 4.), L;
                this->Jacobian_inv /= R;
                this->Jacobian_local = PseudoInverse(this->Jacobian_inv);

                // updateJacobian(0.0);

                // this->Jacobian_tra = this->Jacobian.transpose();

                // Matrix<double, DmsJ, DmsJ> J_null_tmp = this->Jacobian_inv * this->Jacobian;

                // this->Jacobian_null = MatrixXd(DmsJ, DmsJ).setIdentity() - J_null_tmp;

                // MatrixXd M_buff(DmsW, DmsJ);
                // M_buff = this->Jacobian * I_inv;
                // this->M_inv = M_buff * this->Jacobian_tra;


                // this->Mn.setIdentity();

                // this->Mn_inv = this->Mn.inverse();

                // for (size_t i = 0; i < DmsJ; ++i) In(i, i) = I(i, i);

            }

            virtual void updateJacobian(const double& rad) override
            {
                this->rotation = AngleAxisd(rad, Vector3d::UnitZ()).matrix();
                this->Jacobian = this->rotation * this->Jacobian_local;
                this->Jacobian_inv = PseudoInverse(this->Jacobian);
            }


            virtual WorkType estimateGravity() override
            {
                WorkType gravity = WorkType::Zero();
                return gravity;
            }

            virtual WorkType estimateFriction(const WorkType& vel) override
            {
                WorkType friction = WorkType::Zero();
//                if (abs(vel.x) > 0.0)
//                    friction.x = -Math::sign<double>(vel.x) * 0.002 * (Mp + Ml) * g;
//                else
//                    friction.x = 0.0;
//                if (abs(vel.y) > 0.0)
//                    friction.y = -Math::sign<double>(vel.y) * 0.002 * (Mp + Ml) * g;
//                else
//                    friction.y = 0.0;
//                if (abs(res.vel.z) > 0.0)
//                    friction.z = -Math::sign<double>(vel.z) * 0.002 * (Mp + Ml) * g;
//                else
//                    friction.z = 0.0;
                return friction;
            }

            virtual WorkType estimateCoriolis() override
            {
                WorkType coriolis = WorkType::Zero();
                return coriolis;
            }

//             virtual WorkType estimateLimitation(const WorkType& acc) override
//             {
//                 WorkType limit = WorkType::Zero();
//                 bool b_excess = false;
//                 JointType fz(JointType::Zero());
//                 JointType f_excess(JointType::Zero());
//                 fz = C * acc;
//                 for (size_t i = 0; i < DmsJ; ++i)
//                 {
//                     if (fz(i) > Fwz)
//                     {
//                         f_excess(i) = fz(i) - Fwz;
//                         b_excess = true;
//                     }
//                 }
//                 if (b_excess)
//                 {
//                     limit = C_inv * f_excess;
// //                    LOG_NOTICE("EXCESS FORCE");// << f_excess;
//                 }
//                 return limit;
//             }


            // const double& getWheelBase() const { return W; }
            // const double& getTread() const { return T; }
            double getWheelRadius() const { return R; }
            const double& getWheelDiameter() const { return 2.0 * R; }
            const double& getWheelThickness() const { return wheel_thickness; }
        };

    }

}


