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

using ZedGraph;

namespace AHRSInterface
{
    public partial class InfoDump : Form
    {
        delegate void displayAccelAlignmentCallback();
        delegate void displayGyroAlignmentCallback();
        delegate void displayMagCalCallback();
        delegate void displayGyroScaleCallback();
        delegate void displayActiveChannelsCallback();
        delegate void displayGyroBiasCallback();
        delegate void displayStartCalCallback();
        delegate void displayAccelBiasCallback();
        delegate void displayMagCovarianceCallback();
        delegate void displayAccelCovarianceCallback();
        delegate void displayProcessCovarianceCallback();
        delegate void displayEKFConfigCallback();
        delegate void displayBroadcastModeCallback();
        delegate void displayMagBiasCallback();
        delegate void displayMagRefVectorCallback();
        delegate void displayAccelRefVectorCallback();
        delegate void displayStateCovarianceCallback();
        delegate void AppendTextCallback(string text, Color text_color);

        public InfoDump( AHRS sensor )
        {
            InitializeComponent();

            this.sensor = sensor;

            // Add new event handlers
            sensor.PacketReceivedEvent += new PacketDelegate(PacketReceivedEventHandler);
            sensor.PacketTimeoutEvent += new StateDelegate(TimeoutEventHandler);
            sensor.DataReceivedEvent += new DataReceivedDelegate(DataReceivedEventHandler);
            sensor.PacketSentEvent += new PacketDelegate(PacketSentEventHandler);

            displayAHRSStates();

            initializeGraphs();

            timer1.Interval = 50;
            timer1.Start();            
            time = 0;
        }

        AHRS sensor;
        /*********************************************************************************
         * Variables for graph initialization
         * *******************************************************************************/
        GraphPane accelPane, gyroPane, magPane, anglePane, angularRatePane;
        RollingPointPairList xAccelList, yAccelList, zAccelList, xGyroList, yGyroList, zGyroList;
        RollingPointPairList xMagList, yMagList, zMagList, yawList, pitchList, rollList, yawRateList, pitchRateList, rollRateList;
        LineItem xAccelLine, yAccelLine, zAccelLine, xGyroLine, yGyroLine, zGyroLine;
        LineItem xMagLine, yMagLine, zMagLine, yawLine, pitchLine, rollLine, yawRateLine, pitchRateLine, rollRateLine;

        const int SENSOR_GRAPH_POINTS = 100;

        const int YAW_INDEX = 0;
        const int PITCH_INDEX = 1;
        const int ROLL_INDEX = 2;
        const int YAW_RATE_INDEX = 3;
        const int PITCH_RATE_INDEX = 4;
        const int ROLL_RATE_INDEX = 5;
        const int X_MAG_INDEX = 6;
        const int Y_MAG_INDEX = 7;
        const int Z_MAG_INDEX = 8;
        const int X_ACCEL_INDEX = 9;
        const int Y_ACCEL_INDEX = 10;
        const int Z_ACCEL_INDEX = 11;
        const int X_GYRO_INDEX = 12;
        const int Y_GYRO_INDEX = 13;
        const int Z_GYRO_INDEX = 14;

        double time;

