using System;
using Microsoft.SPOT;

using Microsoft.SPOT.Hardware;
using Microsoft.SPOT.IO;

using System.Threading;
using System.Text;
using System.IO;
using System.IO.Ports;

using GHI.Premium.IO;
using GHI.Premium.System;
using GHI.Premium.Hardware;
using GHI.Hardware.EMX;

namespace miniCPF
{
    public class Program
    {
        public static PersistentStorage sdCard = new PersistentStorage("SD");

        private static Timer CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000);

        private static FileStream testFile;

        public static DateTime timeNow;
        public static DateTime synchDateTime;

        public static byte[] logByteArray = new byte[1024];

        public static UInt32 todTicks;
        private static int GPSMessageNum = 0;

        private static int surfaceOpsTimeout = 10000;

        private static bool timeSynched = false;
        private static bool missionRun = true;
        private static bool stateTimedout = false;
        private static bool stopGPS = false;

        private static byte[] GPSInByteArray = new byte[10240];
        private static char[] tempCharArray = new char[20480];
        private static byte[] RMCByteArray = new byte[10240];
        private static int GPSByteNum = 0;
        private static int GPSPortBytesToRead = 0;
        private static byte[] GPSTempInBytes = new byte[10240];

        private static byte[] message = new byte[256];
        private static byte[] queueArray = new byte[256];
        private static byte[] tempQueueArray = new byte[256];

        private static string fileName;

        private static byte[] commentColumn = new byte[1] { 0 };
        private static byte[] latColumn = new byte[1] { 1 };
        private static byte[] lonColumn = new byte[1] { 2 };

        private static int tokenCounter = 0;
        private static int hour = 1;
        private static int minute = 0;
        private static int second = 0;
        private static int year = 0;
        private static int month = 0;
        private static int day = 0;
        private static double lat = 0;
        private static double lon = 0;


        public enum CPFStates : int
        {
            StartInitialize = 0,
            Initialize,
            StartPreMissionDelay,
            PreMissionDelay,
            StartSurfaceOps,
            SynchTime,
            SurfaceOps,
            StartAutoballast,
            Autoballast,
            StartDescend,
            Descend,
            StartDescendPark,
            DescendPark,
            StartAscend,
            Ascend,
            StartAscendPark,
            AscendPark,
            StartEmergencyAscend,
            EmergencyAscend,
            StartRecovery,
            Recovery,
            ProcessErrors,
            Exit,
        }
        public static CPFStates CPFState;

        public static SerialPort GPSPort = new SerialPort("COM3", 9600, Parity.None);

        private static ByteQueue MessageQueue = new ByteQueue();

