// command-line boilerplate
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using System.Threading;

// wiimote stuff
using WiimoteLib;
// many portions of the Wiimote code, including WiimoteLib,
// are lifted from http://blogs.msdn.com/coding4fun/archive/2007/03/14/1879033.aspx

// UDP/sockets stuff
using System.Net;
using System.Net.Sockets;
// UDP/socket code heavily influenced by samples from
// http://www.java2s.com/Code/CSharp/Network/Udp-Client.htm

namespace WiimoteUDPMonitor
{
    class Program
    {
        // UDP/socket statics
        private static string serverIP = "127.0.0.1";
        private static int serverPort = 9050;
        private static int listenPort = 9051;
        private static byte[] outData = new byte[32];
        private static byte[] inData = new byte[32];
        private static IPEndPoint serverIpep = new IPEndPoint(IPAddress.Parse(serverIP), serverPort);
        private static EndPoint serverEp = (EndPoint)serverIpep;
        
        // global socket since Wiimote handlers will be sending the data
        private static Socket server = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);

        // create a global instance of the Wiimote
        private static Wiimote wm = new Wiimote();
        // and all the states we want to track, if we want to filter for changes
        // (the Changed event seems to be called every time the wiimote samples,
        // regardless of whether values changed, and UDP does not like being
        // flooded)

        //IR stuff
        private static bool found1 = false;
        private static bool found2 = false;
        private static bool found3 = false;
        private static bool found4 = false;

        private static float x1 = 0;
        private static float y1 = 0;
        private static float x2 = 0;
        private static float y2 = 0;
        private static float x3 = 0;
        private static float y3 = 0;
        private static float x4 = 0;
        private static float y4 = 0;
        // can also get midpoints, dot-sizes.  See WiimoteLib page for details.

        // accelerometer stuff (AccelState)
        private static float ax = 0;
        private static float ay = 0;
        private static float az = 0;

        // buttons
        private static bool a = false;
        private static bool b = false;
        private static bool up = false;
        private static bool down = false;
        private static bool left = false;
        private static bool right = false;
        private static bool one = false;
        private static bool two = false;
        private static bool plus = false;
        private static bool minus = false;
        private static bool home = false;

        // nunchuk stuff
        private static float nax = 0;
        private static float nay = 0;
        private static float naz = 0;
        private static float nx = 0;
        private static float ny = 0;
        private static bool nc = false;
        private static bool nz = false;

        // classic controller stuff
        private static bool ca = false;
        private static bool cb = false;
        private static bool cx = false;
        private static bool cy = false;
        private static bool cup = false;
        private static bool cdown = false;
        private static bool cleft = false;
        private static bool cright = false;
        private static bool czl = false;
        private static bool czr = false;
        private static bool cplus = false;
        private static bool cminus = false;
        private static bool chome = false;
        private static float ctriggerl = 0;
        private static float ctriggerr = 0;

        // for where it's relevent, use a threshold to filter noise
        private static float threshold = 0.001f;

