using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Collections;
using System.Threading;
using System.Text;
using System.Text.RegularExpressions;
using System.Windows.Forms;
using System.IO.Ports;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Input;
using Input=Microsoft.Xna.Framework.Input; // to provide shorthand to clear up ambiguities

namespace Coachoid
{

    public partial class MainForm : Form
    {

        //To keep track of the current and previous state of the gamepad
        /// <summary>
        /// The current state of the controller
        /// </summary>
        GamePadState gamePadState;
        /// <summary>
        /// The previous state of the controller
        /// </summary>
        GamePadState previousState;

        /// <summary>
        /// Keeps track of the current controller
        /// </summary>
        PlayerIndex playerIndex = PlayerIndex.One;

        /// <summary>
        /// Counter for limiting the time for which the vibration motors are on.
        /// </summary>
        int vibrationCountdown = 0;

        /// <summary>
        /// Contains the current status of the Swath Boat
        /// </summary>
        Hashtable status = new Hashtable();

        /// <summary>
        /// Contains parameters to send to the Swath Boat
        /// </summary>
        Hashtable parameters = new Hashtable();

        /// <summary>
        /// Contains the controls that display the table of properties in the "status" window
        /// </summary>
        Hashtable statusControls = new Hashtable();
        /// <summary>
        /// Contains the dataset log of all incoming data and some properties sent
        /// </summary>
        DataSet LogDS = new DataSet();
        /// <summary>
        /// Contains the datatable of all incoming data and some properties sent
        /// </summary>
        DataTable logTable = new DataTable();
        /// <summary>
        /// logInitTime - initial time of the log started used for timestamps of seconds into recording
        /// </summary>
        DateTime logInitTime;
        /// <summary>
        /// SerialOccupied means we are already recieving string
        /// </summary>
        bool serialOccupied = false;
        /// <summary>
        /// controlsChanged means a gain or desired value was changed, it is sent twice to ensure reception
        /// </summary>
        int controlsChanged = 0;
        /// <summary>
        /// waypoints contains arrays of all the lats and longitudes of the destinations
        /// </summary>
        public string[] waypointLats;
        public string[] waypointLons;
        /// <summary>
        /// Flag that is raised when waypoints are updated
        /// </summary>        
        int waypointsUpdated = 0;
        /// <summary>
        /// Waypoint index for when we're sending waypoints, we only send 3 at a time, this keeps track
        /// </summary>        
        Byte waypointsIndex = 1;

        public MainForm()
        {
            InitializeComponent();
        }

        /// <summary>
        /// When a new controller is selected from the drop down
        /// update the player index and turn off all the vibration motors. 
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void ddlController_SelectedIndexChanged(object sender, EventArgs e)
        {
            switch (this.ddlController.SelectedIndex)
            {
                case 0: playerIndex = PlayerIndex.One; break;
                case 1: playerIndex = PlayerIndex.Two; break;
                case 2: playerIndex = PlayerIndex.Three; break;
                case 3: playerIndex = PlayerIndex.Four; break;
                default: playerIndex = PlayerIndex.One; break;
            }
            this.StopAllVibration();

        }

        private void StopAllVibration()
        {
            GamePad.SetVibration(PlayerIndex.One, 0.0f, 0.0f);
            GamePad.SetVibration(PlayerIndex.Two, 0.0f, 0.0f);
            GamePad.SetVibration(PlayerIndex.Three, 0.0f, 0.0f);
            GamePad.SetVibration(PlayerIndex.Four, 0.0f, 0.0f);

        }

        private void CheckVibrationTimeout()
        {
            if (vibrationCountdown > 0)
            {
                --vibrationCountdown;
                if (vibrationCountdown == 0.0f)
                {
                    GamePad.SetVibration(playerIndex, 0.0f, 0.0f);
                }
            }

        }