        public static void Main()
        {
            Debug.Print("Program Started     " + DateTime.Now.ToString() + "." + DateTime.Now.Millisecond.ToString());

            CPFState = CPFStates.StartInitialize;

            sdCard.MountFileSystem();
            VolumeInfo sdVol = new VolumeInfo("SD");
            Debug.Print("Volume Name:           " + sdVol.Name.ToString());
            Debug.Print("Volume Total Size:     " + sdVol.TotalSize.ToString());
            Debug.Print("Volume Free Space:     " + sdVol.TotalFreeSpace.ToString());
            Debug.Print("Volume File System:    " + sdVol.FileSystem.ToString());
            Debug.Print("Volume Serial Num:     " + sdVol.SerialNumber.ToString());
            Debug.Print("Volume ID:             " + sdVol.VolumeID.ToString());
            Debug.Print("Volume Label:          " + sdVol.VolumeLabel.ToString());
            Debug.Print("Volume Root Dir:       " + sdVol.RootDirectory.ToString());

            String[] fileNames = Directory.GetFiles(sdVol.RootDirectory);
            for (int i = 0; i < fileNames.Length; i++)
            {
                FileInfo fileInfo = new FileInfo(fileNames[i]);
                Debug.Print(i.ToString() + "  File name: " + fileNames[i]
                            + "   Creation Date = " + fileInfo.CreationTime.ToString()
                            + "  Length = " + fileInfo.Length.ToString());
            }

            timeNow = DateTime.Now;
            fileName = sdVol.RootDirectory + "\\" +
                        timeNow.Year.ToString() + "-" +
                        timeNow.Month.ToString() + "-" +
                        timeNow.Day.ToString() + "T" +
                        timeNow.Hour.ToString() + "-" +
                        timeNow.Minute.ToString() + "-" +
                        timeNow.Second.ToString();

            testFile = new FileStream(fileName, FileMode.OpenOrCreate, FileAccess.Write);

            byte[] testBytes = new byte[6] { 65, 66, 67, 68, 69, 70 };

            while (missionRun)
            {
                switch (CPFState)
                {
                    case (CPFStates.StartInitialize):
                        Debug.Print("State = Start Intialize");
                        CPFState = CPFStates.Initialize;
                        break;
                    case (CPFStates.Initialize):
                        Debug.Print("State = Intialize");
                        CPFState = CPFStates.StartPreMissionDelay;
                        break;
                    case (CPFStates.StartPreMissionDelay):
                        Debug.Print("State = Start PremissionDelay");
                        CPFState = CPFStates.PreMissionDelay;
                        break;
                    case (CPFStates.PreMissionDelay):
                        Debug.Print("State = PreMissionDelay");
                        CPFState = CPFStates.StartSurfaceOps;
                        break;
                    case (CPFStates.StartSurfaceOps):
                        Debug.Print("State = Start SurfaceOps " + DateTime.Now.ToString());

                        GPSPort.Open();
                        GPSPort.DiscardInBuffer();
                        GPSPort.DiscardOutBuffer();
                        GPSPort.DataReceived += new SerialDataReceivedEventHandler(GPSPortDataReceived);

                        CPFState = CPFStates.SynchTime;
                        break;
                    case (CPFStates.SynchTime):
                        //Debug.Print("State = SynchTime " + DateTime.Now.ToString());

                        if (!timeSynched)
                        {
                            if (MessageQueue.Count > 0)
                            {
                                processMessageQueue();
                            }
                        }
                        else
                        {
                            CPFStateTimer.Change(surfaceOpsTimeout, 10000);
                            CPFState = CPFStates.SurfaceOps;
                        }
                        break;
                    case (CPFStates.SurfaceOps):
                        if (stateTimedout)
                        {
                            stopGPS = true;
                            CPFStateTimer.Change(1000000, 1000000);
                            CPFState = CPFStates.StartAutoballast;
                        }

                        if (MessageQueue.Count > 0)
                            processMessageQueue();

                        break;
                    case (CPFStates.StartAutoballast):
                        Debug.Print("State = Start Autoballast");
                        CPFState = CPFStates.StartAutoballast;
                        break;
                    case (CPFStates.Autoballast):
                        Debug.Print("State = Autoballast");
                        CPFState = CPFStates.StartDescend;
                        break;
                    case (CPFStates.StartDescend):
                        Debug.Print("State = Start Descend");
                        CPFState = CPFStates.Descend;
                        break;
                    case (CPFStates.Descend):
                        Debug.Print("State = Descend");
                        CPFState = CPFStates.StartDescendPark;
                        break;
                    case (CPFStates.StartDescendPark):
                        Debug.Print("State = Start DescendPark");
                        CPFState = CPFStates.DescendPark;
                        break;
                    case (CPFStates.DescendPark):
                        Debug.Print("State = Descendpark");
                        CPFState = CPFStates.StartAscend;
                        break;
                    case (CPFStates.StartAscend):
                        Debug.Print("State = Start Ascend");
                        CPFState = CPFStates.Ascend;
                        break;
                    case (CPFStates.Ascend):
                        Debug.Print("State = Ascend");
                        CPFState = CPFStates.StartAscendPark;
                        break;
                    case (CPFStates.StartAscendPark):
                        Debug.Print("State = Start AscendPark");
                        CPFState = CPFStates.AscendPark;
                        break;
                    case (CPFStates.AscendPark):
                        Debug.Print("State = AscendPark");
                        CPFState = CPFStates.SurfaceOps;
                        break;
                    case (CPFStates.StartEmergencyAscend):
                        Debug.Print("State = Start EmergencyAscend");
                        CPFState = CPFStates.EmergencyAscend;
                        break;
                    case (CPFStates.EmergencyAscend):
                        Debug.Print("State = EmergencyAscend");
                        CPFState = CPFStates.StartRecovery;
                        break;
                    case (CPFStates.StartRecovery):
                        Debug.Print("State = Start Recovery");
                        CPFState = CPFStates.Recovery;
                        break;
                    case (CPFStates.Recovery):
                        Debug.Print("State = Recovery");
                        break;
                    case (CPFStates.ProcessErrors):
                        Debug.Print("State = ProcessErrors");
                        break;
                    case (CPFStates.Exit):
                        Debug.Print("State = Exit");
                        missionRun = false;
                        break;
                }
                Thread.Sleep(100);
            }
            testFile.Close();
            GPSPort.Close();
            sdCard.UnmountFileSystem();
        }