        private void initializeGraphs()
        {
            accelPane = accelGraph.GraphPane;
            accelPane.Title.Text = "Accelerometers";
            accelPane.XAxis.Title.Text = "Time (s)";
            accelPane.YAxis.Title.Text = "Sensor Output (g)";
            //            accelPane.YAxis.Scale.Max = accelMax;
            //            accelPane.YAxis.Scale.Min = accelMin;

            gyroPane = gyroGraph.GraphPane;
            gyroPane.Title.Text = "Rate Gyros";
            gyroPane.XAxis.Title.Text = "Time (s)";
            gyroPane.YAxis.Title.Text = "Sensor Output (deg/s)";
            //            gyroPane.YAxis.Scale.Max = gyroMax;
            //            gyroPane.YAxis.Scale.Min = gyroMin;

            magPane = magGraph.GraphPane;
            magPane.Title.Text = "Magnetic Sensors";
            magPane.XAxis.Title.Text = "Time (s)";
            magPane.YAxis.Title.Text = "Sensor Output (raw)";
            //            magPane.YAxis.Scale.Max = magMax;
            //            magPane.YAxis.Scale.Min = magMin;

            anglePane = angleGraph.GraphPane;
            anglePane.Title.Text = "Estimated Angles";
            anglePane.XAxis.Title.Text = "Time (s)";
            anglePane.YAxis.Title.Text = "Angle (degrees)";

            angularRatePane = angularRateGraph.GraphPane;
            angularRatePane.Title.Text = "Angular Rates";
            angularRatePane.XAxis.Title.Text = "Time (s)";
            angularRatePane.YAxis.Title.Text = "Anglular Rate (deg/s)";


            xAccelList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            yAccelList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            zAccelList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            xGyroList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            yGyroList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            zGyroList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            xMagList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            yMagList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            zMagList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            yawList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            pitchList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            rollList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            yawRateList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            pitchRateList = new RollingPointPairList(SENSOR_GRAPH_POINTS);
            rollRateList = new RollingPointPairList(SENSOR_GRAPH_POINTS);

            xAccelLine = accelPane.AddCurve("X", xAccelList, Color.Blue, SymbolType.None);
            yAccelLine = accelPane.AddCurve("Y", yAccelList, Color.Red, SymbolType.None);
            zAccelLine = accelPane.AddCurve("Z", zAccelList, Color.Green, SymbolType.None);

            xGyroLine = gyroPane.AddCurve("X", xGyroList, Color.Blue, SymbolType.None);
            yGyroLine = gyroPane.AddCurve("Y", yGyroList, Color.Red, SymbolType.None);
            zGyroLine = gyroPane.AddCurve("Z", zGyroList, Color.Green, SymbolType.None);

            xMagLine = magPane.AddCurve("X", xMagList, Color.Blue, SymbolType.None);
            yMagLine = magPane.AddCurve("Y", yMagList, Color.Red, SymbolType.None);
            zMagLine = magPane.AddCurve("Z", zMagList, Color.Green, SymbolType.None);

            yawLine = anglePane.AddCurve("Yaw", yawList, Color.Blue, SymbolType.None);
            pitchLine = anglePane.AddCurve("Pitch", pitchList, Color.Red, SymbolType.None);
            rollLine = anglePane.AddCurve("Roll", rollList, Color.Green, SymbolType.None);

            yawRateLine = angularRatePane.AddCurve("Yaw Rate", yawRateList, Color.Blue, SymbolType.None);
            pitchRateLine = angularRatePane.AddCurve("Pitch Rate", pitchRateList, Color.Red, SymbolType.None);
            rollRateLine = angularRatePane.AddCurve("Roll Rate", rollRateList, Color.Green, SymbolType.None);

        }

        private void refreshGraphs()
        {
            
            angleGraph.AxisChange();
            angleGraph.Invalidate();

            angularRateGraph.AxisChange();
            angularRateGraph.Invalidate();
           
            gyroGraph.AxisChange();
            gyroGraph.Invalidate();

            accelGraph.AxisChange();
            accelGraph.Invalidate();

            magGraph.AxisChange();
            magGraph.Invalidate();
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            time += timer1.Interval / 1000.0;

            xAccelList.Add(time, sensor.accelX);
            yAccelList.Add(time, sensor.accelY);
            zAccelList.Add(time, sensor.accelZ);

            xGyroList.Add(time, sensor.gyroX);
            yGyroList.Add(time, sensor.gyroY);
            zGyroList.Add(time, sensor.gyroZ);

            xMagList.Add(time, sensor.magX);
            yMagList.Add(time, sensor.magY);
            zMagList.Add(time, sensor.magZ);

            yawList.Add(time, sensor.yawAngle);
            pitchList.Add(time, sensor.pitchAngle);
            rollList.Add(time, sensor.rollAngle);

            yawRateList.Add(time, sensor.yawRate);
            pitchRateList.Add(time, sensor.pitchRate);
            rollRateList.Add(time, sensor.rollRate);

            refreshGraphs();
        }