        private void UpdateControllerState()
        {
            //Get the new gamepad state and save the old state.
            this.previousState = this.gamePadState;
            this.gamePadState = GamePad.GetState(this.playerIndex);
            //If the controller is not connected, let the user know
            if (this.gamePadState.IsConnected)
            {
                ControllerStatus.Text = "Controller Connected";
                ControllerStatus.ForeColor = System.Drawing.Color.Green;
            }
            else
            {
                ControllerStatus.Text = "No Controller";
                ControllerStatus.ForeColor = System.Drawing.Color.Maroon;                
            }
            //I personally prefer to only update the buttons if their state has been changed. 
            if (!this.gamePadState.Buttons.Equals(this.previousState.Buttons))
            {
                this.buttonA.Checked = (this.gamePadState.Buttons.A == Input.ButtonState.Pressed);
                this.buttonB.Checked = (this.gamePadState.Buttons.B == Input.ButtonState.Pressed);
                this.buttonX.Checked = (this.gamePadState.Buttons.X == Input.ButtonState.Pressed);
                this.buttonY.Checked = (this.gamePadState.Buttons.Y == Input.ButtonState.Pressed);
                this.buttonLeftShoulder.Checked = (this.gamePadState.Buttons.LeftShoulder == Input.ButtonState.Pressed);
                this.buttonRightShoulder.Checked = (this.gamePadState.Buttons.RightShoulder == Input.ButtonState.Pressed);
                this.buttonStart.Checked = (this.gamePadState.Buttons.Start == Input.ButtonState.Pressed);
                this.buttonBack.Checked = (this.gamePadState.Buttons.Back == Input.ButtonState.Pressed);
                this.buttonLeftStick.Checked = (this.gamePadState.Buttons.LeftStick == Input.ButtonState.Pressed);
                this.buttonRightStick.Checked = (this.gamePadState.Buttons.RightStick == Input.ButtonState.Pressed);
            }
            //KILL USING BUTTON B
            if (this.buttonB.Checked)
            {
                //Set Mode Parameter to "K" denoting kill or do nothing
                this.parameters["MD"] = "K";
                //play with buttons
                ManEnable.Enabled = true;
                ManDisable.Enabled = false;
                HeadEnBtn.Enabled = false;
                HeadDisBtn.Enabled = false;
            }
            //ENTER MANUAL MODE USING BUTTON A
            if (this.buttonA.Checked)
            {
                //Set Mode Parameter to "M" denoting Manual..
                ManEnable.Enabled = false;
                ManDisable.Enabled = true;
                HeadGrpBox.Enabled = true;
                HeadEnBtn.Enabled = true;
                HeadDisBtn.Enabled = false;
                //Start Sending Manual Control Strings
                ManControlTimer.Start();
                this.parameters["MD"] = "M";
            }
            if (!this.gamePadState.DPad.Equals(this.previousState.DPad))
            {
                this.buttonUp.Checked = (this.gamePadState.DPad.Up == Input.ButtonState.Pressed);
                this.buttonDown.Checked = (this.gamePadState.DPad.Down == Input.ButtonState.Pressed);
                this.buttonLeft.Checked = (this.gamePadState.DPad.Left == Input.ButtonState.Pressed);
                this.buttonRight.Checked = (this.gamePadState.DPad.Right == Input.ButtonState.Pressed);
            }

            //Update the position of the thumb sticks
            //since the thumbsticks can return a number between -1.0 and +1.0 I had to shift (add 1.0)
            //and scale (mutiplication by 100/2, or 50) to get the numbers to be in the range of 0-100
            //for the progress bar
            this.x1Position.Value = (int)((this.gamePadState.ThumbSticks.Left.X + 1.0f) * 100.0f / 2.0f);
            this.y1Position.Value = (int)((this.gamePadState.ThumbSticks.Left.Y + 1.0f) * 100.0f / 2.0f);
            this.x2position.Value = (int)((this.gamePadState.ThumbSticks.Right.X + 1.0f) * 100.0f / 2.0f);
            this.y2position.Value = (int)((this.gamePadState.ThumbSticks.Right.Y + 1.0f) * 100.0f / 2.0f);

            //The triggers return a value between 0.0 and 1.0.  I only needed to scale these values for
            //the progress bar
            this.leftTriggerPosition.Value = (int)((this.gamePadState.Triggers.Left * 100));
            this.rightTriggerPosition.Value = (int)(this.gamePadState.Triggers.Right*100);
            //machine guns!!!
            m1Player = new System.Media.SoundPlayer();
            //M1
            if (this.gamePadState.Triggers.Right > .05)
            {
                //m1Player.SoundLocation = @"C:\Documents and Settings\Administrator\My Documents\Visual Studio 2005\Projects\Coachoid\mg.wav";
                //m1Player.Play();
                //vibrate!!
                GamePad.SetVibration(playerIndex, (float)this.gamePadState.Triggers.Right, (float)0.0);
            }
            else
            {
                m1Player.Stop();
                GamePad.SetVibration(playerIndex, (float)0.0, (float)0.0);
            }
        }

