using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

using System.IO;
using System.IO.Ports;

using NETGeographicLib;

namespace CPFTrack
{
    public partial class Form1 : Form
    {
        public delegate void IUSBLUpdateHandler(object sender, IUSBLData e);
        public event IUSBLUpdateHandler OnIUSBLUpdate;

        public Form1()
        {
            InitializeComponent();
        }

        private SerialPort iusblSerialPort = new SerialPort();
        private static byte[] iusblMsgBuffer;
        private void Form1_Load(object sender, EventArgs e)
        {
            formStripChart = new FormStripChart(this);
            formStripChart.FormClosed += formStripChart_FormClosed;
            formStripChart.Show();

            iusblSerialPort.BaudRate = 9600;
            iusblSerialPort.DataBits = 8;
            iusblSerialPort.Handshake = Handshake.None;
            iusblSerialPort.Parity = Parity.None;
            iusblSerialPort.PortName = "COM5";
            iusblSerialPort.StopBits = StopBits.One;
            iusblMsgBuffer = new byte[iusblSerialPort.ReadBufferSize];
        
            iusblSerialPort.DataReceived += new SerialDataReceivedEventHandler(iusblSerialPort_DataReceived);

            iusblSerialPort.Open();

            timerIUSBL.Enabled = true;
        }

        private static double depthValue = 0.0;
        private static Random randomValue = new Random();

        private void UpdateStatus(IUSBLData args)
        {
            // Make sure someone is listening to event
            if (OnIUSBLUpdate == null) 
                return;

            //ProgressEventArgs args = new ProgressEventArgs(status);
            OnIUSBLUpdate(this, args);
        }

        public class IUSBLData : EventArgs
        {
            public double rangeValue = double.NaN;
            public double xValue = double.NaN;
            public double yValue = double.NaN;
            public double zValue = double.NaN;

            public DateTime timeStamp = new DateTime();
        }
        private static IUSBLData iusblData = new IUSBLData();

 
        FormStripChart formStripChart;
        private void newStripChartToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (formStripChart == null)
            {
                formStripChart = new FormStripChart(this);
                formStripChart.FormClosed += formStripChart_FormClosed;
                formStripChart.Show();
            }
            else
                formStripChart.Activate();
        }

        void formStripChart_FormClosed(object sender, FormClosedEventArgs e)
        {
            formStripChart = null;
        }

        private void exitToolStripMenuItem_Click(object sender, EventArgs e)
        {
            Application.Exit();
        }

        private string inString = string.Empty;
        private StringBuilder inSB = new StringBuilder(256);
        delegate void SetTextCallback(String msgString);
 
        private void iusblSerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {

            //There is no accurate method for checking how many bytes are read 
            //unless you check the return from the Read method 
            int bytesRead = iusblSerialPort.Read(iusblMsgBuffer, 0, iusblMsgBuffer.Length);

            //For the example assume the data we are received is ASCII data. 
            inString += Encoding.ASCII.GetString(iusblMsgBuffer, 0, bytesRead);

            if (inString.Contains("\n"))
            {
                this.BeginInvoke(new SetTextCallback(parseIUSBL), new object[] { inString });
                inString = String.Empty;
            }
        }

        private static string[] inFields;
        private static LLZPoint floatLLZ = new LLZPoint();
        private void parseIUSBL(String inString)
        {
            iusblData.timeStamp = DateTime.Now;
            Console.WriteLine("Received: " + inString);

            inFields = inString.Split(',', 'R', 'X', 'Y', 'Z');

            iusblData.rangeValue = double.Parse(inFields[2]);
            if(inFields.Length > 5)
            {
                iusblData.xValue = double.Parse(inFields[4]);
                tbIUSBLEasting.Text = inFields[4];
                iusblData.yValue = double.Parse(inFields[6]);
                tbIUSBLNorthing.Text = inFields[6];
                iusblData.zValue = double.Parse(inFields[8]);
                tbIUSBLDepth.Text = iusblData.zValue.ToString("f2");
            }
            UpdateStatus(iusblData);

            floatLLZ = FloatXYZToLatLon( iusblData.xValue, iusblData.yValue, iusblData.zValue, 45);
            writeKML(floatLLZ);
        }

