using System;
using Microsoft.SPOT;

using Microsoft.VisualStudio.TestTools.UnitTesting;
using CPF;

using System.Text;
using System.Threading;
using HWModules;
using SWModules;
using SensorModules;

namespace CPFUnitTests
{
    [TestClass]
    public class GPSUnitTests
    {
        [TestMethod]
        public void TestGPSSciMsg()
        {
            GPS1 gps = GPS1.Instance;
            gpsUBlox.GPSPosition gpsPosition = new gpsUBlox.GPSPosition();

            HWModules.MotherBoard motherboard = MotherBoard.Instance;
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.GPSChannel);

            gpsPosition = gps.getPUBX00Position();
            updateLatLon(gpsPosition);
        }

        [TestMethod]
        public void TestGPSandI2C()
        {
            GPS1 gps = GPS1.Instance;
            gpsUBlox.GPSPosition currentGPSPosition = new gpsUBlox.GPSPosition();

            HWModules.MotherBoard motherboard = MotherBoard.Instance;
            Debug.Print("\r\nDisabling gps channel power");
            motherboard.DisableChannelPower(MotherBoard.ChannelNames.GPSChannel);
 
            BTConsole btConsole = BTConsole.Instance;
            btConsole.powerUp();
            btConsole.init();

            int bellowsADC;
            double bellowsPosition;

            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            Debug.Print("\r\nDisabling position sensor channel power");
            motherboard.DisableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);

            StringBuilder sbTemp = new StringBuilder(128);

