using Godot;
using System;
using System.Collections.Generic;

public class Train : Spatial
{
    [Export]
    private float _speed = 1.0f;

    [Export]
    private NodePath _railwayPath;

    private Railway _railway;
    private float _distance = 0;

    [Export]
    private NodePath _carPath;
    private TrainCar _car;

    private Spatial[] _bogies;
    private readonly Dictionary<Spatial, PathFollow> _bogiePathFollows = new Dictionary<Spatial, PathFollow>();

    private ImmediateGeometry _imgui;

    // Called when the node enters the scene tree for the first time.
    public override void _Ready()
    {
        _imgui = new ImmediateGeometry();
        var m = new SpatialMaterial();
        m.VertexColorUseAsAlbedo = true;
        m.FlagsUnshaded = true;
        _imgui.MaterialOverride = m;
        AddChild(_imgui);

        _railway = GetNode<Railway>(_railwayPath);
        _car = GetNode<TrainCar>(_carPath);

        _bogies = new Spatial[_car.Bogies.Length];
        for (int i = 0; i < _bogies.Length; i++)
        {
            var b = _car.Bogies[i];
            _bogies[i] = b;
            _bogiePathFollows[b] = new PathFollow
            {
                RotationMode = PathFollow.RotationModeEnum.Oriented
            };
            _railway.AddPathFollower(_bogiePathFollows[b]);

            // detach the bogies from the car as we need to set the bogie
            // positions in order to calculate the car position and it will
            // get confusing if they're in a parent/child relationship,
            // so make them all siblings
            _car.RemoveChild(b);
            this.AddChild(b);
        }
    }

    // Called every frame. 'delta' is the elapsed time since the previous frame.
    public override void _Process(float delta)
    {
        var length = _railway.Curve.GetBakedLength();

        // first determine which is the front bogie. this is dependent
        // on the direction of travel.
        int travelDir = (int)(_distance / length) == 0 ? -1 : 1;

        DrawTravelDirectionIndicator(travelDir);

        // keep track of total distance
        _distance += delta * _speed;
        // wrap at 2x total distance
        _distance %= length * 2;

        var distance = PingPong(_distance, length);

        // get the position of the first bogie on the track
        var b = _bogies[0];
        var pathFollow = _bogiePathFollows[b];

        pathFollow.Offset = distance;
        b.Translation = pathFollow.Translation;
        b.Rotation = pathFollow.Rotation;

        // now we want to position the car over the first bogie
        GD.Print(_car.Wheelbase);
        _car.Translation = b.Translation;
        _car.Rotation = b.Rotation;

        // TODO: this rotation quat needs to position the other 
        // bogie over the curve
        _car.Translation += _car.Transform.basis
            .RotationQuat()
            .Xform(Vector3.Back) * _car.Wheelbase * 0.5f;
    }

    private void DrawTravelDirectionIndicator(int travelDir)
    {
        _imgui.Clear();
        _imgui.Begin(Mesh.PrimitiveType.LineStrip);
        _imgui.SetColor(new Color(1, 0, 0));
        var c = _car.Translation + Vector3.Up * 2;
        _imgui.AddVertex(c);
        var d = travelDir;
        var l = 5;
        var e = c + _car.Transform.basis
            .RotationQuat()
            .Xform(Vector3.Forward) * d * l;
        _imgui.AddVertex(e);
        _imgui.End();
    }

    // poor mans branching ping pong
    private float PingPong(float t, float length)
    {
        var tml = t % length;

        return t > length
            ? length - tml
            : tml;
    }
}