using System;
using System.Text;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHI.Pins;
using GHI.Processor;
using GHI.IO;
using System.Threading;


namespace IMULogger
{
    public class Program
    {
        private static byte[] IMUDataBytes = new byte[45];
        private static float[] IMUDataFloats = new float[23];

        // output data in byte format (true) or human readable text format (flase)
        private static bool readBytes = false;
        private static bool timerEvent = true;
        //private static string fileName = @"\data52.txt";
        private static int numSamplesRead = 1500;
        private static int loopSampleRate = 45; // [Hz]

        public static void Main()
        {
            OutputPort G400HDR_LED = new OutputPort(GHI.Pins.G400.PD3, false);

            Debug.Print("Program Started");
            G400HDR_LED.Write(false);

            //// Turn ON Blue LED
            //for (int j = 1; j < 10; j++)
            //{
            //    G400HDR_LED.Write(true);
            //    Thread.Sleep(500);
            //    G400HDR_LED.Write(false);
            //}

           Real_Time_Clock.setupRTC();
            DateTime startTime = DateTime.Now;
            
            BNO055.init(BNO055.NDOF);
            
            // Set the file name based on date and time
            string fileName = "\\" + startTime.ToString("dd") + startTime.ToString("MMM") + startTime.ToString("yyyy") + "-" + startTime.ToString("HH") + startTime.ToString("mm") + startTime.ToString("ss")+".txt";
            EngrLogger.init(fileName);


            G400HDR_LED.Write(true);
            //----------------------------------------
            // Main loop to read sensor and save data
            //----------------------------------------
            if (timerEvent)
            {
                Debug.Print("Starting timer loop");
                int timerInterval = 1000 / loopSampleRate;
                //Debug.GC(true);
                Timer myTimer = new Timer(new TimerCallback(timerReadSensor), null, 0, timerInterval);

                // total length of timer duration
                int timerDuration = numSamplesRead / loopSampleRate * 1000;
                Thread.Sleep(timerDuration);
            }
            else
            {
                Debug.Print("Starting counter loop");

                for (int count = 1; count <= numSamplesRead; count++)
                {
                    BNO055.readData(readBytes, ref IMUDataBytes, ref IMUDataFloats);

                    if (readBytes)
                        EngrLogger.saveDataBytes(ref IMUDataBytes);
                    else
                        EngrLogger.saveDataFloats(ref IMUDataFloats);
                }
            }

            // Calculate the sampling rate
            DateTime endTime = DateTime.Now;
            EngrLogger.close();

            TimeSpan elapsedTime = endTime - startTime;
            double sampleRate = 0;
            double elapsedSeconds = (double)elapsedTime.Ticks / (double)TimeSpan.TicksPerSecond;
            sampleRate = numSamplesRead / elapsedSeconds;

            Debug.Print("\n\n Sample Rate for " + numSamplesRead.ToString() + " samples = " + sampleRate.ToString("f1") + " Samples/second");
            // Turn OFF Blue LED
            G400HDR_LED.Write(false);

        }

        static void timerReadSensor(object o)
        {
            BNO055.readData(readBytes, ref IMUDataBytes, ref IMUDataFloats);

            if (readBytes)
                EngrLogger.saveDataBytes(ref IMUDataBytes);
            else
                EngrLogger.saveDataFloats(ref IMUDataFloats);
            // run the timer event

        }
    }
}