        /* **********************************************************************************
         * 
         * Function: void TimeoutEventHandler
         * Inputs: PName packet_type, int flags
         * Outputs: None
         * Return Value: None
         * Dependencies: None
         * Description: 
         * 
         * Handles timeout events generated by the AHRS class - a timeout event occurs if the
         * AHRS class attempts to communicate with the AHRS device and receives no response.
         * 
         * *********************************************************************************/
        void TimeoutEventHandler(StateName packet_type, int flags)
        {
            string message;

            message = "Timeout: ";
            message += System.Enum.Format(typeof(StateName), packet_type, "G");
            message += "\r\n";

            AppendStatusText(message, Color.Red);
        }

        /* **********************************************************************************
        * 
        * Function: void PacketSentEventHandler
        * Inputs: PName packet_type, int flags
        * Outputs: None
        * Return Value: None
        * Dependencies: None
        * Description: 
        * 
        * Handles PacketReceived events generated by the AHRS.
        * 
        * *********************************************************************************/
        void PacketSentEventHandler(PName packet_type, int flags)
        {
            string message;

            message = "Sent ";
            message += System.Enum.Format(typeof(PName), packet_type, "G");
            message += " packet\r\n";

            AppendStatusText(message, Color.Blue);
        }

        /* **********************************************************************************
        * 
        * Function: void DataReceivedEventHandler
        * Inputs: None
        * Outputs: None
        * Return Value: None
        * Dependencies: None
        * Description: 
        * 
        * Handles DataReceived events generated by the AHRS object
        * 
        * *********************************************************************************/
        void DataReceivedEventHandler(int active_channels)
        {
            //            AppendStatusText("Got Data\r\n", Color.Green);
        }

        void PacketReceivedEventHandler(PName packet_type, int flags)
        {
            string message;

            if (packet_type == PName.COMMAND_FAILED)
            {
                message = "Command Failed: ";
                message += System.Enum.Format(typeof(PName), (PName)flags, "G");
                message += "\r\n";
            }
            else if (packet_type == PName.COMMAND_COMPLETE)
            {
                message = "Command Complete: ";
                message += System.Enum.Format(typeof(PName), (PName)flags, "G");
                message += "\r\n";
            }
            else
            {
                message = "Received ";
                message += System.Enum.Format(typeof(PName), packet_type, "G");
                message += " packet\r\n";
            }

            AppendStatusText(message, Color.Green);


            switch (packet_type)
            {
                case PName.ACCEL_ALIGNMENT_REPORT:
                    displayAccelAlignment();
                    break;

                case PName.GYRO_ALIGNMENT_REPORT:
                    displayGyroAlignment();
                    break;

                case PName.MAG_CAL_REPORT:
                    displayMagCal();
                    break;

                case PName.GYRO_SCALE_REPORT:

                    displayGyroScale();

                    break;

                case PName.START_CAL_REPORT:
                    displayStartCal();

                    break;

                case PName.ACTIVE_CHANNEL_REPORT:
                    displayActiveChannels();

                    break;

                case PName.GYRO_BIAS_REPORT:
                    displayGyroBias();
                    break;

                case PName.MAG_BIAS_REPORT:
                    displayMagBias();
                    break;

                case PName.MAG_REF_VECTOR_REPORT:
                    displayMagRefVector();
                    break;

                case PName.ACCEL_REF_VECTOR_REPORT:
                    displayAccelRefVector();
                    break;

                case PName.ACCEL_BIAS_REPORT:
                    displayAccelBias();
                    break;

                case PName.ACCEL_COVARIANCE_REPORT:
                    displayAccelCovariance();
                    break;

                case PName.PROCESS_COVARIANCE_REPORT:
                    displayProcessCovariance();
                    break;

                case PName.MAG_COVARIANCE_REPORT:
                    displayMagCovariance();
                    break;

                case PName.EKF_CONFIG_REPORT:
                    displayEKFConfig();
                    break;

                case PName.BROADCAST_MODE_REPORT:
                    displayBroadcastMode();
                    break;

                case PName.STATE_COVARIANCE_REPORT:
                    displayStateCovariance();
                    break;
            }
        }

        