        private LLZPoint FloatXYZToLatLon(double xIn, double yIn, double zIn, double angleIn)
        {
            XYZPoint boatXYZ;
            XYZPoint floatXYZ;
            XYZPoint rotFloatXYZ;
            LLZPoint floatLLZ;

            double shipLat =  36.802100, shipLon = -121.853717;
   
            tbLatitude.Text = shipLat.ToString("f6");
            tbLongitude.Text = shipLon.ToString("f6");
            
            int zone;
            bool northp;
            UTMUPS.Forward(shipLat, shipLon, out zone, out northp, out boatXYZ.x, out boatXYZ.y, -1, true);
            string zonestr = UTMUPS.EncodeZone(zone, northp, true);
           // Console.WriteLine("Boat UTM x, y = " + boatXYZ.x.ToString("f3") + "  " + boatXYZ.y.ToString("f3"));

            floatXYZ.x = xIn;
            floatXYZ.y = yIn;
            floatXYZ.z = zIn;
            rotFloatXYZ = rotateXY(floatXYZ, angleIn);

         //   Console.WriteLine("Roated float x, y, z = " + rotFloatXYZ.x.ToString("f3") + "  " + rotFloatXYZ.y.ToString("f3") + "  " + rotFloatXYZ.z.ToString("f3"));

            floatXYZ.x = boatXYZ.x + rotFloatXYZ.x;
            floatXYZ.y = boatXYZ.y + rotFloatXYZ.y;
            floatLLZ.z = floatXYZ.z;

         //   Console.WriteLine("Float UTM x, y = " + floatXYZ.x.ToString("f3") + "  " + floatXYZ.y.ToString("f3"));
           

            UTMUPS.DecodeZone(zonestr, out zone, out northp);
            UTMUPS.Reverse(zone, northp, floatXYZ.x, floatXYZ.y, out floatLLZ.lat, out floatLLZ.lon, true);
            Console.WriteLine("UTM to Geo = " + String.Format("{0} {1}", floatLLZ.lat, floatLLZ.lon));

            return (floatLLZ);
        }

        private static StringBuilder sbTemp = new StringBuilder(128);
        private static Queue<LLZPoint> floatTrackQ = new Queue<LLZPoint>();
        private static int floatTrackTrailLength = 128;