        private static void GPSPortDataReceived(object sender, SerialDataReceivedEventArgs e)
        {

            GPSPortBytesToRead = GPSPort.BytesToRead;

            try
            {
                GPSPort.Read(GPSTempInBytes, 0, GPSPortBytesToRead);
            }
            catch
            {
                Debug.Print("GPS Read Exception");          //TODO do something smarter here
            }

            for (int i = 0; i < GPSPortBytesToRead; i++)
            {
                GPSInByteArray[GPSByteNum] = GPSTempInBytes[i];
                if (GPSByteNum == 0)    //Check to make sure first character is = $ (NMEA standard start of string)
                {
                    if (GPSInByteArray[0] == 36)  //only move on to byte index 1 after we get a $
                    {
                        GPSByteNum = GPSByteNum + 1;
                    }
                }
                else
                {
                    if (GPSInByteArray[GPSByteNum] == 10)
                    {
                        Array.Clear(queueArray, 0, queueArray.Length);
                        Array.Copy(GPSInByteArray, queueArray, queueArray.Length);
                        MessageQueue.Enqueue(queueArray);
                        GPSMessageNum = GPSMessageNum + 1;
                        Debug.Print("Got GPS Message Number = " + GPSMessageNum.ToString());
                        //tempCharArray = Encoding.UTF8.GetChars(GPSInByteArray);
                        GPSByteNum = 0;
                    }
                    else
                    {
                        GPSByteNum = GPSByteNum + 1;
                    }
                }
            }
            if (stopGPS)
            {
                GPSPort.DiscardInBuffer();          //TODO This doesn't seem to work 100% of the time, still get exceptions once in a while
                GPSPort.Close();
            }
        }

        private static void parseRMC(byte[] RMCByteArray)
        {
            tokenCounter = 0;
            tempCharArray = UTF8Encoding.UTF8.GetChars(RMCByteArray);

            for (int i = 0; i < 80; i++)
            {
                if (RMCByteArray[i] == 44)  //The NMEA separator is a comma (',') = ASCII 44
                {
                    tokenCounter = tokenCounter + 1;    //Time is the first field after $GPRMC hhmmss.sss
                    if (tokenCounter == 1)
                    {
                        hour = (RMCByteArray[i + 1] - 48) * 10 + (RMCByteArray[i + 2] - 48);
                        minute = (RMCByteArray[i + 3] - 48) * 10 + (RMCByteArray[i + 4] - 48);
                        second = (RMCByteArray[i + 5] - 48) * 10 + (RMCByteArray[i + 6] - 48);
                    }

                    if (tokenCounter == 3)
                    {
                        lat = (RMCByteArray[i + 1] - 48.0) * 10 + (RMCByteArray[i + 2] - 48.0) +                //TODO: Make this work for any number of digits before and after the decimal place
                                ((((RMCByteArray[i + 3] - 48.0) * 10.0) + (RMCByteArray[i + 4] - 48.0) +
                                  ((RMCByteArray[i + 6] - 48.0) * 0.1) + ((RMCByteArray[i + 7] - 48.0) * 0.01) +
                                  ((RMCByteArray[i + 8] - 48.0) * 0.001) + ((RMCByteArray[i + 9] - 48.0) * 0.0001) +
                                  ((RMCByteArray[i + 10] - 48.0) * 0.00001)) / 60.0);
                        if (RMCByteArray[i + 12] == (byte)'S')
                        {
                            lat = -1.0 * lat;
                        }
                    }

                    if (tokenCounter == 5)
                    {
                        lon = ((RMCByteArray[i + 1] - 48.0) * 100.0) + ((RMCByteArray[i + 2] - 48.0) * 10.0) + (RMCByteArray[i + 3] - 48.0) +               //TODO: Make this work for any number of digits before and after the decimal place
                                ((((RMCByteArray[i + 4] - 48.0) * 10.0) + (RMCByteArray[i + 5] - 48.0) +
                                  ((RMCByteArray[i + 7] - 48.0) * 0.1) + ((RMCByteArray[i + 8] - 48.0) * 0.01) +
                                  ((RMCByteArray[i + 9] - 48.0) * 0.001) + ((RMCByteArray[i + 10] - 48.0) * 0.0001) +
                                  ((RMCByteArray[i + 11] - 48.0) * 0.00001)) / 60.0);
                        if (RMCByteArray[i + 12] == (byte)'W')
                        {
                            lon = -1.0 * lon;
                        }
                    }

                    if (tokenCounter == 9)      //Date is the 9th field  ddmmyy
                    {
                        day = (RMCByteArray[i + 1] - 48) * 10 + (RMCByteArray[i + 2] - 48);
                        month = (RMCByteArray[i + 3] - 48) * 10 + (RMCByteArray[i + 4] - 48);
                        year = 2000 + (RMCByteArray[i + 5] - 48) * 10 + (RMCByteArray[i + 6] - 48);
                        synchTime(year, month, day, hour, minute, second);
                    }
                }
            }

            Array.Clear(tempQueueArray, 0, tempQueueArray.Length);
            tempQueueArray = formatLogArray();

            int index;
            index = Array.IndexOf(tempQueueArray, 0);
            Util.InsertValueIntoArray((float)lat, tempQueueArray, index);
            tempQueueArray[index + 4] = 9; // 9 = ASCII tab

            index = Array.IndexOf(tempQueueArray, 0);
            Util.InsertValueIntoArray((float)lon, tempQueueArray, index);
            tempQueueArray[index + 4] = 9; // 9 = ASCII tab

            Array.Clear(tempCharArray, 0, tempCharArray.Length);
            tempCharArray = UTF8Encoding.UTF8.GetChars(queueArray);
      }