        //this is for playing sounds      
        private System.Media.SoundPlayer m1Player;

        //I'm updating the controller display on a timed interval. 
        private void controllerTimer_Tick(object sender, EventArgs e)
        {
            this.CheckVibrationTimeout();
            this.UpdateControllerState();
        }

        private void XnaInputForm_Load(object sender, EventArgs e)
        {
            this.ddlController.SelectedIndex = 0;
            //Start Timer that updates controller status
            this.controllerTimer.Start();
            //Refresh list of available ports
            refresh_ports();
            //Initialize Paramters Hashtable
            this.parameters.Add("MD", "K"); //mode initially in kill mode - no action
            this.parameters.Add("M1", "A00"); //manual mode motor command1
            this.parameters.Add("M2", "B00"); //maunual mode motor command2
        }
        private void refresh_ports()
        {
            //Load Serial Port List
            //get an array of ports
            string[] portnames = System.IO.Ports.SerialPort.GetPortNames();
            //clear current list of ports
            this.PortList.Items.Clear();
            //loop through array and enter available
            foreach (string port in portnames)
            {
                this.PortList.Items.Add(port);
            }
        }
        private void XnaInputForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            //stop controller if its vibrating
            this.StopAllVibration();
            //Stop Sending Manual Control Strings
            ManControlTimer.Stop();
            ManEnable.Enabled = true;
            ManDisable.Enabled = false;

            //IF Serial Port is Open, Close it!
            if (serialPort.IsOpen)
            {
                try
                {
                    serialPort.Close();
                }
                catch (Exception x)
                {
                    MessageBox.Show(x.ToString());
                }
            }
        }

        private void btnConnect_Click(object sender, EventArgs e)
        {
            try
            {   //set serial port name to selected
                serialPort.PortName = PortList.SelectedItem.ToString();
                //serial port properties
                serialPort.NewLine = "\r\n";
                //open serial port
                serialPort.Open();
                //enable disconnect / disable connect / disable portlist
                btnConnect.Enabled = false;
                btnDisconnect.Enabled = true;
                PortList.Enabled = false;
                PortRefBtn.Enabled = false;
                //set connected message
                SerialStat.Text = PortList.SelectedItem.ToString() + " Connected";
                SerialStat.ForeColor = System.Drawing.Color.Green;
            }
            catch(Exception x)
            {
                //there was an error opening serial port
                MessageBox.Show(x.ToString());
            }
        }

        private void VibButton_Click(object sender, EventArgs e)
        {
            GamePad.SetVibration(playerIndex, (float)this.leftMotor.Value, (float)this.rightMotor.Value);
            vibrationCountdown = 30;
        }

