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

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

using GHI.Premium.Hardware;

namespace MFConsoleApplication1
{
    public class Program
    {
        public static void Main()
        {
            Debug.Print("\r\nStarting Program ... \r\n");
  
            int loopNum = 0;

            //DGH stuff
            int DGHReadCount = 0;
            int DGHBytesRead = 0;
            
            byte[] DGHTxBytes;
            byte[] DGHRxBytes = new byte[2];
            byte[] DGHRxByteArray = new byte[20];
            byte[] strippedDGHRxByteArray = new byte[15];

            char[] DGHRxCharArray = new char[20];

            string DGHRead = "$1RD\r";       //$1RD terminated with CR is the command to read data from the DGH
            string DGHRxString = "";
            
            double pressureDBar = 1000.0;
            double ma2bar = (15.0/1.45038) / 16.0;     //4-20 ma transducer; 15 psig full scale; 1 dBar = 1.45038 psi
            double maOffset = 4.0;

            //Elmo Stuff
            byte[] ElmoTxBytes;

            string ElmoStop = "ST;";
            string ElmoMotorOn = "MO=1;";
            string ElmoMotorOff = "MO=0;";
            string ElmoJogVelocity = "JV=";
            string ElmoBegin = "BG;";
            string ElmoVelocityMode = "UM=2;";
            string ElmoJVString = "0.0";

            //Console port sutff
            int consoleBytesRead = 0;
            int consoleReadCount = 0;

            bool waitForBG = true;
 
            byte[] consoleTxBytes;
            byte[] consoleRxBytes = new byte[2];
            byte[] consoleRxByteArray = new byte[3];
            byte byteB = 66;
            byte byteb = 98;
            byte byteG = 71;
            byte byteg = 103;

            string consoleBegin = "BG";
            string consoleStop = "ST";
            string consoleUpload = "UP";

            double RPM2CPS = 500.0 / 60.0;      //500 Counts per Rev divided by 60 seconds per minu
            
            //PID algorithm parameters
            double  K = 100.0,      // Constant for P term of PID
                    Ti = 5000.0,    // Time constant for I term of PID 
                    Td = 0.1,       // Time constant for D term of PID
                    Tf = 2.0,       // Derivative filter time constant
                    Tt = 1.0,       // Anti-windup time constant
                    B = 1.0,        // Setpoint weight
                    h = 0.1,        // Sample period (seconds)
                    I = 0,          // Integrator part of PID calculation
                    y1 = 0,         // first state variable
                    y2 = 0,         // second state variable
                    v = 0.0,        // unclipped PID output
                    u = 0.0,        // clipped PID output
                    uCPS = 0.0,     // u value in counts per second
                    uLow = -3000.0, // minimum possible output
                    uHigh = 3000;   // maximum possible output

            // Pre-computed constants for PID algorithm
            double  den = (Tf * Tf) + (2 * h * Tf) + (2 * h * h),   // denominator of measured signal filter terms
                    p1 = Tf * Tf / den,                             // first part of state variable x2 equation
                    p2 = 2 * h * h / den,                           // second part of state variable x1 equation
                    p3 = K * h / Ti,                                // integral gain
                    p4 = K * Td / h,                                // derivative gain
                    p5 = h / Tt;                                    // anti-windup gain
            
            DateTime loopTime;
            TimeSpan timeDiff;

            // Mission parameters
            double pressureSP = 5.0;    // dBar

            Debug.Print("Setting the RTC to 2013Jan01 00:00:00");
            DateTime timeNow = new DateTime(2013, 1, 1, 0, 0, 0);
            RealTimeClock.SetTime(timeNow);
 
            //Log variables
            double[] pressureDBarLog = new double[5000];
            double[] vLog = new double[5000];
            double[] uLog = new double[5000];
            double[] ILog = new double[5000];
            double[] y1Log = new double[5000];
            double[] y2Log = new double[5000];
            string loopTimeLog = "";


            //Setup the DGH RS232 port for the pressure transducer
            SerialPort DGHPort = new SerialPort("COM1", 38400, Parity.None, 8, StopBits.One);
            DGHPort.ReadTimeout = 0;
            DGHPort.Open();
            DGHPort.Flush();
            DGHTxBytes = Encoding.UTF8.GetBytes(DGHRead);

            //Setup the Elmo RS232 port for motor control
            SerialPort ElmoPort = new SerialPort("COM2", 19200, Parity.None, 8, StopBits.One);
            ElmoPort.ReadTimeout = 0;
            ElmoPort.Open();
            ElmoPort.Flush();

            Debug.Print("Stopping the BE Motor");
            //Setup the Elmo
            //Stop the Elmo
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoStop);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Elmo Motor off
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Set velocity mode
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoVelocityMode);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Set jog velocity to 0
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJogVelocity + "0;");
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Turn Motor on
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOn);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Begin motion
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoBegin);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);

            //Setup the Console port for commands in and data out
            SerialPort ConsolePort = new SerialPort("COM3", 19200, Parity.None, 8, StopBits.One);
            ConsolePort.ReadTimeout = 0;
            ConsolePort.Open();
            ConsolePort.Flush();
            //Write welcome string
            consoleTxBytes = Encoding.UTF8.GetBytes("\r\nBG to start; UP to upload data\r\n");
            ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length);
            Thread.Sleep(1);

            Debug.Print("Waiting for BG (or bg) command");
            //wait for console begin command
            while(waitForBG)
            {
                while (consoleBytesRead < 2)
                {
                    consoleReadCount = ConsolePort.Read(consoleRxBytes, 0, 1);
                    if (consoleReadCount > 0)
                    {
                        consoleRxByteArray[consoleBytesRead] = consoleRxBytes[0];
                        consoleBytesRead = consoleBytesRead + 1;
                    }
                    else
                        Thread.Sleep(1);
                }
                if ( consoleRxByteArray[0] == byteB && consoleRxByteArray[1] == byteG )
                    waitForBG = false;
                else if ( consoleRxByteArray[0] == byteb && consoleRxByteArray[1] == byteg )
                    waitForBG = false;
                else
                {
                    consoleReadCount = 0;
                    consoleBytesRead = 0;
                }
            }
 
            consoleTxBytes = Encoding.UTF8.GetBytes("Starting BE PID loop\r\n");
            ConsolePort.Write(consoleTxBytes, 0, consoleTxBytes.Length);

            Debug.Print("Starting the BE PID loop");
            //Start the depth control loop
            while (loopNum < 1000)
            {
                loopTime = DateTime.Now;
                
                //Send the DGH read command
                DGHPort.Write(DGHTxBytes, 0, DGHTxBytes.Length);

                Debug.Print("Waiting for DGH pressure value");
                // read the data, should terminated with a CR
                DGHBytesRead = 0;
                while (DGHBytesRead < 12)
                {
                    DGHReadCount = DGHPort.Read(DGHRxBytes, 0, 1);
                    if (DGHReadCount > 0)
                    {
                        DGHRxByteArray[DGHBytesRead] = DGHRxBytes[0];
                        DGHBytesRead = DGHBytesRead + 1;
                    }
                    else
                        Thread.Sleep(1);
                }

                //Get rid of non-characters in DGH byte array
                for (int i = 2; i < 11; i++)
                {
                    strippedDGHRxByteArray[i - 2] = DGHRxByteArray[i];
                }
                
                //Convert to bar (pressure)
                DGHRxCharArray = Encoding.UTF8.GetChars(strippedDGHRxByteArray);
                DGHRxString = new string(DGHRxCharArray);
                pressureDBar = Convert.ToDouble(DGHRxString);
                pressureDBar = (pressureDBar - maOffset) * ma2bar;
                
                //PID Algorithm
                y2 =(p1 * y2) + (p2 * (pressureDBar - y1));                    // update filter state x2
                y1 = y1 + y2;                                       // update filter state x1
                
                v = K * ((B * pressureSP) - y1) - (p4 * y2) + I;    // compute nominal output
                
                if (v < uLow)                                       // limit the nominal output to desired range
                    u = uLow;
                else if (v > uHigh)
                    u = uHigh;
                else
                    u = v;
                
                I = I + (p3 * (pressureSP - y1)) + (p5 * (u - v));     // update the integral

                 Debug.Print("Sending new speed to Elmo");
                //Send the new velocity to the Elmo
                uCPS = u * RPM2CPS;
                ElmoJVString = ElmoJogVelocity + uCPS.ToString("F0") + ";";
                ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJVString);
                ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
                ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoBegin);
                //ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);

                pressureDBarLog[loopNum] = pressureDBar;
                y2Log[loopNum] = y2;
                y1Log[loopNum] = y1;
                vLog[loopNum] = v;
                uLog[loopNum] = u;
                ILog[loopNum] = I;
                               
                Debug.Print("Loop time = " + loopTime.ToString() + "." + loopTime.Millisecond.ToString());
                Debug.Print("Loop Number = " + loopNum.ToString());
                Debug.Print("Pressure (dBar)= " + pressureDBar.ToString());
                Debug.Print("Y2 = " + y2.ToString());
                Debug.Print("Y1 = " + y1.ToString());
                Debug.Print("v (RPM) = " + v.ToString());
                Debug.Print("u (RPM) = " + u.ToString());
                Debug.Print("u (counts per second) = " + uCPS.ToString());
                Debug.Print("I = " + I.ToString());
                Debug.Print("");

                //Wait for next cycle time
                do
                {
                    Debug.Print("Waiting for end of 0.1 second cycle");
                    timeNow = DateTime.Now;
                    timeDiff = timeNow.Subtract(loopTime);
                } while (timeDiff.Milliseconds < 100);
                
                loopNum++;
            }

            //Stop the Elmo
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoStop);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Elmo Motor off
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoMotorOff);
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);
            //Set jog velocity to 0
            ElmoTxBytes = Encoding.UTF8.GetBytes(ElmoJogVelocity + "0;");
            ElmoPort.Write(ElmoTxBytes, 0, ElmoTxBytes.Length);
            Thread.Sleep(1);

        }
    }
}