#include <stdio.h>
#include <math.h>

#include <iostream>

using namespace std;



const float MACHINE_EPSILON = 0.00001;

const float c = 300;

struct vector3
{
    float x;
    float y;
    float z;

    vector3() : x(0) , y(0) , z(0) {}
    vector3(float _x , float _y , float _z ): x(_x), y(_y) , z(_z) {}

    //-----------------------------//

    float length2() const
    {
        return x*x + y*y + z*z;
    }

    float length() const
    {
        return sqrt(length2());
    }


    vector3 normalized() const
    {
        float lengthVector = length();
        if (lengthVector < MACHINE_EPSILON)
        {
            return *this;
        }
        // Compute and return the unit vector
        float lengthInv = 1.0 / lengthVector;
        return vector3(x * lengthInv, y * lengthInv, z * lengthInv);
    }

    //-----------------------------//

    float dot( const vector3& v ) const
    {
        return x*v.x + y*v.y + z*v.z;
    }

    vector3 cross( const vector3& vector) const
    {
      return vector3(y * vector.z - z * vector.y,
                     z * vector.x - x * vector.z,
                     x * vector.y - y * vector.x);
    }


    //-----------------------------//

    vector3 operator * ( const float& v ) const
    {
        return vector3( x * v , y * v , z * v );
    }

    vector3 operator / ( const float& v ) const
    {
        return vector3( x / v , y / v , z / v );
    }


    //-----------------------------//

    vector3 operator * ( const vector3 v ) const
    {
        return vector3( x * v.x , y * v.y , z * v.z );
    }

    //-----------------------------//

    vector3 operator + ( const vector3& v ) const
    {
        return vector3( x + v.x , y + v.y , z + v.z );
    }


    vector3 operator - ( const vector3& v ) const
    {
        return vector3( x - v.x , y - v.y , z - v.z );
    }
};


struct vector4
{
    float t;
    float x;
    float y;
    float z;

    vector4() : x(0) , y(0), z(0) , t(0) {}
    vector4(float _x , float _y , float _z , float _t): x(_x),y(_y),z(_z),t(_t) {}

    //-----------------------------//
     /// Minkowski matrices space
    float length2() const
    {
        return fabs( (x*x+y*y+z*z) - (t*t)*(c*c));
    }

    float length() const
    {
        return sqrt(length2());
    }

    //-----------------------------//

    vector3 getPos() const
    {
        return vector3(x,y,z);
    }

    vector3 getProj3D() const
    {
        return vector3(x/t,y/t,z/t);
    }


    //-----------------------------//
    vector4 operator * ( const float& v ) const
    {
        return vector4( x * v , y * v , z * v , t * v);
    }

    vector4 operator / ( const float& v ) const
    {
        return vector4( x / v , y / v , z / v , t / v);
    }
};




/// Loretz factor 
static float gammaa(const vector3& v)
{ 
   float gamma = sqrt(1.0 - v.dot(v)/(c*c));
   gamma = (gamma > MACHINE_EPSILON) ? gamma : MACHINE_EPSILON;
   return gamma;
}



struct matrix4x4
{

    // Elements of the matrix
    union
    {
        float m[4][4];
        float m16[16];
    };




    public :

     // -------------------- Methods -------------------- //

    // Constructor
    matrix4x4(float m_00=0, float m_01=0, float m_02=0, float m_03=0,
              float m_10=0, float m_11=0, float m_12=0, float m_13=0,
              float m_20=0, float m_21=0, float m_22=0, float m_23=0,
              float m_30=0, float m_31=0, float m_32=0, float m_33=0)
    {
        m[0][0] = m_00; m[0][1] = m_01; m[0][2] = m_02;  m[0][3] = m_03;
        m[1][0] = m_10; m[1][1] = m_11; m[1][2] = m_12;  m[1][3] = m_13;
        m[2][0] = m_20; m[2][1] = m_21; m[2][2] = m_22;  m[2][3] = m_23;
        m[3][0] = m_30; m[3][1] = m_31; m[3][2] = m_32;  m[3][3] = m_33;
    }