        private void displayAHRSStates()
        {
            displayBroadcastMode();
            displayEKFConfig();
            displayMagCovariance();
            displayProcessCovariance();
            displayAccelCovariance();
            displayAccelBias();
            displayGyroBias();
            displayActiveChannels();
            displayStartCal();
            displayGyroScale();
            displayMagCal();
            displayGyroAlignment();
            displayAccelAlignment();
            displayMagBias();
            displayMagRefVector();
            displayAccelRefVector();
        }
       
        private void displayStateCovariance()
        {

            if (this.stateCov00.InvokeRequired)
            {
                displayStateCovarianceCallback d = new displayStateCovarianceCallback(displayStateCovariance);
                this.Invoke(d, new object[] { });
            }
            else
            {
                float[,] state_covariance = this.sensor.state_covariance;

                stateCov00.Text = state_covariance[0, 0].ToString();
                stateCov01.Text = state_covariance[0, 1].ToString();
                stateCov02.Text = state_covariance[0, 2].ToString();

                stateCov10.Text = state_covariance[1, 0].ToString();
                stateCov11.Text = state_covariance[1, 1].ToString();
                stateCov12.Text = state_covariance[1, 2].ToString();

                stateCov20.Text = state_covariance[2, 0].ToString();
                stateCov21.Text = state_covariance[2, 1].ToString();
                stateCov22.Text = state_covariance[2, 2].ToString();
            }

        }

        private void displayAccelAlignment()
        {
            
            if (this.accelAlignment00.InvokeRequired)
            {
                displayAccelAlignmentCallback d = new displayAccelAlignmentCallback(displayAccelAlignment);
                this.Invoke(d, new object[] { });
            }
            else
            {
                float[,] accel_alignment = this.sensor.accel_alignment;

                accelAlignment00.Text = accel_alignment[0, 0].ToString();
                accelAlignment01.Text = accel_alignment[0, 1].ToString();
                accelAlignment02.Text = accel_alignment[0, 2].ToString();

                accelAlignment10.Text = accel_alignment[1, 0].ToString();
                accelAlignment11.Text = accel_alignment[1, 1].ToString();
                accelAlignment12.Text = accel_alignment[1, 2].ToString();

                accelAlignment20.Text = accel_alignment[2, 0].ToString();
                accelAlignment21.Text = accel_alignment[2, 1].ToString();
                accelAlignment22.Text = accel_alignment[2, 2].ToString();
            }
            
        }

        private void displayAccelBias()
        {
            if (this.accelXBias.InvokeRequired)
            {
                displayAccelBiasCallback d = new displayAccelBiasCallback(displayAccelBias);
                this.Invoke(d, new object[] { });
            }
            else
            {
                accelXBias.Text = sensor.x_accel_bias.ToString();
                accelYBias.Text = sensor.y_accel_bias.ToString();
                accelZBias.Text = sensor.z_accel_bias.ToString();
            }

        }

        private void displayBroadcastMode()
        {
            if (this.broadcastModeEnabled.InvokeRequired)
            {
                displayBroadcastModeCallback d = new displayBroadcastModeCallback(displayBroadcastMode);
                this.Invoke(d, new object[] { });
            }
            else
            {
                if (sensor.broadcast_enabled)
                    broadcastModeEnabled.Checked = true;
                else
                    listenModeEnabled.Checked = true;

                broadcastFrequencyBox.Value = (decimal)sensor.broadcast_rate;
            }

        }

        private void displayEKFConfig()
        {
            if (this.accelUpdateEnabled.InvokeRequired)
            {
                displayEKFConfigCallback d = new displayEKFConfigCallback(displayEKFConfig);
                this.Invoke(d, new object[] { });
            }
            else
            {
                accelUpdateEnabled.Checked = sensor.accel_enabled;
                magUpdateEnabled.Checked = sensor.mag_enabled;
            }

        }

        private void displayProcessCovariance()
        {
            if (this.processVariance.InvokeRequired)
            {
                displayProcessCovarianceCallback d = new displayProcessCovarianceCallback(displayProcessCovariance);
                this.Invoke(d, new object[] { });
            }
            else
            {
                processVariance.Text = sensor.process_covariance.ToString();
            }

        }

        private void displayMagCovariance()
        {
            if (this.magVariance.InvokeRequired)
            {
                displayMagCovarianceCallback d = new displayMagCovarianceCallback(displayMagCovariance);
                this.Invoke(d, new object[] { });
            }
            else
            {
                magVariance.Text = sensor.mag_covariance.ToString();
            }

        }