        private void writeKML(LLZPoint inFloatLLZ)
        {
            StreamWriter kmlFile = new StreamWriter("usblTrack.kml");
   
            kmlFile.WriteLine(@"<?xml version=""1.0"" encoding=""UTF-8""?>");
            kmlFile.WriteLine(@"<kml xmlns=""http://www.opengis.net/kml/2.2"" ");
            kmlFile.WriteLine(@" xmlns:gx=""http://www.google.com/kml/ext/2.2"">");
            kmlFile.WriteLine("<Document>");
            kmlFile.WriteLine("   <Placemark>");
            kmlFile.WriteLine("      <gx:Track>");
            kmlFile.WriteLine("         <altitudeMode>absolute</altitudeMode>");

            floatTrackQ.Enqueue(inFloatLLZ);
            if (floatTrackQ.Count >= floatTrackTrailLength)
                floatTrackQ.Dequeue();

            foreach(LLZPoint value in floatTrackQ)
            {
                sbTemp.Clear();
                sbTemp.Append("         <gx:coord>");
                sbTemp.Append(value.lon.ToString("f6"));
                sbTemp.Append(" ");
                sbTemp.Append(value.lat.ToString("f6"));
                sbTemp.Append(" ");
                sbTemp.Append(value.z.ToString("f6"));
                sbTemp.Append("</gx:coord>");
                kmlFile.WriteLine(sbTemp.ToString());
            }
            //for (int i = 1; i > -100; i--)
            //{
            //    sbTemp.Clear();
            //    sbTemp.Append("         <gx:coord>-121.80 36.805 ");
            //    sbTemp.Append(i.ToString());
            //    sbTemp.Append("</gx:coord>");
            //    kmlFile.WriteLine(sbTemp.ToString());
            //}

            kmlFile.WriteLine("      </gx:Track>");
            kmlFile.WriteLine("  </Placemark>");
            kmlFile.WriteLine("</Document>");
            kmlFile.WriteLine("</kml>");

            kmlFile.Close();
        }    
//   kmlFile.WriteLine(@"  <Placemark>\n');
//   kmlFile.WriteLine(@"     <name>Paragon</name>\n');
//   kmlFile.WriteLine(@"     <Model id="Paragon">\n');
//   kmlFile.WriteLine(@"        <Location id="Paragon Model Location">\n');
//   kmlFile.WriteLine(@"           <longitude>%3.9f</longitude>\n', lon(end));
//   kmlFile.WriteLine(@"           <latitude>%3.9f</latitude>\n', lat(end));
//   kmlFile.WriteLine(@"           <altitude>0</altitude>\n');
//   kmlFile.WriteLine(@"        </Location>\n');
//   kmlFile.WriteLine(@"        <Orientation>\n');
//   kmlFile.WriteLine(@"           <heading>%f</heading>\n', mod(heading + 180, 360));
//   kmlFile.WriteLine(@"           <tilt>0</tilt>\n');
//   kmlFile.WriteLine(@"           <roll>0</roll>\n');
//   kmlFile.WriteLine(@"        </Orientation>\n');
//   kmlFile.WriteLine(@"        <Scale>\n');
//   kmlFile.WriteLine(@"           <x>1</x>\n');
//   kmlFile.WriteLine(@"           <y>1</y>\n');
//   kmlFile.WriteLine(@"           <z>1</z>\n');
//   kmlFile.WriteLine(@"        </Scale>\n');
//   kmlFile.WriteLine(@"        <Link>\n');
//   kmlFile.WriteLine(@"           <href>Paragon.dae</href>\n');
//   kmlFile.WriteLine(@"        </Link>\n');
//   kmlFile.WriteLine(@"     </Model>\n');
//   kmlFile.WriteLine(@"  </Placemark>\n\n');
//
//   kmlFile.WriteLine(@"       <LookAt>\n');
//   kmlFile.WriteLine(@"          <longitude>%3.9f</longitude>\n', lon(end));
//   kmlFile.WriteLine(@"          <latitude>%3.9f</latitude>\n', lat(end));
//   kmlFile.WriteLine(@"          <altitude>%f</altitude>\n', GEZoom);
// % kmlFile.WriteLine(@"          <tilt>0</tilt>\n');
// % kmlFile.WriteLine(@"          <heading>0</heading>\n');
//   kmlFile.WriteLine(@"          <altitudeMode>relativeToGround</altitudeMode>\n');
//   kmlFile.WriteLine(@"       </LookAt>\n\n');
//
//   kmlFile.WriteLine(@"</Document>\n');
//   kmlFile.WriteLine(@"</kml>\n');
//    
//    fclose(fid);
//end
     

        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (iusblSerialPort.IsOpen)
                iusblSerialPort.Close();
        }

        private static String outString = "POS V6,4 R5000.00,X5000.00,Y0.00,Z";
        private static double depth = 0.0;
        private static double angle = 0.0;
        private static double radius = 2000;
        private const double angleToRad = Math.PI / 180.0;
        private void timerIUSBL_Tick(object sender, EventArgs e)
        {
            double x, y;

            angle = angle + 6.0;
            x = radius * Math.Sin(angle * angleToRad);
            y = radius * Math.Cos(angle * angleToRad);
            depth = depth - 1.0 + randomValue.NextDouble() - 0.5;

            sbTemp.Clear();
            sbTemp.Append("POS V6,4 R");
            sbTemp.Append(radius.ToString("f2"));
            sbTemp.Append(",X");
            sbTemp.Append(x.ToString("f2"));
            sbTemp.Append(",Y");
            sbTemp.Append(y.ToString("f2"));
            sbTemp.Append(",Z");
            sbTemp.Append(depth.ToString("f2"));
            sbTemp.Append("\r\n");
            Console.WriteLine("Sending: " + sbTemp.ToString());
            iusblSerialPort.Write(sbTemp.ToString());
        }

        private static double[][] rotationMatrix = new double[][]
        { 
            new double[]{0, 0},
            new double[]{0, 0}
        };

        public struct XYZPoint
        {
            public double x;
            public double y;
            public double z;
        }

        public struct LLZPoint
        {
            public double lat;
            public double lon;
            public double z;
        }

        private static double angleRad = double.NaN;
        private const double deg2rad = Math.PI / 180.0;
        private static XYZPoint outPoint;

        private static XYZPoint rotateXY(XYZPoint inPoint, double angleDeg)
        {
            angleRad = angleDeg * deg2rad;
            rotationMatrix[0][0] = Math.Cos(angleRad);
            rotationMatrix[0][1] = Math.Sin(-1.0 * angleRad);
            rotationMatrix[1][0] = Math.Sin(angleRad);
            rotationMatrix[1][1] = Math.Cos(angleRad);

            outPoint.x = (rotationMatrix[0][0] * inPoint.x) + (rotationMatrix[0][1] * inPoint.y);
            outPoint.y = (rotationMatrix[1][0] * inPoint.x) + (rotationMatrix[1][1] * inPoint.y);

            Console.WriteLine("New x = " + outPoint.x.ToString("f3") + "   New Y = " + outPoint.y.ToString("f3"));

            return (outPoint);
        }
    }
}