        private void btnDisconnect_Click(object sender, EventArgs e)
        {
            try
            {
                //Close serialPort
                serialPort.Close();
                //enable disconnect / disable connect / disable portlist
                btnConnect.Enabled = true;
                btnDisconnect.Enabled = false;
                PortList.Enabled = true;
                PortRefBtn.Enabled = true;
                //set connected message
                SerialStat.Text = PortList.SelectedItem.ToString() + " DisConnected";
                SerialStat.ForeColor = System.Drawing.Color.Maroon;
 
            }
            catch (Exception x)
            {
                //display error
                MessageBox.Show(x.ToString());
            }
        }
        //ControlsChanged is called when a gain or desired value is changed
        private void controlsChangedCall(object sender, EventArgs e)
        {
            this.controlsChanged = 1;
        }
        //This function responds with all needed parameters to boat after it requests them
        private void respond_to_request()
        {
            String output = "";
            //construct string based on mode
            switch (this.parameters["MD"].ToString())
            {
                //kill mode
                case "K":
                    output += "$MDK";
                    output += "$M1a00";
                    output += "$M2b00";
                    break;
                //manual mode
                case "M":
                    output += "$MDM";
                    output += "$M1" + this.parameters["M1"];
                    output += "$M2" + this.parameters["M2"];
                    break;
                //heading and Velocity mode
                case "H":
                    //output mode
                    output += "$MDH";
                    //only output values if something has changed
                    if (this.controlsChanged > 0)
                    {
                        //output power scaling
                        output += "$PS" + PowerScale.Value.ToString();
                        //HEADING CONTROL
                        //output desired heading
                        output += "$DH" + DesiredSlider.Value.ToString().PadLeft(3, '0');
                        //output proportional heading
                        output += "$HP" + headPval.Value.ToString().PadLeft(2, '0');
                        //output integral heading
                        output += "$HI" + headIval.Value.ToString().PadLeft(2, '0');
                        //output derivative heading
                        output += "$HD" + headDval.Value.ToString().PadLeft(2, '0');
                        //VELOCITY CONTROL
                        //output desired velocity
                        output += "$DV" + DesiredVel.Value.ToString();
                        //output proportional velocity
                        output += "$VP" + velPval.Value.ToString().PadLeft(2, '0');
                        //output integral velocity
                        output += "$VI" + velIval.Value.ToString().PadLeft(2, '0');
                        //output derivative velocity
                        output += "$VD" + velDval.Value.ToString().PadLeft(2, '0');

                        //we send this out twice to ensure its been recieved, if controlsChanged is greate than 2, set to 0
                        this.controlsChanged++;
                        if (this.controlsChanged > 2)
                        {
                            this.controlsChanged = 0;
                        }
                    }
                    break;
                //fully autonomous mode
                case "A":
                    //output mode
                    output += "$MDA";
                    //only output values if something has changed
                    if (this.controlsChanged > 0)
                    {
                        //output power scaling
                        output += "$PS" + PowerScale.Value.ToString();
                        //HEADING CONTROL
                        //output proportional heading
                        output += "$HP" + headPval.Value.ToString().PadLeft(2, '0');
                        //output integral heading
                        output += "$HI" + headIval.Value.ToString().PadLeft(2, '0');
                        //output derivative heading
                        output += "$HD" + headDval.Value.ToString().PadLeft(2, '0');
                        //VELOCITY CONTROL
                        //output desired velocity
                        output += "$DV" + DesiredVel.Value.ToString();
                        //output proportional velocity
                        output += "$VP" + velPval.Value.ToString().PadLeft(2, '0');
                        //output integral velocity
                        output += "$VI" + velIval.Value.ToString().PadLeft(2, '0');
                        //output derivative velocity
                        output += "$VD" + velDval.Value.ToString().PadLeft(2, '0');

                        //we send this out twice to ensure its been recieved, if controlsChanged is greate than 2, set to 0
                        this.controlsChanged++;
                        if (this.controlsChanged > 2)
                        {
                            this.controlsChanged = 0;
                        }
                    }
                    //WAYPOINTS
                    if (this.waypointsUpdated > 0)
                    {
                        //output clear waypoints instruction if we're restarting
                        if (this.waypointsIndex == 1)
                        {
                            output += "$CW";
                        }
                        //Lattitude
                        //$LT(1 byte index)123.567890
                        byte index = this.waypointsIndex;
                        foreach(String lat in waypointLats)
                        {
                            output += "$LT" + System.Convert.ToChar(index).ToString() + lat.ToString().PadRight(10, '0').Substring(0, 10);
                            index++;
                            if (index >= this.waypointsIndex + 3)
                            {
                                break;
                            }
                        }
                        //LONGITUDE
                        //$LN(1 byte index)123.567890
                        index = this.waypointsIndex;
                        char[] neg = { '-' }; // negative sign to trim
                        foreach (String lon in waypointLons)
                        {
                            output += "$LN" + System.Convert.ToChar(index).ToString() + lon.ToString().TrimStart(neg).PadRight(10,'0').Substring(0, 10);
                            index++;
                            if (index >= this.waypointsIndex + 3)
                            {
                                break;
                            }
                        }

                        //increment waypoint counter
                        this.waypointsIndex += 3;

                        if (System.Convert.ToInt32(this.waypointsIndex) >= waypointLons.GetLength(0))
                        {
                            this.waypointsIndex = 1;
                            this.waypointsUpdated = 0;
                        }
                    }
                    break;

            }
            /*
             * NOTE:
             * System.BitConverter.GetBytes(); returns array of bytes from a c# variable - look at overloads
             * System.BitConverter.ToSingle(); takes 4 bytes and retrun single precision float
             * Lattitude Waypoint format: "LT" + byte representing waypoint number + 4bytes representing IEEE float value
             * Longitude Waypoint format: "LN" + byte representing waypoint number + 4bytes representing IEEE float value
             * desired heading format: "DH"+4bytes representing IEEE float
             */
            //output
            serialPort.WriteLine(output);
        }