        private void displayAccelCovariance()
        {
            if (this.accelVariance.InvokeRequired)
            {
                displayAccelCovarianceCallback d = new displayAccelCovarianceCallback(displayAccelCovariance);
                this.Invoke(d, new object[] { });
            }
            else
            {
                accelVariance.Text = sensor.accel_covariance.ToString();
            }

        }


        private void displayGyroBias()
        {
            if (this.gyroXBias.InvokeRequired)
            {
                displayGyroBiasCallback d = new displayGyroBiasCallback(displayGyroBias);
                this.Invoke(d, new object[] { });
            }
            else
            {

                gyroXBias.Text = sensor.x_gyro_bias.ToString();
                gyroYBias.Text = sensor.y_gyro_bias.ToString();
                gyroZBias.Text = sensor.z_gyro_bias.ToString();
            }

        }

        private void displayMagBias()
        {
            if (this.magXBias.InvokeRequired)
            {
                displayMagBiasCallback d = new displayMagBiasCallback(displayMagBias);
                this.Invoke(d, new object[] { });
            }
            else
            {

                magXBias.Text = sensor.x_mag_bias.ToString();
                magYBias.Text = sensor.y_mag_bias.ToString();
                magZBias.Text = sensor.z_mag_bias.ToString();
            }

        }

        private void displayMagRefVector()
        {
            if (this.magXRef.InvokeRequired)
            {
                displayMagRefVectorCallback d = new displayMagRefVectorCallback(displayMagRefVector);
                this.Invoke(d, new object[] { });
            }
            else
            {

                magXRef.Text = sensor.x_mag_ref.ToString();
                magYRef.Text = sensor.y_mag_ref.ToString();
                magZRef.Text = sensor.z_mag_ref.ToString();
            }

        }

        private void displayAccelRefVector()
        {
            if (this.accelXRef.InvokeRequired)
            {
                displayAccelRefVectorCallback d = new displayAccelRefVectorCallback(displayAccelRefVector);
                this.Invoke(d, new object[] { });
            }
            else
            {

                accelXRef.Text = sensor.x_accel_ref.ToString();
                accelYRef.Text = sensor.y_accel_ref.ToString();
                accelZRef.Text = sensor.z_accel_ref.ToString();
            }

        }

        private void displayStartCal()
        {
            if (this.gyroStartupCalEnabled.InvokeRequired)
            {
                displayStartCalCallback d = new displayStartCalCallback(displayStartCal);
                this.Invoke(d, new object[] { });
            }
            else
            {
                if (sensor.zero_gyros_on_startup)
                    gyroStartupCalEnabled.Checked = true;
                else
                    gyroStartupCalEnabled.Checked = false;
            }

        }

        private void displayActiveChannels()
        {
            if (this.activeXAccel.InvokeRequired)
            {
                displayActiveChannelsCallback d = new displayActiveChannelsCallback(displayActiveChannels);
                this.Invoke(d, new object[] {  });
            }
            else
            {
                if (sensor.accelZ_active)
                    activeZAccel.Checked = true;
                else
                    activeZAccel.Checked = false;
                if (sensor.accelY_active)
                    activeYAccel.Checked = true;
                else
                    activeYAccel.Checked = false;
                if (sensor.accelX_active)
                    activeXAccel.Checked = true;
                else
                    activeXAccel.Checked = false;

                if (sensor.gyroZ_active)
                    activeZGyro.Checked = true;
                else
                    activeZGyro.Checked = false;
                if (sensor.gyroY_active)
                    activeYGyro.Checked = true;
                else
                    activeYGyro.Checked = false;
                if (sensor.gyroX_active)
                    activeXGyro.Checked = true;
                else
                    activeXGyro.Checked = false;

                if (sensor.magZ_active)
                    activeZMag.Checked = true;
                else
                    activeZMag.Checked = false;
                if (sensor.magY_active)
                    activeYMag.Checked = true;
                else
                    activeYMag.Checked = false;
                if (sensor.magX_active)
                    activeXMag.Checked = true;
                else
                    activeXMag.Checked = false;

                if (sensor.roll_rate_active)
                    activeRollRate.Checked = true;
                else
                    activeRollRate.Checked = false;
                if (sensor.pitch_rate_active)
                    activePitchRate.Checked = true;
                else
                    activePitchRate.Checked = false;
                if (sensor.yaw_rate_active)
                    activeYawRate.Checked = true;
                else
                    activeYawRate.Checked = false;

                if (sensor.roll_active)
                    activeRoll.Checked = true;
                else
                    activeRoll.Checked = false;
                if (sensor.pitch_active)
                    activePitch.Checked = true;
                else
                    activePitch.Checked = false;
                if (sensor.yaw_active)
                    activeYaw.Checked = true;
                else
                    activeYaw.Checked = false;

            }

        }