            Debug.Print("\r\nEnabling position sensor channel power");
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);
            Thread.Sleep(1000);

            for (int i = 0; i < 5; i++)
            {
                Debug.Print("Reading string pot");
                bellowsADC = buoyancyEngine.getBellowsADCValue();
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                Debug.Print(i + "\tUnit Test Bellows Position = " +
                            bellowsPosition.ToString("f3") +
                            " ADC = " + bellowsADC);
                Thread.Sleep(1000);
            }

            Debug.Print("\r\nEnabling GPS power");
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.GPSChannel);
            Thread.Sleep(1000);

            for (int i = 0; i < 5; i++)
            {
                Debug.Print("Reading string pot");
                bellowsADC = buoyancyEngine.getBellowsADCValue();
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                Debug.Print(i + "\tUnit Test Bellows Position = " +
                            bellowsPosition.ToString("f3") +
                            " ADC = " + bellowsADC);
                Thread.Sleep(1000);
            }

            for (int i = 0; i < 5; i++)
            {
                sbTemp.Clear();
                sbTemp.Append(i.ToString());
                sbTemp.Append(" Sending GPS getPosition");
                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                currentGPSPosition = gps.getPUBX00Position();
                sbTemp.Clear();
                sbTemp.Append(currentGPSPosition.rawMessage.ToString());
                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                Thread.Sleep(1000);
            }

            for (int i = 0; i < 5; i++)
            {
                Debug.Print("Reading string pot");
                bellowsADC = buoyancyEngine.getBellowsADCValue();
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                Debug.Print(i + "\tUnit Test Bellows Position = " +
                            bellowsPosition.ToString("f3") +
                            " ADC = " + bellowsADC);
                Thread.Sleep(1000);
            }

            Debug.Print("\r\nDisabling GPS power");
            motherboard.DisableChannelPower(MotherBoard.ChannelNames.GPSChannel);
            Thread.Sleep(1000);

            for (int i = 0; i < 5; i++)
            {
                Debug.Print("Reading string pot");
                bellowsADC = buoyancyEngine.getBellowsADCValue();
                bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                Debug.Print(i + "\tUnit Test Bellows Position = " +
                            bellowsPosition.ToString("f3") +
                            " ADC = " + bellowsADC);
                Thread.Sleep(1000);
            }

        }

        [TestMethod]
        public void TestGPS()
        {
            GPS1 gps = GPS1.Instance;
            gpsUBlox.GPSPosition currentGPSPosition = new gpsUBlox.GPSPosition();
            gpsUBlox.SVStatus currentSVStatus = new gpsUBlox.SVStatus();

            HWModules.MotherBoard motherboard = MotherBoard.Instance;
            motherboard.EnableChannelPower(MotherBoard.ChannelNames.GPSChannel);

            BTConsole btConsole = BTConsole.Instance;
            btConsole.powerUp();
            btConsole.init();

            int bellowsADC;
            double bellowsPosition;

            BuoyancyEngine buoyancyEngine = BuoyancyEngine.Instance;
            motherboard.DisableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);

            StringBuilder sbTemp = new StringBuilder(128);

            //sbTemp.Clear();
            //sbTemp.Append("Forcing cold start");
            //Debug.Print(sbTemp.ToString());
            //gps.forceColdstart();

            Thread.Sleep(10000);

            char[] tokens = new char[2];
            tokens[0] = ',';
            tokens[1] = '*';

            for (int i = 0; i < 500; i++)
            {
                if (i < 5)
                {
                    Debug.Print("Disabling GPS power");
                    motherboard.DisableChannelPower(MotherBoard.ChannelNames.GPSChannel);
                    Debug.Print("\r\nEnabling position sensor channel power");
                    motherboard.EnableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);
                    Debug.Print("Reading string pot");
                    bellowsADC = buoyancyEngine.getBellowsADCValue();
                    bellowsPosition = buoyancyEngine.getBellowsPositionStringPot();
                    EngrLogger.Comment(i + "\tUnit Test Bellows Position = " +
                                bellowsPosition.ToString("f3") +
                                " ADC = " + bellowsADC);
                    Debug.Print("Enabling GPS power");
                    motherboard.EnableChannelPower(MotherBoard.ChannelNames.GPSChannel);
                    Thread.Sleep(1000);
                }

                if (i == 5)
                {
                    Debug.Print("\r\nDisabling position sensor channel power");
                    motherboard.DisableChannelPower(MotherBoard.ChannelNames.PositionSensorChannel);
                }


                sbTemp.Clear();
                sbTemp.Append(i.ToString());
                sbTemp.Append(" Sending GPS getPosition");
                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                currentGPSPosition = gps.getPUBX00Position();
                sbTemp.Clear();
                sbTemp.Append(currentGPSPosition.rawMessage.ToString());
                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                var pubx00Split = sbTemp.ToString().Split(',');
                sbTemp.Clear();
                sbTemp.Append("UTC time: ");
                sbTemp.Append(pubx00Split[2]);
                sbTemp.Append("\r\n");

                sbTemp.Append("Num Sats used in fix: ");
                sbTemp.Append(pubx00Split[18]);
                sbTemp.Append("\r\n");

                sbTemp.Append("Nav status: ");
                sbTemp.Append(pubx00Split[18]);
                sbTemp.Append("\r\n");

                sbTemp.Append("HDOP: ");
                sbTemp.Append(pubx00Split[15]);
                sbTemp.Append("\r\n");
                sbTemp.Append("VDOP: ");
                sbTemp.Append(pubx00Split[16]);
                sbTemp.Append("\r\n");
                sbTemp.Append("TDOP: ");
                sbTemp.Append(pubx00Split[17]);
                sbTemp.Append("\r\n");
                sbTemp.Append("hAccuracy: ");
                sbTemp.Append(pubx00Split[9]);
                sbTemp.Append("\r\n");
                sbTemp.Append("vAccusary: ");
                sbTemp.Append(pubx00Split[10]);
                sbTemp.Append("\r\n");

                sbTemp.Append("Lat: ");
                sbTemp.Append(pubx00Split[3]);
                sbTemp.Append(pubx00Split[4]);
                sbTemp.Append("\r\n");
                sbTemp.Append("Lon: ");
                sbTemp.Append(pubx00Split[5]);
                sbTemp.Append(pubx00Split[6]);
                sbTemp.Append("\r\n");
                sbTemp.Append("altitude: ");
                sbTemp.Append(pubx00Split[7]);
                sbTemp.Append("\r\n");

                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                sbTemp.Append("Sending GPS PUBX,03 Get Sat Status");
                currentSVStatus = gps.getSatStatus();
                sbTemp.Clear();
                sbTemp.Append(currentSVStatus.rawMessage.ToString());
                Debug.Print(sbTemp.ToString());
                btConsole.SendLine(sbTemp);

                var pubx03Split = sbTemp.ToString().Split(tokens);
                if (pubx03Split.Length > 3)
                {
                    sbTemp.Clear();
                    sbTemp.Append("Num Sats tracked: ");
                    int numSatsTracked = Int32.Parse(pubx03Split[2]);
                    sbTemp.Append(numSatsTracked.ToString());
                    sbTemp.Append("\r\n");

                    sbTemp.Append("SatID\tstatus\taz.\telev.\tC_NO\tcarrier lock time (status: - Not used, U Used in fix, e Eph avail but not used in fix)");
                    sbTemp.Append("\r\n");

                    for (int j = 0; j < numSatsTracked; j++)
                    {
                        sbTemp.Append(pubx03Split[3 + 6 * j]);
                        sbTemp.Append("\t\t");
                        sbTemp.Append(pubx03Split[4 + 6 * j]);
                        sbTemp.Append("\t\t");
                        sbTemp.Append(pubx03Split[5 + 6 * j]);
                        sbTemp.Append('\t');
                        sbTemp.Append(pubx03Split[6 + 6 * j]);
                        sbTemp.Append("\t\t");
                        sbTemp.Append(pubx03Split[7 + 6 * j]);
                        sbTemp.Append("\t\t");
                        sbTemp.Append(pubx03Split[8 + 6 * j]);
                        sbTemp.Append("\r\n");
                    }

                    Debug.Print(sbTemp.ToString());
                    btConsole.SendLine(sbTemp);
                }

                //sbTemp.Clear();
                //sbTemp.Append("Sending GPS Nav-Orb");
                //Debug.Print(sbTemp.ToString());
                //btConsole.SendLine(sbTemp);

                //navOrbBytes = gps.getNavOrb();
                //sbTemp.Clear();
                //sbTemp.Append("Nav-Orb bytes: ");
                //sbTemp.Append(navOrbBytes.ToString());
                //Debug.Print(sbTemp.ToString());
                //btConsole.SendLine(sbTemp);

                Thread.Sleep(2000);
            }
        }

        private static StringBuilder gpsPositionSB = new StringBuilder(128);

        public static void updateLatLon(gpsUBlox.GPSPosition position)
        {
            double lat = double.NaN;
            double lon = double.NaN;

            gpsPositionSB.Clear();
            gpsPositionSB.Append("# GPS fix obtained in (seconds) \t");
            gpsPositionSB.Append(position.gpsTTFF.ToString());
            gpsPositionSB.Append(Literals.crlfString);

            gpsPositionSB.Append("#Fix: \tLon\tLat\tmm/dd/yyyy\thhmms\tnumSats\tnmeaLon\tnmeaLat\tgpsTimeOfFix");
            gpsPositionSB.Append(Literals.crlfString);

            gpsPositionSB.Append("Fix:");
            gpsPositionSB.Append(Literals.tabChar);
            lon = convertLonToDecDeg(position.lonSB.ToString());
            gpsPositionSB.Append(lon.ToString("f8"));

            gpsPositionSB.Append(Literals.tabChar);
            lat = convertLatToDecDeg(position.latSB.ToString());
            gpsPositionSB.Append(lat.ToString("f8"));

            gpsPositionSB.Append(Literals.tabChar);
            gpsPositionSB.Append(position.cpfDateTimeOfFix.ToString("mm/dd/yyyy HH:mm:ss"));

            gpsPositionSB.Append(Literals.tabChar);
            gpsPositionSB.Append(position.numSats);

            gpsPositionSB.Append(Literals.tabChar);
            gpsPositionSB.Append(position.lonSB);

            gpsPositionSB.Append(Literals.tabChar);
            gpsPositionSB.Append(position.latSB);

            gpsPositionSB.Append(Literals.tabChar);
            gpsPositionSB.Append(position.gpsTimeOfFix);
        }

        private static double convertLatToDecDeg(string sLat)
        {
            int decPtIndex = 0;
            double lat = double.NaN;
            double deg;
            double min;

            //TODO P1 need to make sure pubx always has 2 digits of minutes
            try
            {
                decPtIndex = sLat.IndexOf('.');
                min = double.Parse(sLat.Substring(decPtIndex - 2, sLat.Length - decPtIndex));
                deg = double.Parse(sLat.Substring(0, decPtIndex - 2));
                lat = deg + min / 60.0;
                if (sLat.Substring(sLat.Length - 1, 1) == "S")
                    lat = -1 * lat;
            }
            catch
            {
                lat = double.NaN;
            }

            return lat;
        }

        private static double convertLonToDecDeg(string sLon)
        {
            int decPtIndex = 0;
            double lon = double.NaN;
            double deg;
            double min;

            //TODO P1 need to make sure pubx always has 2 digits of minutes
            try
            {
                decPtIndex = sLon.IndexOf('.');
                min = double.Parse(sLon.Substring(decPtIndex - 2, sLon.Length - decPtIndex));
                deg = double.Parse(sLon.Substring(0, decPtIndex - 2));
                lon = deg + min / 60.0;
                if (sLon.Substring(sLon.Length - 1, 1) == "W")
                    lon = -1 * lon;
            }
            catch
            {
                lon = double.NaN;
            }

            return lon;
        }
    }
}