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)
        {
            labelIUSBLRange.ForeColor = Color.Black;

            //From https://code.msdn.microsoft.com/serialport-brief-example-ac0d5004
            //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);

            inString += Encoding.ASCII.GetString(iusblMsgBuffer, 0, bytesRead);

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

        private static LLZPoint iusblLLZ = new LLZPoint();
        private static string[] inFields;
        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]);
            tbIUSBLRange.Text = 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 = inFields[8];
            }
            else
            {
                iusblData.xValue = double.NaN;
                tbIUSBLEasting.Text = "-";
                iusblData.yValue = double.NaN;
                tbIUSBLNorthing.Text = "-";
                iusblData.zValue = double.NaN;
                tbIUSBLDepth.Text = "-";
            }

            double boatHeading = -45.0;
            iusblLLZ = iusblXYZToLatLon(iusblData.xValue, iusblData.yValue, iusblData.zValue, boatHeading);
            writeKML(iusblLLZ);

            UpdateStatus(iusblData);
        }

        private void simIUSBL(StringBuilder inSB)
        {
            Console.WriteLine(inSB.ToString());

            depthValue = depthValue - randomValue.NextDouble();

            iusblData.xValue = 0.0;
            iusblData.yValue = 0.0;
            iusblData.zValue = depthValue;
            iusblData.timeStamp = DateTime.Now;

            //ProgressEventArgs args = new ProgressEventArgs("test status");

            tbIUSBLDepth.Text = iusblData.zValue.ToString("f1");
            tbIUSBLNorthing.Text = iusblData.yValue.ToString("f1");
            tbIUSBLEasting.Text = iusblData.xValue.ToString("f1");

            UpdateStatus(iusblData);

           // writeKML(iusblData);
        }

        private const int cpfQueueDepth = 256;
        private Queue<LLZPoint> cpfPositionQueue = new Queue<LLZPoint>(cpfQueueDepth);
        private static StreamWriter kmlFile;
        private static StringBuilder sbTemp = new StringBuilder(128);
        private void writeKML(LLZPoint inLLZ)
        {
            kmlFile = new StreamWriter("usblTrack.kml");

            if (cpfPositionQueue.Count > cpfQueueDepth)
                cpfPositionQueue.Dequeue();
            cpfPositionQueue.Enqueue(inLLZ);

            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>");

            foreach(LLZPoint value in cpfPositionQueue)
            {
                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());
            }

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

            kmlFile.Close();
         }    

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

        private static String outString = "POS V6,4\r\n";
        private static double depth = 0.0;
        private static double angle = 0.0;
        private static double x, y;
        private void timerIUSBL_Tick(object sender, EventArgs e)
        {
            depth = depth - 1.0;
            x = 1000.0 * Math.Sin(angle * deg2rad);
            y = 1000.0 * Math.Cos(angle * deg2rad);
            angle = angle + 5.0;

            sbTemp.Clear();
            sbTemp.Append("POS V6,4 R1000.0,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: " + outString);
            //iusblSerialPort.Write(outString);
            Console.WriteLine("Sending: " + sbTemp.ToString());
            iusblSerialPort.Write(sbTemp.ToString());
            labelIUSBLRange.ForeColor = Color.Red;
        }

        private static LLZPoint iusblXYZToLatLon(double xIn, double yIn, double zIn, double angleIn)
        {
            XYZPoint shipXYZ;
            XYZPoint iusblXYZ;
            XYZPoint rotIUSBLXYZ;
            LLZPoint iusblLLZ;

            double shipLat = 36.588137, shipLon = -122.812716;

            int zone;
            bool northp;
            UTMUPS.Forward(shipLat, shipLon, out zone, out northp, out shipXYZ.x, out shipXYZ.y, -1, true);
            string zonestr = UTMUPS.EncodeZone(zone, northp, true);

            iusblXYZ.x = xIn;
            iusblXYZ.y = yIn;
            iusblXYZ.z = zIn;
            rotIUSBLXYZ = rotateXY(iusblXYZ, angleIn);

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

            iusblXYZ.x = shipXYZ.x + rotIUSBLXYZ.x;
            iusblXYZ.y = shipXYZ.y + rotIUSBLXYZ.y;
            iusblLLZ.z = iusblXYZ.z;

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

            return (iusblLLZ);
        }

        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);
        }
    }
}

//   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