        private void displayGyroAlignment()
        {
            if (this.gyroAlignment00.InvokeRequired)
            {
                displayGyroAlignmentCallback d = new displayGyroAlignmentCallback(displayGyroAlignment);
                this.Invoke(d, new object[] { });
            }
            else
            {
                float[,] gyro_alignment = this.sensor.gyro_alignment;

                gyroAlignment00.Text = gyro_alignment[0, 0].ToString();
                gyroAlignment01.Text = gyro_alignment[0, 1].ToString();
                gyroAlignment02.Text = gyro_alignment[0, 2].ToString();

                gyroAlignment10.Text = gyro_alignment[1, 0].ToString();
                gyroAlignment11.Text = gyro_alignment[1, 1].ToString();
                gyroAlignment12.Text = gyro_alignment[1, 2].ToString();

                gyroAlignment20.Text = gyro_alignment[2, 0].ToString();
                gyroAlignment21.Text = gyro_alignment[2, 1].ToString();
                gyroAlignment22.Text = gyro_alignment[2, 2].ToString();
            }

        }

        private void displayMagCal()
        {
            if (this.magAlignment00.InvokeRequired)
            {
                displayMagCalCallback d = new displayMagCalCallback(displayMagCal);
                this.Invoke(d, new object[] { });
            }
            else
            {
                float[,] mag_cal = this.sensor.mag_cal;

                magAlignment00.Text = mag_cal[0, 0].ToString();
                magAlignment01.Text = mag_cal[0, 1].ToString();
                magAlignment02.Text = mag_cal[0, 2].ToString();

                magAlignment10.Text = mag_cal[1, 0].ToString();
                magAlignment11.Text = mag_cal[1, 1].ToString();
                magAlignment12.Text = mag_cal[1, 2].ToString();

                magAlignment20.Text = mag_cal[2, 0].ToString();
                magAlignment21.Text = mag_cal[2, 1].ToString();
                magAlignment22.Text = mag_cal[2, 2].ToString();
            }

        }

        private void displayGyroScale()
        {
            if (this.gyroXScale.InvokeRequired)
            {
                displayGyroScaleCallback d = new displayGyroScaleCallback(displayGyroScale);
                this.Invoke(d, new object[] { });
            }
            else
            {
                float x_scale = sensor.x_gyro_scale;
                float y_scale = sensor.y_gyro_scale;
                float z_scale = sensor.z_gyro_scale;

                gyroXScale.Text = x_scale.ToString();
                gyroYScale.Text = y_scale.ToString();
                gyroZScale.Text = z_scale.ToString();
            }

        }

        private void InfoDump_Load(object sender, EventArgs e)
        {

        }

        private void SynchButton_Click(object sender, EventArgs e)
        {
            sensor.synch();
        }

        private void gyroAlignmentCommitButton_Click(object sender, EventArgs e)
        {
            float[,] dat = new float[3, 3];


            dat[0, 0] = (float)Convert.ToDouble(gyroAlignment00.Text);
            dat[0, 1] = (float)Convert.ToDouble(gyroAlignment01.Text);
            dat[0, 2] = (float)Convert.ToDouble(gyroAlignment02.Text);

            dat[1, 0] = (float)Convert.ToDouble(gyroAlignment10.Text);
            dat[1, 1] = (float)Convert.ToDouble(gyroAlignment11.Text);
            dat[1, 2] = (float)Convert.ToDouble(gyroAlignment12.Text);

            dat[2, 0] = (float)Convert.ToDouble(gyroAlignment20.Text);
            dat[2, 1] = (float)Convert.ToDouble(gyroAlignment21.Text);
            dat[2, 2] = (float)Convert.ToDouble(gyroAlignment22.Text);

            sensor.gyro_alignment = dat;

            sensor.synch();

        }

