using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;

namespace ArdupilotMega.HIL
{
    class XPlane : Hil
    {
        Socket SimulatorRECV;
        UdpClient XplanesSEND;
        EndPoint Remote = (EndPoint)(new IPEndPoint(IPAddress.Any, 0));

        // place to store the xplane packet data
        float[][] DATA = new float[113][];

        public override void SetupSockets(int recvPort, int SendPort, string simIP)
        {
            // setup receiver
            IPEndPoint ipep = new IPEndPoint(IPAddress.Any, recvPort);

            SimulatorRECV = new Socket(AddressFamily.InterNetwork,
                            SocketType.Dgram, ProtocolType.Udp);

            SimulatorRECV.Bind(ipep);

            UpdateStatus(-1, "Listerning on port UDP " + recvPort + " (sim->planner)\n");


            // setup sender
            XplanesSEND = new UdpClient(simIP, SendPort);

            UpdateStatus(-1, "Sending to port UDP " + SendPort + " (planner->sim)\n");

            setupXplane();

            UpdateStatus(-1, "Sent xplane settings\n");
        }
        public override void Shutdown()
        {
            try
            {
                SimulatorRECV.Close();
            }
            catch { }
            try
            {
                XplanesSEND.Close();
            }
            catch { }
        }

        public override void GetFromSim(ref sitl_fdm sitldata)
        {
            if (SimulatorRECV.Available > 0)
            {
                byte[] udpdata = new byte[1500];
                int receviedbytes = 0;
                try
                {
                    while (SimulatorRECV.Available > 0)
                    {
                        receviedbytes = SimulatorRECV.ReceiveFrom(udpdata, ref Remote);
                    }
                }
                catch { }

                if (udpdata[0] == 'D' && udpdata[1] == 'A')
                {
                    // Xplanes sends
                    // 5 byte header
                    // 1 int for the index - numbers on left of output
                    // 8 floats - might be useful. or 0 if not
                    int count = 5;
                    while (count < receviedbytes)
                    {
                        int index = BitConverter.ToInt32(udpdata, count);

                        DATA[index] = new float[8];

                        DATA[index][0] = BitConverter.ToSingle(udpdata, count + 1 * 4); ;
                        DATA[index][1] = BitConverter.ToSingle(udpdata, count + 2 * 4); ;
                        DATA[index][2] = BitConverter.ToSingle(udpdata, count + 3 * 4); ;
                        DATA[index][3] = BitConverter.ToSingle(udpdata, count + 4 * 4); ;
                        DATA[index][4] = BitConverter.ToSingle(udpdata, count + 5 * 4); ;
                        DATA[index][5] = BitConverter.ToSingle(udpdata, count + 6 * 4); ;
                        DATA[index][6] = BitConverter.ToSingle(udpdata, count + 7 * 4); ;
                        DATA[index][7] = BitConverter.ToSingle(udpdata, count + 8 * 4); ;

                        count += 36; // 8 * float
                    }

                    bool xplane9 = !xplane10;

                    if (xplane9)
                    {
                        sitldata.pitchDeg = (DATA[18][0]);
                        sitldata.rollDeg = (DATA[18][1]);
                        sitldata.yawDeg = (DATA[18][2]);
                        sitldata.pitchRate = (DATA[17][0] * rad2deg);
                        sitldata.rollRate = (DATA[17][1] * rad2deg);
                        sitldata.yawRate = (DATA[17][2] * rad2deg);

                        sitldata.heading = ((float)DATA[19][2]);
                    }
                    else
                    {
                        sitldata.pitchDeg = (DATA[17][0]);
                        sitldata.rollDeg = (DATA[17][1]);
                        sitldata.yawDeg = (DATA[17][2]);
                        sitldata.pitchRate = (DATA[16][0] * rad2deg);
                        sitldata.rollRate = (DATA[16][1] * rad2deg);
                        sitldata.yawRate = (DATA[16][2] * rad2deg);

                        sitldata.heading = (DATA[18][2]);
                    }

                    sitldata.airspeed = ((DATA[3][5] * .44704));

                    sitldata.latitude = (DATA[20][0]);
                    sitldata.longitude = (DATA[20][1]);
                    sitldata.altitude = (DATA[20][2] * ft2m);

                    sitldata.speedN = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
                    sitldata.speedE = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));

                    Matrix3 dcm = new Matrix3();
                    dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);