        private void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            //if occupied we are already recieving the string
            if (serialOccupied)
            {
                return;
            }
            else
            {
                //we will recieve data
                serialOccupied = true;
            }

            String newData = "";
            //Read in new line
            try
            {
                newData = serialPort.ReadLine() + serialPort.NewLine;
            }
            catch (Exception x)
            {
                MessageBox.Show(x.ToString());
            }
            //we're done recieving string, serial port is unoccupied
            serialOccupied = false;
            //check if data is request
            if (newData == "$RQ\r\n")
            {
                //request for data, send parameters
                this.respond_to_request();
                return;
            }
            CommBox.Invoke(new addToCommBoxDelegate(this.addToCommBox), new object[] { newData.ToString() });
            //check  for correctness and parse if its properly formatted
            //regular expressions
            //^ denotes beginning of string
            // $$ = literal $
            // $ denotes end of string
            if (Regex.IsMatch(newData, "^[$].*:.*(\r\n)$", RegexOptions.Singleline))
            {
                    //This is a report to base of data, parse it!
                    this.Invoke(new parseDataDelegate(parseData), new Object[] { newData.ToString() });
                    //update the map using delegate
                    this.Invoke(new update_mapDelegate(update_map));
                    //update the avionics display using delegate
                    this.Invoke(new update_avionicsDelegate(update_avionics));
                    //add data to the log(
                    if (logDataStop.Enabled == true)
                    {
                        this.update_log();
                    }
            }
            else
            {
                //*data was bad, mention it?
            }
        }

        //delegate for parseData function
        delegate void parseDataDelegate(String buffer);
        //This function is called when a full line is recieved in the serial buffer
        //it parses the data and displays it on the forms
        private void parseData(String buffer)
        {
            //check for spaces, if so, we likely had an error, so ignore string
            if (buffer.Contains(" "))
            {
                return;
            }
            //remove line breaks 
            buffer = buffer.Trim();
            buffer = buffer.Replace("\r", "");
            buffer = buffer.Replace("\n", "");
            buffer = buffer.Replace(Environment.NewLine," ");
            //perform check to make sure string is valid
            if (!buffer.StartsWith('$'.ToString()) || !buffer.Contains(':'.ToString()))
            {
                return;
            }
            //remove first '$' to eliminate troubles
            buffer = buffer.Remove(0, 1);
            //parse data into hasharray
            //first clear hastable as its going to be rewritten
            this.status.Clear();
            //split string into key / value pairs
            string[] parameters = buffer.Split('$');
            //make sure we split the string
            if (parameters.Length == 0)
            {
                //note bad data recieved
                CommBox.AppendText("bad data");
                return;
            }
            //split into key and values and put into hashtable
            foreach(String param in parameters)
            {
                string[] keyvals = param.Split(':');
               /* //DEBUGGING
                CommBox.AppendText("\r\nDEBUG:");
                foreach (String test in keyvals)
                {
                    CommBox.AppendText(test + '#');
                }
               *///END DEBUGGING
                if (keyvals.Length <= 1)
                {
                    //note bad data recieved
                    CommBox.AppendText("bad data1");
                    continue;
                }
                //check if key already exists, if so, update, if not, add
                if (this.status.ContainsKey(keyvals[0]))
                {
                    this.status[keyvals[0]] = keyvals[1];
                }
                else
                {
                    this.status.Add(keyvals[0], keyvals[1]);
                }
            }
/*            //list all parameters in statusgrid
            this.statusGrid.ColumnCount = this.status.Count;
            //clear statusgrid
            this.statusGrid.Controls.Clear();
            //add new status info the the statusgrid
            int i = 0;
            foreach (string key in this.status.Keys)
            {   
                //check if control already exists and modify, otherwise create new
                if (this.statusControls.ContainsKey(key))
                {
                    Label olabel = (Label)Controls.Find(key, true)[0];
                    olabel.Text = key + ": " + this.status[key].ToString();
                    this.statusGrid.Refresh();
                    //this.statusControls[key] = olabel;
                }
                else
                {
                    //create new labels
                    Label nlabel = new Label();
                    nlabel.Name = key;
                    nlabel.Text = key + ": " + this.status[key].ToString();
                    this.statusGrid.Controls.Add(nlabel, 0, i);
                    //this.statusControls.Add(key, nlabel);
                }
                i++;
            }
*/
        }

        public delegate void update_mapDelegate();
        /**
         * This function updates the google map!
        **/
        public void update_map()
        {
            //check if we got longitude / latitiude position
            if(this.status.ContainsKey("LAT") && this.status.ContainsKey("LON"))
            {
                //Update position of Boat
            //This line runs the javascript function to move the boat
                Map.Document.InvokeScript("moveBoat", new object[] { Convert.ToDouble(this.status["LAT"]), -Convert.ToDouble(this.status["LON"]) });
            }
        }

        /*
         * Updates the status controls that display information
         */
        public delegate void update_avionicsDelegate();

        public void update_avionics()
        {
            //update Compass Heading
            if(this.status.ContainsKey("HDG"))
            {
                //update compass
                this.heading.Value = (int)(System.Convert.ToDouble(this.status["HDG"])/3.6);
                //update oscilloscop
                headingScope.MoveRays( DesiredSlider.Value , (int)System.Convert.ToDouble(this.status["HDG"]));

            }
            //update amps
            if (this.status.ContainsKey("A1"))
            {
                this.starAmps.Value = (int) System.Convert.ToDouble(this.status["A1"]);
            }
            if (this.status.ContainsKey("A2"))
            {
                this.portAmps.Value = (int)System.Convert.ToDouble(this.status["A2"]);
            }
            //update SOG
            if (this.status.ContainsKey("SOG"))
            {
                //update gauge - needs to be scaled by 10
                this.velocity.Value = (int)(System.Convert.ToDouble(this.status["SOG"].ToString()) * 10);
                //update oscilloscope - also scaled by 10
                velocityScope.MoveRays((int)System.Convert.ToInt32(DesiredVel.Value * 10), System.Convert.ToInt32(System.Convert.ToDouble(this.status["SOG"].ToString()) * 10));
            }
            //update compass desired heading if in autonomous mode
            if (this.status.ContainsKey("DH") && this.parameters.ContainsKey("MD") && this.parameters["MD"].ToString() == "A")
            {
                int desHead = System.Convert.ToInt32(System.Convert.ToDouble(this.status["DH"].ToString()));
                DesiredSlider.Value = desHead;
                DesiredLabel.Text = desHead.ToString();
                DesiredCompass.Value = desHead;
            }
        }

        /*
        * Creates the log table
        */
        public void create_log()
        {
            //clear log
            this.logTable = new DataTable();
            //name log
            logTable.TableName = "SWATH_Log";
            //add columns
            // this will be the primary key
            DataColumn timeCol = new DataColumn("Time", typeof(double));
            logTable.Columns.Add(timeCol);
            // make this the primary key
            logTable.PrimaryKey = new DataColumn[] { timeCol };
            // add some fields for all the data
            logTable.Columns.Add(new DataColumn("TimeStamp", typeof(string)));
            logTable.Columns.Add(new DataColumn("Latitude", typeof(string)));
            logTable.Columns.Add(new DataColumn("Longitude", typeof(string)));
            logTable.Columns.Add(new DataColumn("Heading", typeof(string)));
            logTable.Columns.Add(new DataColumn("Desired_Heading", typeof(int)));
            logTable.Columns.Add(new DataColumn("headPgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("headIgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("headDgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("Velocity", typeof(string)));
            logTable.Columns.Add(new DataColumn("Desired_Velocity", typeof(string)));
            logTable.Columns.Add(new DataColumn("velPgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("velIgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("velDgain", typeof(int)));
            logTable.Columns.Add(new DataColumn("M1_Current", typeof(string)));
            logTable.Columns.Add(new DataColumn("M2_Current", typeof(string)));

        }

        /*
         * Adds new data to the log
         */
        public void update_log()
        {
            // add record to the table
            DataRow row;
            row = logTable.NewRow();

            //add timestamp
            TimeSpan duration = DateTime.Now - logInitTime;
            row["Time"] = duration.TotalSeconds;

            //parameters recieved from swath
            if (this.status.ContainsKey("TME"))
                row["TimeStamp"] = this.status["TME"].ToString();
            if (this.status.ContainsKey("LAT"))
                row["Latitude"] = this.status["LAT"].ToString();
            if (this.status.ContainsKey("LON"))
                row["Longitude"] = this.status["LON"].ToString();
            if (this.status.ContainsKey("HDG"))
                row["Heading"] = this.status["HDG"].ToString();
            if (this.status.ContainsKey("A1"))
                row["M1_Current"] = this.status["A1"].ToString();
            if (this.status.ContainsKey("A2"))
                row["M2_Current"] = this.status["A2"].ToString();
            if (this.status.ContainsKey("SOG"))
                row["Velocity"] = this.status["SOG"].ToString();
            //no conditions are required for control values
            row["Desired_Heading"] = DesiredSlider.Value;
            row["headPgain"] = headPval.Value;
            row["headIgain"] = headIval.Value;
            row["headDgain"] = headDval.Value;
            row["Desired_Velocity"] = DesiredVel.Value.ToString();
            row["velPgain"] = velPval.Value;
            row["velIgain"] = velIval.Value;
            row["velDgain"] = velDval.Value;
            logTable.Rows.Add(row);
        }


        /*
         * This function is a delegate that adds string to the CommBox
         */
        public delegate void addToCommBoxDelegate(string text);

        public void addToCommBox(string text)
        {
            CommBox.AppendText(text);
        }

        private void SendBtn_Click(object sender, EventArgs e)
        {
            if (serialPort.IsOpen)
            {
                serialPort.WriteLine(txtToSend.Text.ToString());
                CommBox.AppendText(Environment.NewLine + txtToSend.Text.ToString());
                txtToSend.Clear();
            }
        }
        //Enables Manual Control
        private void ManEnable_Click(object sender, EventArgs e)
        {
            ManEnable.Enabled = false;
            ManDisable.Enabled = true;
            HeadGrpBox.Enabled = true;
            //Start Sending Manual Control Strings
            ManControlTimer.Start();
            //Set Mode Parameter to "M" denoting Manual
            this.parameters["MD"] = "M";
        }

        private void ManDisable_Click(object sender, EventArgs e)
        {
            //Stop Sending Manual Control Strings
            ManControlTimer.Stop();
            ManEnable.Enabled = true;
            ManDisable.Enabled = false;
            HeadGrpBox.Enabled = false;
            //Set Mode Parameter to "K" denoting kill or do nothing
            this.parameters["MD"] = "K";
        }

        private void ManControlTimer_Tick(object sender, EventArgs e)
        {
            this.ManualControl();
        }

        private void ManualControl()
        {
            //ManualTest.Text = this.gamePadState.ThumbSticks.Left.Y.ToString("X");
            //Get Joystick Value -1.0 to 1.0 & Multiply by 127 & Scale by powerscale
            float y1 = (this.gamePadState.ThumbSticks.Left.Y * 127)*((float)PowerScale.Value / 100);
            float y2 = (this.gamePadState.ThumbSticks.Left.X * 127)*((float)PowerScale.Value / 100);
            //round to nearest whole number
            int y1rnd = Math.Abs( (int) Math.Round(y1));
            int y2rnd = Math.Abs((int)Math.Round(y2));
            //Convert to string
            string y1str = y1rnd.ToString("X");
            string y2str = y2rnd.ToString("X");
            //String must be 2 characters in length
            if (y1str.Length == 1)
            {
                y1str = "0" + y1str; 
            }
            if (y2str.Length == 1)
            {
                y2str = "0" + y2str;
            }
            //B is forward / reverse
            if (y1 > -3)
            {
      
                y1str = "b" + y1str;
                //A is left / right turn
                if (y2 > 0)
                {
                    y2str = "a" + y2str;
                }
                else
                {
                    y2str = "A" + y2str;
                }
            }
            else
            {
                y1str = "B" + y1str;
                //A is left / right turn
                if (y2 > 0)
                {
                    y2str = "A" + y2str;
                }
                else
                {
                    y2str = "a" + y2str;
                }
            }

            //put out test box!
            ManualTest.Text = y2str;
            //assign values to paramters of boat
            this.parameters["M1"] = y1str;
            this.parameters["M2"] = y2str;

            //OUTPUT SERIAL STRING - not used anymore
            if (serialPort.IsOpen)
            {
                //serialPort.WriteLine(y1str);
                //serialPort.WriteLine(y2str);
            }
            else
            {
                this.ManControlTimer.Stop();
                ManEnable.Enabled = true;
                ManDisable.Enabled = false;
                MessageBox.Show("No Connection To Serial Port");
            }


        }

        private void tabPage1_Click(object sender, EventArgs e)
        {

        }

        private void Map_DocumentCompleted(object sender, WebBrowserDocumentCompletedEventArgs e)
        {

        }

        private void add_point_Click(object sender, EventArgs e)
        {
            //This line runs the javascript function "addMarker()" with parameters in object
            Map.Document.InvokeScript("addMarker", new object[] {pointDescription.Text.ToString()});
            pointDescription.Clear();

        }

        private void groupBox6_Enter(object sender, EventArgs e)
        {

        }

        private void PortRefBtn_Click(object sender, EventArgs e)
        {
            //refresh list of ports
            refresh_ports();
        }

        private void ClearMonitor_Click(object sender, EventArgs e)
        {
            CommBox.Clear();
        }

        private void txtToSend_Enter(object sender, EventArgs e)
        {

        }

        private void MainForm_FormClosed(object sender, FormClosedEventArgs e)
        {
            //IF Serial Port is Open, Close it!
            if (serialPort.IsOpen)
            {
                try
                {
                    serialPort.Close();
                }
                catch (Exception x)
                {
                    MessageBox.Show(x.ToString());
                }
            }
        }

        private void tabPage2_Click(object sender, EventArgs e)
        {

        }

        private void DesiredSlider_ValueChanged(object sender, EventArgs e)
        {
            DesiredCompass.Value = DesiredSlider.Value;
            DesiredLabel.Text = DesiredSlider.Value.ToString();
            this.controlsChanged = 1;
        }

        private void HeadEnBtn_Click(object sender, EventArgs e)
        {
            HeadEnBtn.Enabled = false;
            HeadDisBtn.Enabled = true;
            //Set Mode Parameter to "H" denoting Heading & Velocity Control
            this.parameters["MD"] = "H";
        }

        private void HeadDisBtn_Click(object sender, EventArgs e)
        {
            HeadEnBtn.Enabled = true;
            HeadDisBtn.Enabled = false;
            //Set Mode Parameter to "M" denoting Manual Control
            this.parameters["MD"] = "M";
        }

        private void NumericUpDown2_ValueChanged(object sender, EventArgs e)
        {

        }

        private void logDataStop_Click(object sender, EventArgs e)
        {
            //create new dataset
            DataSet ds = new DataSet();
            //name dataset
            ds.DataSetName = "SWATH";
            // add the table to the DataSet
            ds.Tables.Add(this.logTable);
            if (saveFileDialog.ShowDialog() == DialogResult.OK)
            {
                // dump to XML
                ds.WriteXml(saveFileDialog.FileName);
            }
            logDataStop.Enabled = false;
            logDataStart.Enabled = true;
        }

        private void logDataStart_Click(object sender, EventArgs e)
        {
            logDataStart.Enabled = false;
            logDataStop.Enabled = true;
            //reset timestamp
            this.logInitTime = DateTime.Now;
            //create log
            create_log();
        }
        //this function runs the javasctipt function clear_traces and clears the history trace of the boat
        private void clear_trace_Click(object sender, EventArgs e)
        {
            Map.Document.InvokeScript("clear_traces", new object[] {});
        }

        private void update_waypoints_Click(object sender, EventArgs e)
        {
           Object lats = new Object();
           Object lons = new Object();
           lats = Map.Document.InvokeScript("query_lats", new object[] { });
           lons = Map.Document.InvokeScript("query_lons", new object[] {});
           //careful about longitude sign's!
           //explode into arrays
           waypointLats = lats.ToString().Split(',');
           waypointLons = lons.ToString().Split(',');
           //set waypoints updated flag
           waypointsUpdated = 1;
       }

        private void autoEnable_Click(object sender, EventArgs e)
        {
            HeadEnBtn.Enabled = false;
            HeadDisBtn.Enabled = false;
            autoDisable.Enabled = true;
            autoEnable.Enabled = false;
            //Set Mode Parameter to "A" denoting Autonomous Control
            this.parameters["MD"] = "A";
        }

        private void autoDisable_Click(object sender, EventArgs e)
        {
            HeadEnBtn.Enabled = true;
            HeadDisBtn.Enabled = false;
            autoEnable.Enabled = true;
            autoDisable.Enabled = false;
            //Set Mode Parameter to "M" denoting Manual Control
            this.parameters["MD"] = "M";
        }






    }
}