    // Constructor
    matrix4x4(float n[4][4])
    {
        m[0][0]=n[0][0]; m[0][1]=n[0][1]; m[0][2]=n[0][2]; m[0][3]=n[0][3];
        m[1][0]=n[1][0]; m[1][1]=n[1][1]; m[1][2]=n[1][2]; m[1][3]=n[1][3];
        m[2][0]=n[2][0]; m[2][1]=n[2][1]; m[2][2]=n[2][2]; m[2][3]=n[2][3];
        m[3][0]=n[3][0]; m[3][1]=n[3][1]; m[3][2]=n[3][2]; m[3][3]=n[3][3];
    }


    // Set the matrix to the identity matrix
    matrix4x4 setToIdentity()
    {
        m[0][0] = 1.f; m[0][1] = 0.f; m[0][2] = 0.f;  m[0][3] = 0.f;
        m[1][0] = 0.f; m[1][1] = 1.f; m[1][2] = 0.f;  m[1][3] = 0.f;
        m[2][0] = 0.f; m[2][1] = 0.f; m[2][2] = 1.f;  m[2][3] = 0.f;
        m[3][0] = 0.f; m[3][1] = 0.f; m[3][2] = 0.f;  m[3][3] = 1.f;
        return *this;
    }

    // Return the transpose matrix
    matrix4x4 getTranspose() const
    {
        return matrix4x4(m[0][0], m[1][0], m[2][0], m[3][0],
                         m[0][1], m[1][1], m[2][1], m[3][1],
                         m[0][2], m[1][2], m[2][2], m[3][2],
                         m[0][3], m[1][3], m[2][3], m[3][3]);
    }



    // * operator
    matrix4x4 operator*(float f) const
    {
        return matrix4x4(m[0][0]*f, m[0][1]*f, m[0][2]*f,  m[0][3]*f,
                         m[1][0]*f, m[1][1]*f, m[1][2]*f,  m[1][3]*f,
                         m[2][0]*f, m[2][1]*f, m[2][2]*f,  m[2][3]*f,
                         m[3][0]*f, m[3][1]*f, m[3][2]*f,  m[3][3]*f);
    }


    vector4 operator*( const vector4 &v ) const
    {
        vector4 u = vector4(m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3]*v.t,
                            m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3]*v.t,
                            m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]*v.t,
                            m[3][0]*v.x + m[3][1]*v.y + m[3][2]*v.z + m[3][3]*v.t);


         return u;
    }



    /// Matrix4x4 Loretz Boost (Lambda_syaytem) L(v,b)
    static matrix4x4 LorentzMatrix( const  float& v , const vector3& n )
    {
            matrix4x4 M;

            /// lorentz  gamma factor
            /// float gamma = 1.0 / gammaa(n*v);
            float gamma = 1.0 * gammaa(n*v);

            M.m[0][0]= 1.0+((gamma - 1.0)*((n.x * n.x)));
            M.m[1][0]=     ((gamma - 1.0)*((n.y * n.x)));
            M.m[2][0]=     ((gamma - 1.0)*((n.z * n.x)));
            M.m[3][0]=   (v*n.x*gamma)/(c*c);

            M.m[0][1]=     ((gamma - 1.0)*((n.x * n.y)));
            M.m[1][1]= 1.0+((gamma - 1.0)*((n.y * n.y)));
            M.m[2][1]=     ((gamma - 1.0)*((n.z * n.y)));
            M.m[3][1]=   (v*n.y*gamma)/(c*c);

            M.m[0][2]=      ((gamma - 1.0)*((n.x * n.z)));
            M.m[1][2]=      ((gamma - 1.0)*((n.y * n.z)));
            M.m[2][2]=  1.0+((gamma - 1.0)*((n.z * n.z)));
            M.m[3][2]=   (v*n.z*gamma)/(c*c);

            M.m[0][3]=v*n.x*gamma;
            M.m[1][3]=v*n.y*gamma;
            M.m[2][3]=v*n.z*gamma;
            M.m[3][3]=gamma;

            return M;
    }

};



