using System;
using System.Numerics;
using System.Runtime.InteropServices;

using PointID = System.UInt64;

namespace Demo
{
    public class LivePlotter
    {
        [DllImport("LivePlotter.dll")]
        public static extern bool start(int win_w, int win_h);

        [DllImport("LivePlotter.dll")]
        public static extern bool stop();

        [DllImport("LivePlotter.dll")]
        public static extern PointID create_point(float x, float y, float z);

        [DllImport("LivePlotter.dll")]
        public static extern void set_color(PointID id, float r, float g, float b);

        [DllImport("LivePlotter.dll")]
        public static extern void set_scale(PointID id, float scale);

        [DllImport("LivePlotter.dll")]
        public static extern void update_point(PointID id, float x, float y, float z);

        [DllImport("LivePlotter.dll")]
        public static extern void set_lifetime(PointID id, float new_lifetime_s);

        [DllImport("LivePlotter.dll")]
        public static extern void clear_point(PointID id);

        public static void PlotCameraPoses()
        {
            string[] lines = File.ReadAllLines("poses.csv");
            for (int camera_i = 1; camera_i < lines.Length; camera_i++)
            {
                string[] words = lines[camera_i].Split(',');
                float x = float.Parse(words[0]);
                float y = float.Parse(words[1]);
                float z = float.Parse(words[2]);
                float w = float.Parse(words[3]);
                float i = float.Parse(words[4]);
                float j = float.Parse(words[5]);
                float k = float.Parse(words[6]);

                Pose pose = new(new Quaternion(i, j, k, w), new Vector3(x, y, z));
                for (int m = 0; m < 3; m++)
                {
                    for (int n = 0; n < 25; n++)
                    {
                        Vector3 local = new(0);
                        local[m] = n * 10.0f;
                        Vector3 global = pose * local;
                        PointID id = create_point(global.X, global.Y, global.Z);

                        Vector3 color = new(0);
                        color[m] = 1.0f;
                        set_color(id, color.X, color.Y, color.Z);
                        set_scale(id, 15);
                    }
                }
            }
        }
    }
}