using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;

public class PlayerMovement : FlatlandMovement
{
    bool isinertial = true;

    // Start is called before the first frame update
    void Start()
    {
        v = new Vector3(0.0f, 0.0f, 0.0f);
        gamma = 1.0f;
        cnt = 0;
        orientation = new Vector3(0.0f, 0.0f, 1.0f);
        time = 0.0f;
        isinertial = true;
        alpha = new Vector3(0.0f, 0.0f, 0.0f);
    }

    void Update()
    {
        base.Update();
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        base.FixedUpdate();

        var prevup = transform.up;
        var prevfor = transform.forward;
        var prevorient = orientation;
        transform.Translate((float)Constants.c * Vector3.up * Time.fixedDeltaTime * (float)gamma, Space.World); // move up by 1 second
        cnt++;
        if (cnt % 480 == 0)
        {
            cnt = 0;
        }
        //transform.Translate(Vector3.forward * Time.fixedDeltaTime, Space.World);
        //transform.Translate(0.5f*Vector3.forward * Mathf.Cos(2*Mathf.PI*cnt/480) * Time.fixedDeltaTime, Space.World);
        //transform.Translate(0.5f*Vector3.left * Mathf.Sin(2 * Mathf.PI * cnt / 480) * Time.fixedDeltaTime, Space.World);
        //orientation = Vector3.forward * Mathf.Cos(2 * Mathf.PI * cnt / 480) + Vector3.left * Mathf.Sin(2 * Mathf.PI * cnt / 480);
        //v = (float)beta * Vector3.forward * Mathf.Cos(2 * Mathf.PI * cnt / 480) + (float)beta * Vector3.left * Mathf.Sin(2 * Mathf.PI * cnt / 480);
        //v = alpha.normalized * (alpha.magnitude * Time.time / Mathf.Sqrt(1.0f + (alpha.magnitude * Time.time) * (alpha.magnitude * Time.time)));
        //v = new Vector3(0.7f, 0.0f, 0.0f);

        if (alpha.magnitude > 0.0f)
        {

            var atmp = alpha.magnitude * Time.fixedDeltaTime;


            double[] vtmp = { (atmp * (alpha.x / alpha.magnitude)), 0.0,(atmp * (alpha.z / alpha.magnitude)) };
            var deltavnaive = V.DenseOfArray(vtmp);

            double[] tmp = {Constants.c * Constants.Gamma(deltavnaive.L2Norm()),
                      deltavnaive[0] * Constants.Gamma(deltavnaive.L2Norm()),
                      deltavnaive[2] * Constants.Gamma(deltavnaive.L2Norm())};

            var deltav = V.DenseOfArray(tmp);


            if (v.magnitude > 0.0f)
            {
                deltav = Constants.BoostMatrix(-v) * deltav;
            }

            var tt = deltav[0] / Constants.c;

            Vector3 finaldeltav = new Vector3((float)(deltav[1]/tt), 0.0f, (float)(deltav[2]/tt));

            var hmm = finaldeltav - v;

            v = finaldeltav;
        }

        beta = v.magnitude / (float)Constants.c;

        gamma = Constants.Gamma(v.magnitude);



        transform.Translate(v * Time.fixedDeltaTime * (float)gamma, Space.World);

        var vt = v + (float)Constants.c * Vector3.up;

        vt.x = -vt.x;

        vt.z = -vt.z;

        vt.Normalize();


        transform.localScale = new Vector3(1.0f, 1.0f, 1.0f); // release child to change x'-axis scale
        //theobject.transform.up = vt;

        var neworientation = (Quaternion.FromToRotation(Vector3.up, vt)) * orientation;

        theobject.transform.rotation = Quaternion.LookRotation(neworientation, vt);

        theobject.transform.parent = null;
        //theobject.transform.RotateAround(theobject.transform.position, vt, Vector3.Angle(prevorient, orientation));

        Vector3 newforward = new Vector3(vt.y * (v.x / v.magnitude), Mathf.Sqrt(vt.x * vt.x + vt.z * vt.z), vt.y * (v.z / v.magnitude));

        transform.rotation = Quaternion.LookRotation(newforward, vt);

        theobject.transform.parent = transform;

        transform.localScale = new Vector3(1.0f, 1.0f, (float)gamma); // scale x'-axis scale (distance dilation)


        time += Time.fixedDeltaTime;
    }

    public bool isInertial()
    {
        return isinertial;
    }
}