                    // rad = tas^2 / (tan(angle) * G)
                    float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.8f * Math.Tan(sitldata.rollDeg * deg2rad)));

                    float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs

                    // a = v^2/r
                    float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;

                    Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8));

                    Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));

                    accel_body -= centrip_accel;

                    sitldata.xAccel = DATA[4][5] * 9.8;
                    sitldata.yAccel = DATA[4][6] * 9.8;
                    sitldata.zAccel = (0 - DATA[4][4]) * 9.8;

                    //      Console.WriteLine(accel_body.ToString());
                    //      Console.WriteLine("        {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);

                }
            }
        }
        public override void SendToSim()
        {
            roll_out = (float)MainV2.comPort.MAV.cs.hilch1 / rollgain;
            pitch_out = (float)MainV2.comPort.MAV.cs.hilch2 / pitchgain;
            throttle_out = ((float)MainV2.comPort.MAV.cs.hilch3) / throttlegain;
            rudder_out = (float)MainV2.comPort.MAV.cs.hilch4 / ruddergain;

            // Limit min and max
            roll_out = Constrain(roll_out, -1, 1);
            pitch_out = Constrain(pitch_out, -1, 1);
            rudder_out = Constrain(rudder_out, -1, 1);
            throttle_out = Constrain(throttle_out, 0, 1);

            // sending only 1 packet instead of many.

            byte[] Xplane = new byte[5 + 36 + 36];

            if (heli)
            {
                Xplane = new byte[5 + 36 + 36 + 36];
            }

            Xplane[0] = (byte)'D';
            Xplane[1] = (byte)'A';
            Xplane[2] = (byte)'T';
            Xplane[3] = (byte)'A';
            Xplane[4] = 0;

            Array.Copy(BitConverter.GetBytes((int)25), 0, Xplane, 5, 4); // packet index

            Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 9, 4); // start data
            Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 13, 4);
            Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 17, 4);
            Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 21, 4);

            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 25, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 29, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 33, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 37, 4);

            // NEXT ONE - control surfaces

            Array.Copy(BitConverter.GetBytes((int)11), 0, Xplane, 41, 4); // packet index

            Array.Copy(BitConverter.GetBytes((float)(pitch_out * REV_pitch)), 0, Xplane, 45, 4); // start data
            Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll)), 0, Xplane, 49, 4);
            Array.Copy(BitConverter.GetBytes((float)(rudder_out * REV_rudder)), 0, Xplane, 53, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 57, 4);

            Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll * 0.5)), 0, Xplane, 61, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 65, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 69, 4);
            Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 73, 4);

            if (heli)
            {
                Array.Copy(BitConverter.GetBytes((float)(0)), 0, Xplane, 53, 4);


                int a = 73 + 4;
                Array.Copy(BitConverter.GetBytes((int)39), 0, Xplane, a, 4); // packet index
                a += 4;
                Array.Copy(BitConverter.GetBytes((float)(12 * collective_out)), 0, Xplane, a, 4); // main rotor 0 - 12
                a += 4;
                Array.Copy(BitConverter.GetBytes((float)(12 * rudder_out)), 0, Xplane, a, 4); // tail rotor -12 - 12
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
                a += 4;
                Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
            }

            try
            {
                XplanesSEND.Send(Xplane, Xplane.Length);

            }
            catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); }
        }
        public override void SendToAP(sitl_fdm sitldata)
        {
            TimeSpan gpsspan = DateTime.Now - lastgpsupdate;

            // add gps delay
            if (gpsspan.TotalMilliseconds >= GPS_rate)
            {
                lastgpsupdate = DateTime.Now;

                // save current fix = 3
                sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;

                //                Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));

                // return buffer index + 5 = (3 + 5) = 8 % 6 = 2
                oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];

                //comPort.sendPacket(oldgps);

                gpsbufferindex++;
            }

            MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();

            hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec

            hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
            hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
            hilstate.alt = (int)(oldgps.altitude * 1000); // mm

            //   Console.WriteLine(hilstate.alt);

            hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
            hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
            hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
            hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
            hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
            hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)

            hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100
            hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100
            hilstate.vz = 0; // m/s * 100

            hilstate.xacc = (short)(sitldata.xAccel * 1000); // (mg)
            hilstate.yacc = (short)(sitldata.yAccel * 1000); // (mg)
            hilstate.zacc = (short)(sitldata.zAccel * 1000); // (mg)

            MainV2.comPort.sendPacket(hilstate);

            MainV2.comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t()
            {
                airspeed = (float)sitldata.airspeed
            });
        }
        public override void GetFromAP() { }

        void setupXplane()
        {
            // sending only 1 packet instead of many.

            byte[] Xplane = new byte[5 + 4 * 8];

            Xplane[0] = (byte)'D';
            Xplane[1] = (byte)'S';
            Xplane[2] = (byte)'E';
            Xplane[3] = (byte)'L';
            Xplane[4] = 0;

            if (xplane10)
            {
                int pos = 5;
                Xplane[pos] = 0x3;
                pos += 4;
                Xplane[pos] = 0x4;
                pos += 4;
                Xplane[pos] = 0x6;
                pos += 4;
                Xplane[pos] = 0x10;
                pos += 4;
                Xplane[pos] = 0x11;
                pos += 4;
                Xplane[pos] = 0x12;
                pos += 4;
                Xplane[pos] = 0x14;
                pos += 4;
                Xplane[pos] = 0x15;
                pos += 4;
            }
            else
            {
                int pos = 5;
                Xplane[pos] = 0x3;
                pos += 4;
                Xplane[pos] = 0x4;
                pos += 4;
                Xplane[pos] = 0x6;
                pos += 4;
                Xplane[pos] = 0x11;
                pos += 4;
                Xplane[pos] = 0x12;
                pos += 4;
                Xplane[pos] = 0x13;
                pos += 4;
                Xplane[pos] = 0x14;
                pos += 4;
                Xplane[pos] = 0x15;
                pos += 4;
            }

            try
            {
                XplanesSEND.Send(Xplane, Xplane.Length);

            }
            catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); }

        }
    }
}