using System;
using System.Collections;
using WiimoteLib;

namespace Log740
{
    public class WiimoteButtonEvent : EventArgs
    {
        public BUTTON_TYPE ButtonType;
        public bool Pressed;

        public WiimoteButtonEvent(BUTTON_TYPE type, bool pressed)
        {
            ButtonType = type;
            Pressed = pressed;
        }
    }

    public enum BUTTON_TYPE
    {
        A_BUTTON,
        B_BUTTON,
        HOME_BUTTON,
        PLUS_BUTTON,
        MINUS_BUTTON,
        ONE_BUTTON,
        TWO_BUTTON,
        Z_BUTTON,
        C_BUTTON,
        LEFT,
        RIGHT,
        UP,
        DOWN
    }

    public class WiiInput
    {
        public static double JOYSTICK_DEADZONE = 0.02;

        private long total_updates;
        private int last_index;
        private int nb_samples;

        private Point3F accel;
        private Point3F[] accel_samples;
        private Point3F DownAxis;
        private Quaternion initial_rotation;
        private Quaternion rotation;

        private bool cummulative_rotation;

        private Wiimote wm;

        private bool rumble = false;
        private bool led1 = false, led2 = false, led3 = false, led4 = false;

        private NunchukState nunchuck;
        private ButtonState buts;

        private Queue event_queue = Queue.Synchronized(new Queue());

        /* Public handlers */
        public event EventHandler<WiimoteButtonEvent> ButtonPress;
        public event EventHandler<WiimoteButtonEvent> ButtonDepress;

        /* methods */

        public WiiInput(int Smooth, Quaternion InitialRotation, bool CummulativeRotation)
        {
            this.nb_samples = Smooth;
            this.last_index = 0;
            this.total_updates = 0;
            this.accel_samples = new Point3F[Smooth];
            this.wm = new Wiimote();

            this.DownAxis.X = 0;
            this.DownAxis.Y = 0;
            this.DownAxis.Z = -1;

            this.accel = this.DownAxis; // initialize with down axis, for cummulative rotation

            this.rotation = InitialRotation;

            this.initial_rotation = InitialRotation;
            this.cummulative_rotation = CummulativeRotation;

            this.wm.WiimoteChanged += wm_WiimoteChanged;
            this.wm.WiimoteExtensionChanged += wm_WiimoteExtensionChanged;
            try
            {
                this.wm.Connect();
            }
            catch (WiimoteNotFoundException)
            {
                // Do something smart here
            }
            this.wm.SetReportType(InputReport.IRAccel, true);
        }

        public void Disconnect()
        {
            wm.Disconnect();
        }

        public void SetLEDs(bool led1, bool led2, bool led3, bool led4)
        {
            if (led1 != this.led1 || led2 != this.led2 || led3 != this.led3 || led4 != this.led4)
            {
                this.led1 = led1;
                this.led2 = led2;
                this.led3 = led3;
                this.led4 = led4;
                this.wm.SetLEDs(led1, led2, led3, led4);
            }
        }

        public void SetRumble(bool value)
        {
            if (value != rumble)
            {
                rumble = value;
                wm.SetRumble(rumble);
            }
        }
        
        public void DoEvents()
        {
            while (this.event_queue.Count != 0)
            {
                WiimoteButtonEvent evt = (WiimoteButtonEvent)this.event_queue.Dequeue();

                if (evt.Pressed)
                {
                    this.ButtonPress(this, evt);
                }
                else
                {
                    this.ButtonDepress(this, evt);
                }
            }
        }

        private void AddEvent(WiimoteButtonEvent evt)
        {
            this.event_queue.Enqueue(evt);
        }

        private static bool validState(WiimoteState ws)
        {
            return !float.IsInfinity(ws.AccelState.Values.X) && !float.IsInfinity(ws.AccelState.Values.Y) && !float.IsInfinity(ws.AccelState.Values.Z);
        }

        private void wm_WiimoteExtensionChanged(object sender, WiimoteExtensionChangedEventArgs args)
        {
            if (args.Inserted)
                wm.SetReportType(InputReport.IRExtensionAccel, true);
            else
                wm.SetReportType(InputReport.IRAccel, true);
        }

