using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO;

namespace FileDecoder
{
    class Program
    {
        [STAThread]
        static void Main(string[] args)
        {
            using (var ofd = new OpenFileDialog())
            {
                System.Windows.Forms.MessageBox.Show("Select input file");
                DialogResult result = ofd.ShowDialog();
                Stream dataFile = new FileStream(ofd.FileName, FileMode.Open, FileAccess.Read, FileShare.Read);

                System.Windows.Forms.MessageBox.Show("Select output file");
                result = ofd.ShowDialog();
                StreamWriter outputFile = new StreamWriter(ofd.FileName, false);

                StringBuilder logMessage = new StringBuilder();
                char token = '\t';

                logMessage.Append("X Gyro(deg/s) \t Y Gyro \t Z Gyro \t X Accel(mg) \t Y Accel \t Z Accel \t MTSposition");
                outputFile.WriteLine(logMessage.ToString());
                logMessage.Clear();

                BinaryReader SensorValues = new BinaryReader(dataFile);

                double[] scaledResults = new double[6];

                for (int k = 0; k < dataFile.Length/15; k++)
                {
                    for (int i = 0; i < 3; i++)
                    {
                        byte[] read = SensorValues.ReadBytes(2);
                        Array.Reverse(read); //For Endianess 
                        Int16 Value = BitConverter.ToInt16(read, 0);
                        scaledResults[i] = gyroScale(Value);
                        logMessage.Append(scaledResults[i].ToString());
                        logMessage.Append(token);
                    }
                    for (int i = 3; i < 6; i++)
                    {
                        byte[] read = SensorValues.ReadBytes(2);
                        Array.Reverse(read); //For Endianess 
                        Int16 Value = BitConverter.ToInt16(read, 0);
                        scaledResults[i] = accelScale(Value);
                        logMessage.Append(scaledResults[i].ToString());
                        logMessage.Append(token);
                    }
                    byte[] inBytes = SensorValues.ReadBytes(3);
                    int MTSPositionUint = (((inBytes[0] & 0x7F) << 16) | ((inBytes[1] & 0xFF) << 8) | (inBytes[2] & 0xFF)) * 2;
                    double bellowsPosition = 0.0005 * MTSPositionUint; //in mm
                    logMessage.Append(bellowsPosition.ToString());
                    logMessage.Append(token);

                    outputFile.WriteLine(logMessage);
                    logMessage.Clear();
                }
            }
        }






        //scales accelerometer measurements and returns data in 10^-4 g's
        public static double accelScale(short sensorData)
        {
            double finalData = (sensorData) * 0.25; // Multiply by accel sensitivity (0.25 mg/LSB)
            return finalData;
        }

        //scale delta velocity measurements and return mm/sec
        //roundabout way of rounding to 4 decimal places
        public static double deltaVelocityScale(short sensorData)
        {
            double finalData = (sensorData) * 2.5; // Multiply by velocity scale (2.5 mm/sec/LSB)
            return finalData;
        }

        //scales gyro measurements and returns degrees/second
        //same for delta angle
        public static double gyroScale(short sensorData)
        {
            double finalData = (sensorData) * 0.005; //(0.005 degrees/LSB)
            return finalData;
        }

    }
}