        private void InvalidateAllButton_Click(object sender, EventArgs e)
        {
            sensor.Invalidate();
            sensor.synch();
        }

        private void accelAlignmentCommitButton_Click(object sender, EventArgs e)
        {
            float[,] dat = new float[3, 3];

            dat[0, 0] = (float)Convert.ToDouble(accelAlignment00.Text);
            dat[0, 1] = (float)Convert.ToDouble(accelAlignment01.Text);
            dat[0, 2] = (float)Convert.ToDouble(accelAlignment02.Text);

            dat[1, 0] = (float)Convert.ToDouble(accelAlignment10.Text);
            dat[1, 1] = (float)Convert.ToDouble(accelAlignment11.Text);
            dat[1, 2] = (float)Convert.ToDouble(accelAlignment12.Text);

            dat[2, 0] = (float)Convert.ToDouble(accelAlignment20.Text);
            dat[2, 1] = (float)Convert.ToDouble(accelAlignment21.Text);
            dat[2, 2] = (float)Convert.ToDouble(accelAlignment22.Text);

            sensor.accel_alignment = dat;

            sensor.synch();
        }

        private void magAlignmentCommitButton_Click(object sender, EventArgs e)
        {
            float[,] dat = new float[3, 3];

            dat[0, 0] = (float)Convert.ToDouble(magAlignment00.Text);
            dat[0, 1] = (float)Convert.ToDouble(magAlignment01.Text);
            dat[0, 2] = (float)Convert.ToDouble(magAlignment02.Text);

            dat[1, 0] = (float)Convert.ToDouble(magAlignment10.Text);
            dat[1, 1] = (float)Convert.ToDouble(magAlignment11.Text);
            dat[1, 2] = (float)Convert.ToDouble(magAlignment12.Text);

            dat[2, 0] = (float)Convert.ToDouble(magAlignment20.Text);
            dat[2, 1] = (float)Convert.ToDouble(magAlignment21.Text);
            dat[2, 2] = (float)Convert.ToDouble(magAlignment22.Text);

            sensor.mag_cal = dat;

            sensor.synch();
        }

        private void zeroGyrosButton_Click(object sender, EventArgs e)
        {
            sensor.ZeroRateGyros();
        }

        private void flashCommitButton_Click(object sender, EventArgs e)
        {
            sensor.WriteToFlash();
        }

        private void EKFResetButton_Click(object sender, EventArgs e)
        {
            sensor.EKF_Reset();
        }

        private void resetToFactoryButton_Click(object sender, EventArgs e)
        {
            sensor.ResetToFactory();
            sensor.Invalidate();
            sensor.synch();
        }

        private void gyroStartupCalEnabled_CheckedChanged(object sender, EventArgs e)
        {
            
        }

        private void activeChannelsCommitButton_Click(object sender, EventArgs e)
        {
            sensor.yaw_active = activeYaw.Checked;
            sensor.pitch_active = activePitch.Checked;
            sensor.roll_active = activeRoll.Checked;
            sensor.yaw_rate_active = activeYawRate.Checked;
            sensor.pitch_rate_active = activePitchRate.Checked;
            sensor.roll_rate_active = activeRollRate.Checked;
            sensor.magX_active = activeXMag.Checked;
            sensor.magY_active = activeYMag.Checked;
            sensor.magZ_active = activeZMag.Checked;
            sensor.gyroX_active = activeXGyro.Checked;
            sensor.gyroY_active = activeYGyro.Checked;
            sensor.gyroZ_active = activeZGyro.Checked;
            sensor.accelX_active = activeXAccel.Checked;
            sensor.accelY_active = activeYAccel.Checked;
            sensor.accelZ_active = activeZAccel.Checked;

            sensor.synch();
        }