        static void Main(string[] args)
        {   
            // setup the event to handle state changes
            //wm.WiimoteChanged += new WiimoteChangedEventHandler(wm_OnWiimoteChanged);
            wm.WiimoteChanged += wm_OnWiimoteChanged;
            
            // setup the event to handle insertion/removal of extensions
            //wm.WiimoteExtensionChanged += new WiimoteExtensionChangedEventHandler(wm_OnWiimoteExtensionChanged);
            wm.WiimoteExtensionChanged += wm_OnWiimoteExtensionChanged;
            
            // connect to the Wiimote
            wm.Connect();
            
            // set the report type to return the IR sensor and accelerometer data (buttons always come back)
            // all we really need is the IR
            wm.SetReportType(InputReport.IRAccel, true);


            // once we have wiimote(s), find the desired app over UDP

            // find the desired app over UDP

            // set up a receiving port
            Socket listener = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
            IPEndPoint listenIpep = new IPEndPoint(IPAddress.Parse(serverIP), listenPort);//(IPAddress.Any, 0);
            EndPoint listenEp = (EndPoint)listenIpep; // one EP to bind to
            listener.Bind(listenEp);
            IPEndPoint incomingIpep = new IPEndPoint(IPAddress.Any, 0);
            EndPoint incomingEp = (EndPoint)incomingIpep; // and one to get the incoming IP/port in

            // repeat sending a test until we hear back
            string testMsg = "I HAS MANY DATAS. U WANT?";
            outData = Encoding.ASCII.GetBytes(testMsg);
            int recv = 0;
            int retry = 0;
            string inStr = "";
            Console.WriteLine("Broadcasting presence to IP{0} port {1}", serverIP, serverPort);
            while (recv == 0 || String.Compare(inStr,"I CAN HAS DATAS?",0)!=0)
            {
                Console.WriteLine("Attempt #{0}", retry);
                try
                {
                    server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
                    Console.WriteLine("Sent");
                    inData = new byte[512];
                    recv = listener.ReceiveFrom(inData, ref incomingEp);
                    inStr = new string(Encoding.ASCII.GetChars(inData));
                    Console.WriteLine("Received message: {0}",inStr);
                }
                catch (SocketException e)
                {
                    if (e.ErrorCode == 10054)
                        recv = 0;
                    else if (e.ErrorCode == 10040)
                    {
                        Console.WriteLine("Error receiving packet / no packet received");
                        recv = 0;
                    }
                }

                if (recv == 0)
                {
                    retry++;
                    Thread.Sleep(1000);
                }
            }
            // spin while the wiimote callbacks do their thing
            while (true)
            {
                Thread.Sleep(1000);
            }
        }

        static void wm_OnWiimoteExtensionChanged(object sender, WiimoteExtensionChangedEventArgs args)
        {
            if (args.Inserted)
                wm.SetReportType(InputReport.IRExtensionAccel, true);    // return extension data
            else
                wm.SetReportType(InputReport.IRAccel, true);            // back to original mode
        }

