using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace PG_Sim
{
    /// <summary>
    /// Class operates with human interface device (keyboard, gamepad, pg_simulator)
    /// </summary>
    public static class Controll
    {
        #region HID names (KEYBOARD, GAMEPAD, PG_SIM) 
        public const int KEYBOARD = 1;
        public const int GAMEPAD = 2;
        public const int PG_SIM = 3;
        #endregion

        static public bool run = false;
        static private int init_hid = 0;
        static private byte bytes_received = 255;
        static private char[] buffer = new char[20];
        static private bool init = false;
        static private double calib_z = 0;

        // controls handles
        public static Support.USB_Joystick js;
        private static System.IO.Ports.SerialPort serialPort1 = new System.IO.Ports.SerialPort();

        public static void Init(System.Windows.Forms.Form form)
        {
            // com port parameters
            serialPort1.BaudRate = 115200;
            serialPort1.PortName = "COM6";

            // first testing pluged the pg_sim
            try
            {
                serialPort1.Open();
                // sets up receiving method
                serialPort1.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(serialRxd);
                init_hid = PG_SIM;
                run = true;
            }
            catch
            {
                Support.Logger.Add("Notice: COM port is not openable!");
            }

            // testing pluged the gamepad
            if (init_hid == 0)
            {
                try
                {   // initialize joystick
                    js = new Support.USB_Joystick(0, 1000, .20, form);
                    js.PresetValues();
                    init_hid = GAMEPAD;
                    run = true;
                }
                catch
                {
                    Support.Logger.Add("Notice: Joystic is not connected!");
                }
            }
            // joystick or pg_sim is not connected
            if (init_hid == 0)
                // sets up a keyboard as HID
                init_hid = KEYBOARD;
        }

        public static void keyboardInput(System.Windows.Forms.KeyEventArgs e,System.Threading.Thread Th_Physics)
        {
            #region system controls 
            // zobrazení/skrytí sil nebo padáku
            if (e.KeyCode.ToString() == "F")
                Renderer.showForce = !Renderer.showForce;
            if (e.KeyCode.ToString() == "G")
                Renderer.showGlide = !Renderer.showGlide;

            // pauza mezernikem
            if (e.KeyCode == Keys.Space)
            {
                if (Physics.run_solver == true)
                {
                    Physics.run_solver = false;
                    Th_Physics.Abort();
                }
                else
                {
                    Physics.run_solver = true;
                    Th_Physics = new System.Threading.Thread(Physics.Solver);
                    Th_Physics.Name = "Physics";
                    Th_Physics.Start();
                }
            }

            // vypnutí aplikace escapem
            if (e.KeyCode == Keys.Escape)
                Application.Exit();
            #endregion;

            #region camera select 
            if (e.KeyValue == 49) // stisknutá 1 na alfanum. 
                Renderer.camera = 1;
            if (e.KeyValue == 50) // stisknutá 2 na alfanum.
                Renderer.camera = 2;
            if (e.KeyValue == 51) // stisknutá 3 na alfanum.
                Renderer.camera = 3;
            if (e.KeyValue == 52) // stisknutá 4 na alfanum.
                Renderer.camera = 4;
            #endregion

            #region paraglide controls 
            if (init_hid == KEYBOARD)
            {
                switch (e.KeyCode)
                {
                    case Keys.L:
                        Physics.Controlls("right");
                        break;
                    case Keys.J:
                        Physics.Controlls("left");
                        break;
                    case Keys.I:
                        Physics.Controlls("bothDown");
                        break;
                    case Keys.K:
                        Physics.Controlls("bothUp");
                        break;
                }
            }
            #endregion
        }


        public static void SaveValues()
        {
            if (init_hid == 0)
                throw new InvalidOperationException("HID is not init!");
            while (run || init_hid==KEYBOARD)
            {
                // put controll data to Physics
                if (init_hid == PG_SIM)
                    serialTxd();
                else if (init_hid == GAMEPAD)
                {
                    Controls control_values = GetData_Joystick();
                    
                    // send to Physics
                    Physics.Controlls(control_values);
                }
 
                // slows down speed of getting datas
                System.Threading.Thread.Sleep(10);
            }
        }

        private static Controls GetData_Joystick()
        {
            // read values
            try
            {
                js.GetData();
            }
            catch
            {
                throw new InvalidOperationException("Some problem with joystick, maybe is not connected!");
            }

            // set controls
            double leftAdd = (double)(js.State.Y - 500) / 25000;
            double rightAdd = js.ButtonPressed(1) ? 0.02 : (js.ButtonPressed(4) ? -0.02 : 0);
            double speedAdd = js.ButtonPressed(6) ? 0.02 : (js.ButtonPressed(5) ? -0.02 : 0);

            double harness = 0;
            if (js.State.X == 0)
                harness = -0.02;
            else if (js.ButtonPressed(2))
                harness = 0.02;

            return new Controls(leftAdd,rightAdd,speedAdd,harness);
        }

        private static void serialRxd(object sender, System.IO.Ports.SerialDataReceivedEventArgs e) 
        {
            while (serialPort1.BytesToRead > 0)
            {
                char c = Convert.ToChar(serialPort1.ReadChar());
                if (c == 'x') // uvodni znak - reset prijmoveho engine
                    bytes_received = 0;
                else
                    if (bytes_received < 20) // pridame znak do zpravy
                    {
                        if ((c >= '0') && (c <= '9')) // je to cislice
                        {
                            buffer[bytes_received] = Convert.ToChar(c - '0');
                            bytes_received++;
                        }
                        else
                            if ((c >= 'a') && (c <= 'f')) // je to pismeno a az f
                            {
                                buffer[bytes_received] = Convert.ToChar(c - 'a' + 10);
                                bytes_received++;
                            }
                            else // je to nejaka blbost nebo cokoliv jineho, napr. uplne jina blbost
                                bytes_received = 255;
                    }

                if (bytes_received == 20) // mame celou zpravu
                {
                    const int offset = 2940;
                    const double delka_ridicek = 300;
                    const double tilt_mpy = 16;
                    const double speed_mpy = 20;

                    if (!init)
                    {
                        calib_z = (prevod(16) + prevod(12)) / 2;
                        init = true;
                    }

                    double leva_r = 0;// prevod(0) / delka_ridicek; // leva ridicka
                    double prava_r = 0;//prevod(4)/delka_ridicek;
                    double acc_X = (prevod(8) - calib_z) / 1000;
                    double acc_Y = (prevod(12) - calib_z) / 1000;
                    double acc_Z = (prevod(16) - calib_z) / 1000;
                    bytes_received = 255;

                    // vypocet uhlu
                    double tilt_angle = Math.Atan2(acc_Y, acc_X) * 180 / Math.PI;
                    double speed_angle = Math.Atan2(acc_Z, acc_X) * 180 / Math.PI;
                    //Console.WriteLine(speed_angle);
                    // odešleme do modelu
                    Physics.Controlls(leva_r, prava_r, tilt_angle / tilt_mpy, speed_angle / speed_mpy);
                }
            }
        }
        private static void serialTxd()
        {
            // vytvorit zpravu
            double left_force = -12 * 1;// leva_r + 100; //Math.Pow(leva_r, 2) / 10;

            serialPort1.Write("x" + doubleToHex(left_force) + "a5b6ab");
        }

        public static void Destroy()
        {
            // turn off the SaveValue cycle
            run = false;

            // destroy joystick session
            if (init_hid == PG_SIM)
            {
                serialPort1.Close();
            }
            else if (init_hid == GAMEPAD)
                js.Dispose();

            init_hid = 0;
        }

        // ***************** Helpful methods ***********************************************
        private static string doubleToHex(double value) 
        {
            string hex = "";
            Int32 val_int = Convert.ToInt32(value);
            for (int i = 3; i >= 0; i--)
            {
                Int32 val_i = (val_int >> (i * 4)) & 0x0F;
                char add;
                if (val_i < 10)
                    add = Convert.ToChar('0' + val_i);
                else
                    add = Convert.ToChar('a' + (val_i - 10));
                hex += add;
            }
            return hex;
        }
        private static double prevod(short i) 
        {
            Int32 vysledek = 0;

            vysledek = (Convert.ToInt32(buffer[i]) << 12);
            vysledek |= (Convert.ToInt32(buffer[i + 1]) << 8);
            vysledek |= (Convert.ToInt32(buffer[i + 2]) << 4);
            vysledek |= Convert.ToInt32(buffer[i + 3]);
            return Convert.ToDouble((Int16)vysledek);
        }
    }

    public struct Controls
    {
        public double leftBrake;   // < 0,1>
        public double rightBrake;  // < 0,1>
        public double speed;       // < 0,1>
        public double harness;     // <-1,1>

        public Controls(double zero)
        {
            leftBrake = 0;
            rightBrake = 0;
            speed = 0;
            harness = 0;
        }
        public Controls(double leftB, double rightB, double speedSystem, double tilt)
        {
            leftBrake = leftB;
            rightBrake = rightB;
            speed = speedSystem;
            harness = tilt;
        }
        

        public string printVal(string variable)
        {
            switch (variable)
            {
                case "left":
                    return Math.Round(leftBrake * 100).ToString()+"%";
                case "right":
                    return Math.Round(rightBrake * 100).ToString() + "%";
                case "speed":
                    return Math.Round(speed * 100).ToString() + "%";
            }
            return "";
        }
    }
}