        private void wm_WiimoteChanged(object sender, WiimoteChangedEventArgs args)
        {
            WiimoteState ws = args.WiimoteState;
            Point3F old_accel;
            long weight = 1;

            if (validState(ws))
            {
                this.accel_samples[this.last_index] = ws.AccelState.Values;

                this.total_updates += 1;
                this.last_index = (this.last_index + 1) % this.nb_samples;

                old_accel = accel;

                accel.X = 0;
                accel.Y = 0;
                accel.Z = 0;

                foreach (Point3F sample in this.accel_samples)
                {
                    accel.X += sample.X * weight;
                    accel.Y += sample.Y * weight;
                    accel.Z += sample.Z * weight;

                    weight += 1;
                }

                weight = Math.Min(this.nb_samples, this.total_updates);
                weight = (weight * weight) / 2 + weight - 1;

                weight = Math.Max(weight, 1);

                accel.X /= weight;
                accel.Y /= weight;
                accel.Z /= weight;

                /* from left-hand to right-hand */
                accel.Z = -accel.Z;

                if (this.cummulative_rotation)
                {
                    this.rotation = Quaternion.BetweenAxis(old_accel, this.accel) * this.rotation;
                }
                else
                {
                    this.rotation = Quaternion.BetweenAxis(this.DownAxis, this.initial_rotation * this.accel);
                }
            }

            if (this.ButtonPress != null)
            {
                if (ws.ButtonState.A && !this.buts.A)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.A_BUTTON, true));
                }

                if (ws.ButtonState.B && !this.buts.B)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.B_BUTTON, true));
                }

                if (ws.ButtonState.Home && !this.buts.Home)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.HOME_BUTTON, true));
                }

                if (ws.ButtonState.Plus && !this.buts.Plus)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.PLUS_BUTTON, true));
                }

                if (ws.ButtonState.Minus && !this.buts.Minus)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.MINUS_BUTTON, true));
                }

                if (ws.ButtonState.One && !this.buts.One)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.ONE_BUTTON, true));
                }

                if (ws.ButtonState.Two && !this.buts.Two)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.TWO_BUTTON, true));
                }

                if (ws.ButtonState.Left && !this.buts.Left)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.LEFT, true));
                }
                if (ws.ButtonState.Right && !this.buts.Right)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.RIGHT, true));
                }
                if (ws.ButtonState.Up && !this.buts.Up)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.UP, true));
                }
                if (ws.ButtonState.Down && !this.buts.Down)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.DOWN, true));
                }
            }

            if (this.ButtonDepress != null)
            {
                if (!ws.ButtonState.A && this.buts.A)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.A_BUTTON, false));
                }

                if (!ws.ButtonState.B && this.buts.B)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.B_BUTTON, false));
                }

                if (!ws.ButtonState.Home && this.buts.Home)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.HOME_BUTTON, false));
                }

                if (!ws.ButtonState.Plus && this.buts.Plus)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.PLUS_BUTTON, false));
                }

                if (!ws.ButtonState.Minus && this.buts.Minus)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.MINUS_BUTTON, false));
                }

                if (!ws.ButtonState.One && this.buts.One)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.ONE_BUTTON, false));
                }

                if (!ws.ButtonState.Two && this.buts.Two)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.TWO_BUTTON, false));
                }

                if (!ws.ButtonState.Left && this.buts.Left)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.LEFT, false));
                }
                if (!ws.ButtonState.Right && this.buts.Right)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.RIGHT, false));
                }
                if (!ws.ButtonState.Up && this.buts.Up)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.UP, false));
                }
                if (!ws.ButtonState.Down && this.buts.Down)
                {
                    AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.DOWN, false));
                }
            }

            /* copy new state over old */
            this.buts = ws.ButtonState;

            if (ws.ExtensionType == WiimoteLib.ExtensionType.Nunchuk)
            {
                if (this.ButtonPress != null)
                {
                    if (ws.NunchukState.Z && !this.nunchuck.Z)
                    {
                        AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.Z_BUTTON, true));
                    }

                    if (ws.NunchukState.C && !this.nunchuck.C)
                    {
                        AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.C_BUTTON, true));
                    }
                }

                if (this.ButtonDepress != null)
                {
                    if (!ws.NunchukState.Z && this.nunchuck.Z)
                    {
                        AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.Z_BUTTON, false));
                    }

                    if (!ws.NunchukState.C && this.nunchuck.C)
                    {
                        AddEvent(new WiimoteButtonEvent(BUTTON_TYPE.C_BUTTON, false));
                    }
                }

                this.nunchuck = ws.NunchukState;
            }
        }

        public NunchukState Nunchuck
        {
            get
            {
                return this.nunchuck;
            }
        }

        public ButtonState Buttons
        {
            get
            {
                return this.buts;
            }
        }

        public Quaternion Rotation
        {
            get
            {
                return this.rotation;
            }
        }

        public Point3F Accelerators
        {
            get
            {
                return this.accel;
            }
        }
    }
}