        private void gyroBiasCommitButton_Click(object sender, EventArgs e)
        {
            if (gyroStartupCalEnabled.Checked)
            {
                sensor.zero_gyros_on_startup = true;
            }
            else
            {
                sensor.zero_gyros_on_startup = false;
            }

            sensor.x_gyro_bias = Convert.ToInt16(gyroXBias.Text);
            sensor.y_gyro_bias = Convert.ToInt16(gyroYBias.Text);
            sensor.z_gyro_bias = Convert.ToInt16(gyroZBias.Text);
            
            sensor.synch();
        }

        private void gyroScaleCommitButton_Click(object sender, EventArgs e)
        {
            sensor.x_gyro_scale = (float)Convert.ToDouble(gyroXScale.Text);
            sensor.y_gyro_scale = (float)Convert.ToDouble(gyroYScale.Text);
            sensor.z_gyro_scale = (float)Convert.ToDouble(gyroZScale.Text);

            sensor.synch();
        }

        private void selfTestButton_Click(object sender, EventArgs e)
        {
            sensor.SelfTest();
        }

        private void accelBiasCommitButton_Click(object sender, EventArgs e)
        {
            sensor.x_accel_bias = Convert.ToInt16(accelXBias.Text);
            sensor.y_accel_bias = Convert.ToInt16(accelYBias.Text);
            sensor.z_accel_bias = Convert.ToInt16(accelZBias.Text);

            sensor.synch();
        }

        private void EKFCommitButton_Click(object sender, EventArgs e)
        {
            sensor.accel_covariance = (float)Convert.ToDouble(accelVariance.Text);
            sensor.process_covariance = (float)Convert.ToDouble(processVariance.Text);
            sensor.mag_covariance = (float)Convert.ToDouble(magVariance.Text);

            sensor.mag_enabled = magUpdateEnabled.Checked;
            sensor.accel_enabled = accelUpdateEnabled.Checked;

            sensor.synch();
        }

        private void comCommitButton_Click(object sender, EventArgs e)
        {
            sensor.broadcast_enabled = broadcastModeEnabled.Checked;
            sensor.broadcast_rate = (UInt16)broadcastFrequencyBox.Value;

            sensor.synch();
        }

        private void AppendStatusText(string text, Color text_color)
        {
            // InvokeRequired required compares the thread ID of the
            // calling thread to the thread ID of the creating thread.
            // If these threads are different, it returns true.
            if (this.statusBox.InvokeRequired)
            {
                AppendTextCallback d = new AppendTextCallback(AppendStatusText);
                this.Invoke(d, new object[] { text, text_color });
            }
            else
            {
                this.statusBox.SelectionColor = text_color;
                this.statusBox.AppendText(text);
                this.statusBox.ScrollToCaret();
            }
        }

        private void magBiasCommitButton_Click(object sender, EventArgs e)
        {
            sensor.x_mag_bias = Convert.ToInt16(magXBias.Text);
            sensor.y_mag_bias = Convert.ToInt16(magYBias.Text);
            sensor.z_mag_bias = Convert.ToInt16(magZBias.Text);

            sensor.synch();
        }

        private void magRefCommitButton_Click(object sender, EventArgs e)
        {
            sensor.x_mag_ref = Convert.ToInt16(magXRef.Text);
            sensor.y_mag_ref = Convert.ToInt16(magYRef.Text);
            sensor.z_mag_ref = Convert.ToInt16(magZRef.Text);

            sensor.synch();

        }

        private void accelReferenceCommitButton_Click(object sender, EventArgs e)
        {
            sensor.x_accel_ref = Convert.ToInt16(accelXRef.Text);
            sensor.y_accel_ref = Convert.ToInt16(accelYRef.Text);
            sensor.z_accel_ref = Convert.ToInt16(accelZRef.Text);

            sensor.synch();
        }

        private void zeroYawButton_Click(object sender, EventArgs e)
        {
            sensor.AutoSetMagRef();
        }

        private void setAccelReferenceButton_Click(object sender, EventArgs e)
        {
            sensor.AutoSetAccelRef();
        }

        private void getStateCovarianceButton_Click(object sender, EventArgs e)
        {
            sensor.GetStateCovariance();
        }
    }
}