struct Observer
{
    Observer()
    : pos(vector3(0,0,0)), 
      vel(vector3(0,0,0))
    {
      
    }
    Observer(const vector3& _pos , const vector3& _vel)
    : pos(_pos) , 
      vel(_vel)
    {
         
    }
    
    vector3 pos;
    vector3 vel;
};



struct DynamicsParticle
{
    float   mMassa;
    vector4 mPosition;
    vector3 mAcceleration;
    vector3 mVelocity;


    DynamicsParticle( vector4 _pos , float _massa )
      : mPosition(_pos) , mMassa(_massa)
    {

    }

    //-----------------------------//
    /// Integration Loretz 
    void Integration( float _dt , const Observer& observ)
    {
       /// Shift time Dt0 in observer system 
       mPosition.t = (1.0 - (observ.vel.dot(mPosition.getPos() - observ.pos)) / (c*c)) * gammaa(mVelocity-observ.vel);
    
       /// Integration Euler and Loretz method
       mVelocity = (mVelocity + (mAcceleration * _dt)) / (1.0 + ((mAcceleration * _dt).dot(mVelocity)) / (c*c));
       mPosition = matrix4x4::LorentzMatrix( mVelocity.length() * _dt , mVelocity.normalized() ) *  mPosition;
    }

    //-----------------------------//

    void applyImpuls( const vector3& _impuls )
    {
        vector3 impuls =  (_impuls / (mMassa/gammaa(mVelocity)) );
        mVelocity  = ((mVelocity + impuls) / (1.0 + ((impuls).dot(mVelocity)/(c*c))));              
    }

    //-----------------------------//

    void applyForce( const vector3& _force )
    {
       vector3 acceleration =  (_force / (mMassa/gammaa(mVelocity)) );
       mAcceleration = mAcceleration + acceleration;
    }


   private:

};




int main(int argc, char *argv[])
{
    
    
    Observer observer(vector3(1,1,1),vector3(0,0,0));


    vector4 position(10,12,1,16);
    float   massa = 3.0;

    DynamicsParticle body( position , massa );


    ///  time step 
    float dt = 1.f;

    for (int i = 0; i < 100; ++i)
    {
        vector3 impulse(0,i,0);
       
        body.applyImpuls( impulse );
        body.Integration( dt, observer );


        cout<< "velocity: " << body.mVelocity.length() << "  time: " << body.mPosition.t <<endl;
    }



    return 0;
}

/**
   * Orthogonal unit vector

   */

   SIMD_INLINE IVector3D<T> GetOneUnitOrthogonalVector() const
   {
       assert(Length() > MACHINE_EPSILON);

       // Get the minimum element of the vector
       IVector3D<T> vectorAbs(IAbs(x), IAbs(y), IAbs(z));

       int minElement = vectorAbs.GetMinAxis();

       if (minElement == 0)
       {
           return IVector3D<T>(0.0, -z, y) / ISqrt(y*y + z*z);
       }
       else if (minElement == 1)
       {
           return IVector3D<T>(-z, 0.0, x) / ISqrt(x*x + z*z);
       }
       else
       {
           return IVector3D<T>(-y, x, 0.0) / ISqrt(x*x + y*y);
       }
  }




/**
    * @brief BiUnitGrammSchmidt

    * @param n -axis

    * @param p -ortogonal_left

    * @param q -ortogonal_up

    * Gramm schmidt process

    * https://www.math.hmc.edu/calculus/tutorials/gramschmidt/gramschmidt.pdf

    */
   static SIMD_INLINE void BiUnitGrammSchmidt(const IVector3D<T>& n_axis , IVector3D<T>& p, IVector3D<T>& q )
   {
       p = n_axis.GetOneUnitOrthogonalVector();
       q = n_axis.Cross(p);

   }