        static void wm_OnWiimoteChanged(object sender, WiimoteChangedEventArgs args)
        {
            // current state information
            WiimoteState ws = args.WiimoteState;

            // write out the state of the A button    
            // Debug.WriteLine(ws.ButtonState.A);

            /*
            if (found1 != ws.IRState.Found1) Console.WriteLine("found1 differs: {0}!={1}", found1, ws.IRState.Found1);
            if (found2 != ws.IRState.Found2) Console.WriteLine("found2 differs: {0}!={1}", found2, ws.IRState.Found2);
            if (found3 != ws.IRState.Found3) Console.WriteLine("found3 differs: {0}!={1}", found3, ws.IRState.Found3);
            if (found4 != ws.IRState.Found4) Console.WriteLine("found4 differs: {0}!={1}", found4, ws.IRState.Found4);
            if (Math.Abs(x1 - ws.IRState.X1) > threshold) Console.WriteLine("X1 differs: {0}!={1}", x1, ws.IRState.X1);
            if (Math.Abs(y1 - ws.IRState.Y1) > threshold) Console.WriteLine("Y1 differs: {0}!={1}", y1, ws.IRState.Y1);
            if (Math.Abs(x2 - ws.IRState.X2) > threshold) Console.WriteLine("X2 differs: {0}!={1}", x2, ws.IRState.X2);
            if (Math.Abs(y2 - ws.IRState.Y2) > threshold) Console.WriteLine("Y2 differs: {0}!={1}", y2, ws.IRState.Y2);
            if (Math.Abs(x3 - ws.IRState.X3) > threshold) Console.WriteLine("X3 differs: {0}!={1}", x3, ws.IRState.X3);
            if (Math.Abs(y3 - ws.IRState.Y3) > threshold) Console.WriteLine("Y3 differs: {0}!={1}", y3, ws.IRState.Y3);
            if (Math.Abs(x4 - ws.IRState.X4) > threshold) Console.WriteLine("X4 differs: {0}!={1}", x4, ws.IRState.X4);
            if (Math.Abs(y4 - ws.IRState.Y4) > threshold) Console.WriteLine("Y4 differs: {0}!={1}", y4, ws.IRState.Y4);
            */

            // send packets with the updated info
            // IR points (can alternatively use .RawX1/.RawY1/etc.)
            if (
                found1!=ws.IRState.IRSensors[0].Found ||
                Math.Abs(x1 - ws.IRState.IRSensors[0].RawPosition.X) > threshold ||
                Math.Abs(y1 - ws.IRState.IRSensors[0].RawPosition.Y) > threshold
            )
            {
                found1 = ws.IRState.IRSensors[0].Found;

                x1 = ws.IRState.IRSensors[0].RawPosition.X;
                y1 = ws.IRState.IRSensors[0].RawPosition.Y;

                string packetData = String.Format("0 {0} {1} {2}", found1, x1, y1);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            if (
                found2 != ws.IRState.IRSensors[1].Found ||
                Math.Abs(x2 - ws.IRState.IRSensors[1].RawPosition.X) > threshold ||
                Math.Abs(y2 - ws.IRState.IRSensors[1].RawPosition.Y) > threshold
            )
            {
                found2 = ws.IRState.IRSensors[1].Found;

                x2 = ws.IRState.IRSensors[1].RawPosition.X;
                y2 = ws.IRState.IRSensors[1].RawPosition.Y;

                string packetData = String.Format("1 {0} {1} {2}", found2, x2, y2);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            if (
                found3 != ws.IRState.IRSensors[2].Found ||
                Math.Abs(x3 - ws.IRState.IRSensors[2].RawPosition.X) > threshold ||
                Math.Abs(y3 - ws.IRState.IRSensors[2].RawPosition.Y) > threshold
            )
            {
                found3 = ws.IRState.IRSensors[2].Found;

                x3 = ws.IRState.IRSensors[2].RawPosition.X;
                y3 = ws.IRState.IRSensors[2].RawPosition.Y;

                string packetData = String.Format("2 {0} {1} {2}", found3, x3, y3);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            if (
                found4 != ws.IRState.IRSensors[3].Found ||
                Math.Abs(x4 - ws.IRState.IRSensors[3].RawPosition.X) > threshold ||
                Math.Abs(y4 - ws.IRState.IRSensors[3].RawPosition.Y) > threshold
            )
            {
                found4 = ws.IRState.IRSensors[3].Found;

                x4 = ws.IRState.IRSensors[3].RawPosition.X;
                y4 = ws.IRState.IRSensors[3].RawPosition.Y;

                string packetData = String.Format("3 {0} {1} {2}", found4, x4, y4);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // acclelerometer (can alternatively use .RawX/.RawY/.RawZ)
            if (
                Math.Abs(ax - ws.AccelState.RawValues.X) > threshold ||
                Math.Abs(ay - ws.AccelState.RawValues.Y) > threshold ||
                Math.Abs(az - ws.AccelState.RawValues.Z) > threshold
            )
            {
                ax = ws.AccelState.RawValues.X;
                ay = ws.AccelState.RawValues.Y;
                az = ws.AccelState.RawValues.Z;

                string packetData = String.Format("a {0} {1} {2}", ax, ay, az);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // buttons
            if (
                a != ws.ButtonState.A ||
                b != ws.ButtonState.B ||
                up != ws.ButtonState.Up ||
                left != ws.ButtonState.Left ||
                down != ws.ButtonState.Down ||
                right != ws.ButtonState.Right ||
                one != ws.ButtonState.One ||
                two != ws.ButtonState.Two ||
                plus != ws.ButtonState.Plus ||
                minus != ws.ButtonState.Minus ||
                home != ws.ButtonState.Home
            )
            {
                a = ws.ButtonState.A;
                b = ws.ButtonState.B;
                up = ws.ButtonState.Up;
                left = ws.ButtonState.Left;
                down = ws.ButtonState.Down;
                right = ws.ButtonState.Right;
                one = ws.ButtonState.One;
                two = ws.ButtonState.Two;
                plus = ws.ButtonState.Plus;
                minus = ws.ButtonState.Minus;
                home = ws.ButtonState.Home;

                // could pack these into binary, but python is not
                // nearly as happy doing char->binary covnersion
                // as C is at doing binary->byte->char
                string packetData = "b ";
                if (a) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (b) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (up) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (left) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (down) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (right) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (one) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (two) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (plus) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (minus) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (home) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";

                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // nunchuk accel  (can alternatively use .RawX/.RawY/.RawZ)
            if (
                Math.Abs(nax - ws.NunchukState.AccelState.RawValues.X) > threshold ||
                Math.Abs(nay - ws.NunchukState.AccelState.RawValues.Y) > threshold ||
                Math.Abs(naz - ws.NunchukState.AccelState.RawValues.Z) > threshold
            )
            {
                nax = ws.NunchukState.AccelState.RawValues.X;
                nay = ws.NunchukState.AccelState.RawValues.Y;
                naz = ws.NunchukState.AccelState.RawValues.Z;

                string packetData = String.Format("n {0} {1} {2}", nax, nay, naz);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // nunchuk joystick & buttons
            if (
                Math.Abs(nx - ws.NunchukState.Joystick.X ) > threshold ||
                Math.Abs(ny - ws.NunchukState.Joystick.Y) > threshold ||
                nc != ws.NunchukState.C ||
                nz != ws.NunchukState.Z 
            )
            {
                nx = ws.NunchukState.Joystick.X;
                ny = ws.NunchukState.Joystick.Y;
                nc = ws.NunchukState.C;
                nz = ws.NunchukState.Z;

                string packetData = String.Format("j {0} {1} {2} {3}", nx, ny, nc, nz);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // classic controller buttons
            if (
                ca != ws.ClassicControllerState.ButtonState.A ||
                cb != ws.ClassicControllerState.ButtonState.B ||
                cx != ws.ClassicControllerState.ButtonState.X ||
                cy != ws.ClassicControllerState.ButtonState.Y ||
                cup != ws.ClassicControllerState.ButtonState.Up ||
                cleft != ws.ClassicControllerState.ButtonState.Left ||
                cdown != ws.ClassicControllerState.ButtonState.Down ||
                cright != ws.ClassicControllerState.ButtonState.Right ||
                czl != ws.ClassicControllerState.ButtonState.ZL ||
                czr != ws.ClassicControllerState.ButtonState.ZR ||
                cplus != ws.ClassicControllerState.ButtonState.Plus ||
                cminus != ws.ClassicControllerState.ButtonState.Minus ||
                chome != ws.ClassicControllerState.ButtonState.Home
            )
            {
                ca = ws.ClassicControllerState.ButtonState.A;
                cb = ws.ClassicControllerState.ButtonState.B;
                cx = ws.ClassicControllerState.ButtonState.X;
                cy = ws.ClassicControllerState.ButtonState.Y;
                cup = ws.ClassicControllerState.ButtonState.Up;
                cleft = ws.ClassicControllerState.ButtonState.Left;
                cdown = ws.ClassicControllerState.ButtonState.Down;
                cright = ws.ClassicControllerState.ButtonState.Right;
                czl = ws.ClassicControllerState.ButtonState.ZL;
                czr = ws.ClassicControllerState.ButtonState.ZR;
                cplus = ws.ClassicControllerState.ButtonState.Plus;
                cminus = ws.ClassicControllerState.ButtonState.Minus;
                chome = ws.ClassicControllerState.ButtonState.Home;

                // could pack these into binary, but python is not
                // nearly as happy doing char->binary covnersion
                // as C is at doing binary->byte->char
                string packetData = "c ";
                if (ca) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cb) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cx) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cy) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cup) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cleft) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cdown) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cright) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (czl) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (czr) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cplus) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (cminus) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";
                if (chome) packetData = packetData + "1 ";
                else packetData = packetData + "0 ";

                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }

            // classic controller triggers (can alternatively use .RawTriggerL/.RawTriggerR)
            if (
                Math.Abs(ctriggerl - ws.ClassicControllerState.TriggerL ) > threshold ||
                Math.Abs(ctriggerr - ws.ClassicControllerState.TriggerR) > threshold 
            )
            {
                ctriggerl = ws.ClassicControllerState.TriggerL;
                ctriggerr = ws.ClassicControllerState.TriggerR;

                string packetData = String.Format("t {0} {1}", ctriggerl, ctriggerr);
                outData = Encoding.ASCII.GetBytes(packetData);
                server.SendTo(outData, outData.Length, SocketFlags.None, serverIpep);
            }
        }

    }
}