        private static byte[] formatLogArray()
        {
            timeNow = DateTime.Now;
            Array.Clear(tempQueueArray, 0, tempQueueArray.Length);

            string dateTime = timeNow.ToString();

            // Year/Month/Day tab
            for (int i = 6; i < 10; i++)
                tempQueueArray[i - 6] = (byte)dateTime[i];
            tempQueueArray[4] = (byte)'/';
            for (int i = 0; i < 2; i++)
                tempQueueArray[i + 5] = (byte)dateTime[i];
            tempQueueArray[7] = (byte)'/';
            for (int i = 3; i < 5; i++)
                tempQueueArray[i + 5] = (byte)dateTime[i];
            tempQueueArray[10] = 9;     //9 = ASCII tab

            // Hour:Minute:Second tab
            for (int i = 11; i < 19; i++)
                tempQueueArray[i] = (byte)dateTime[i];
            tempQueueArray[19] = 9;     //9 = ASCII tab

            tempCharArray = UTF8Encoding.UTF8.GetChars(tempQueueArray);

            return tempQueueArray;


        }


        private static void synchTime(int year, int month, int day, int hour, int minute, int second)
        {
            synchDateTime = new DateTime(year, month, day, hour, minute, second);

            Debug.Print("Before synchTime DateTime.Now = " + DateTime.Now.ToString());
            Debug.Print("Before synchTime RTC.Now = " + RealTimeClock.GetTime().ToString());

            Utility.SetLocalTime(synchDateTime);
            RealTimeClock.SetTime(synchDateTime); //This will set the hardware Real-time Clock to what is in DT

            Debug.Print("After synchTime DateTime.Now = " + DateTime.Now.ToString());
            Debug.Print("After synchTime RTC.Now = " + RealTimeClock.GetTime().ToString());
            timeSynched = true;
        }

        private static void CPFStateTimerCallback(object o)
        {
            Debug.Print("CPF State Timed Out " + DateTime.Now.ToString() + "." + DateTime.Now.Millisecond.ToString());
            stateTimedout = true;
        }

        private static void processMessageQueue()
        {
            do
            {
                Array.Clear(message, 0, message.Length);
                message = MessageQueue.Dequeue();
                if (!timeSynched)
                {
                    if ((message[1] == (byte)'G') && (message[2] == (byte)'P') && (message[3] == (byte)'R') &&
                        (message[4] == (byte)'M') && (message[5] == (byte)'C'))
                    {
                        parseRMC(message);
                    }
                }
                logMessage(message);

            } while (MessageQueue.Count > 0);
        }

        private static void logMessage(byte[] message)
        {
            Array.Clear(logByteArray, 0, logByteArray.Length);

            timeNow = DateTime.Now;

            Utility.InsertValueIntoArray(logByteArray, 0, 4, (UInt32)timeNow.Year);
            logByteArray[4] = (byte)'-';
            Utility.InsertValueIntoArray(logByteArray, 5, 4, (UInt32)timeNow.Month);
            logByteArray[9] = (byte)'-';
            Utility.InsertValueIntoArray(logByteArray, 10, 4, (UInt32)timeNow.Day);
            logByteArray[14] = 9;        //tab delimeter

            //todTicks = (UInt32)(timeNow.TimeOfDay.Ticks / 10000);
            Microsoft.SPOT.Hardware.Utility.InsertValueIntoArray(logByteArray, 15, 4, (UInt32)(timeNow.TimeOfDay.Ticks / 10000));
            logByteArray[19] = 9;        //tab delimeter

            Array.Copy(message, 0, logByteArray, 20, message.Length);

            testFile.Write(logByteArray, 0, logByteArray.Length);
        }
    }
}