using StereoKit;

namespace slash;

static class Rig
{
    public static Pose head = Pose.Identity;

    public static DeltaBool btn_select = new(false);
    public static DeltaBool btn_back = new(false);

    public static DeltaBool[] btn_grip;
    public static DeltaBool l_btn_grip => btn_grip[(int)Handed.Left];
    public static DeltaBool r_btn_grip => btn_grip[(int)Handed.Right];

    public static Pose[] pose_hld;
    public static Pose l_hld => pose_hld[(int)Handed.Left];
    public static Pose r_hld => pose_hld[(int)Handed.Right];

    public static Pose[] pose_aim;
    public static Pose l_aim => pose_aim[(int)Handed.Left];
    public static Pose r_aim => pose_aim[(int)Handed.Right];

    public static Vec3 fullstick = Vec3.Up;
    public static Pose r_con_stick = Pose.Identity;

    public static XYZi new_dir = new(0, 0, 1);

    public static bool[] upper_sheathes = new bool[] { true, true };
    public static bool[] lower_sheathes = new bool[] { true, true };

    public static bool[] holding = new bool[] { false, false };

    public static void Init()
    {
        btn_grip = new DeltaBool[] { new(false), new(false) };
        pose_hld = new Pose[] { new(), new() };
        pose_aim = new Pose[] { new(), new() };
    }

    public static void Frame()
    {
        head = Input.Head;

        // flatscreen dev controls
        if (Device.DisplayType == DisplayType.Flatscreen)// Device.Name == "Simulator")
        {
            btn_select.Step(Input.Key(Key.MouseLeft).IsActive());
            btn_back.Step(Input.Key(Key.MouseRight).IsActive());

            pose_hld[(int)Handed.Right] = new Pose(
                V.XYZ(SKMath.Sin(Time.Totalf * 6f) * 0.6f * 0.0f, 0.5f, 1.0f),
                Quat.FromAngles(0, 0, 45) * Quat.FromAngles(0, SKMath.Sin(Time.Totalf * 6f) * 60f, 0)
            );
            pose_aim[(int)Handed.Left] = new Pose(
                V.XYZ(1.0f, 0.5f, 0.5f), // V.XYZ(SKMath.Sin(Time.Totalf * 2f) * 0.6f, 0.5f, 0.5f),
                Quat.Identity
            );

            if (Input.Key(Key.A).IsJustActive()) new_dir = new(-1, 0, 0);
            if (Input.Key(Key.S).IsJustActive()) new_dir = new(+1, 0, 0);
            if (Input.Key(Key.W).IsJustActive()) new_dir = new(0, 0, -1);
            if (Input.Key(Key.R).IsJustActive()) new_dir = new(0, 0, +1);
            if (Input.Key(Key.Shift).IsJustActive()) new_dir = new(0, -1, 0);
            if (Input.Key(Key.Space).IsJustActive()) new_dir = new(0, +1, 0);
            fullstick = new_dir.ToVec3;
        }
        else
        {
            Controller l_con = Input.Controller(Handed.Left);
            btn_grip[(int)Handed.Left].Step(l_con.grip > 0.5f);
            pose_hld[(int)Handed.Left] = l_con.pose;
            pose_aim[(int)Handed.Left] = l_con.aim;

            Controller r_con = Input.Controller(Handed.Right);
            btn_grip[(int)Handed.Right].Step(r_con.grip > 0.5f);
            pose_hld[(int)Handed.Right] = r_con.pose;
            pose_aim[(int)Handed.Right] = r_con.aim;

            btn_select.Step(r_con.x1.IsActive() || r_con.trigger > 0.5f);
            btn_back.Step(r_con.x2.IsActive());

            // bool con_tracked = r_con.trackedPos > TrackState.Lost;

            Vec2 stick = r_con.stick;
            Quat stick_rot = Quat.FromAngles(stick.y * -90, 0, stick.x * +90);
            float stick_sign = r_con.IsStickClicked ? -1 : +1;
            r_con_stick = r_con.aim;
            // r_con_stick.position += r_con_stick.orientation * V.XYZ(0.0065f, -0.012f, -0.05f);
            // r_con_stick.orientation *= Quat.FromAngles(-50, 0, 0);
            fullstick = r_con_stick.orientation * stick_rot * Vec3.Up * stick_sign;

            // Vec3 fullstick = r_hand.palm.orientation * Vec3.Up;
            float ax = Maths.abs(fullstick.x);
            float ay = Maths.abs(fullstick.y);
            float az = Maths.abs(fullstick.z);
            if (ax > ay && ax > az) new_dir = new(Maths.sign(fullstick.x), 0, 0);
            if (ay > ax && ay > az) new_dir = new(0, Maths.sign(fullstick.y), 0);
            if (az > ax && az > ay) new_dir = new(0, 0, Maths.sign(fullstick.z));

            // sheathe grabs
            // shoulders
            // put hand in head space
            // if behind head then handedness = left or right sheathe
            // else check for left or right of head to grab at left or right sheathe
            for (int i = 0; i < (int)Handed.Max; i++)
            {
                if (!holding[i])
                {
                    Vec3 local_pos = head.orientation.Inverse * (pose_hld[i].position - head.position);
                    Handed side = local_pos.x < 0 ? Handed.Left : Handed.Right;
                    if (upper_sheathes[(int)side])
                    {
                        if (btn_grip[i].delta == +1)
                        {
                            upper_sheathes[(int)side] = false;
                            holding[i] = true;
                        }
                    }
                }
            }
        }
    }
}