/*-------------------------------------------- DYNAMIC MODEL -----
|  Class: DynamicModel
|
|  Author: Pierre Baudin
|
|  Purpose: To determine expected pressure on CPF transducer. 
|        Dynamic model calculates expected depth based on fluid displaced 
|        from top to bottom ballast
|
|  Parameters:
|       uCPS: Commanded hydraulic pump cycles from PID control loop
|	    velocity: Calculated CPF velocity, used in drag calculation
|
|  Outcome: 
|       Sends Pressure Command to main CPFtest System  
|   
    *-------------------------------------------------------------------*/

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;


namespace CPFConsole
{
    class DynamicModel
    {
        public double encoderCountsPerRevolution = 500.0;
        public double motorInterpolation = 4.0;
        public double pumpDisplacement = 0.692 * Math.Pow(10, -6);
        public double pumpEfficiency = 0.4;

        private readonly Form1 form;
        private static double weightIntegration = 0;
        private static double velocityFromIntegral = 0;
        private static double positionFromIntegral = -1;
        private static double acceleration = 0;
        private static double netForce = 0;


        private static double dragForce = 0;
        private static double buoyancyLoss = 0;

        public double targetDepth;




        //Constructor:
        public DynamicModel(Form1 form)
        {
            this.form = form;
        }

        public void CalculateBuoyantForce()
        {


            targetDepth = Convert.ToDouble(form.anchorDepth);

            if (form.PID_IsOn)
            {
                if (Regex.IsMatch(form.uCPS_Value, @"\d"))
                {
                    double uCPS = (Convert.ToDouble(form.uCPS_Value));
                    var weightDisplaced = weightDisplacedPerSecond(uCPS) / 2.0;
                    weightIntegration += weightDisplaced;
                }

                dragForce = dragForceCalculation(velocityFromIntegral);
                buoyancyLoss = buoyancyLossCompression(positionFromIntegral);

                var massFactor = 1;
                var systemMass = 40.0 * massFactor; // kg

                netForce = (weightIntegration - dragForce);


                acceleration = netForce / systemMass;
                velocityFromIntegral += acceleration / 2.0;


            }



            if ((Math.Abs(positionFromIntegral) >= targetDepth) && !(form.CPFState_Value == "Deanchor"))//target depth reached. Trigger entry to Anchor state
            {
                velocityFromIntegral = 0;
                weightIntegration = 0;
                acceleration = 0;
            }

            if ((positionFromIntegral >= 0))//Surface Reached
            {
                velocityFromIntegral = 0;
                weightIntegration = 0;
                acceleration = 0;
                positionFromIntegral = -1;
            }
            if (form.CPFState_Value == "DeAnchor")
                velocityFromIntegral = 0.07;

            positionFromIntegral += velocityFromIntegral / 2.0;
            // else if (positionFromIntegral >= 0)//CPF has resurfaced
            //    velocity = 0;


            form.modelAcceleration = Convert.ToString(Math.Round(acceleration, 4));
            form.modelVelocity = Convert.ToString(Math.Round(velocityFromIntegral, 4));
            form.modelDepth = Convert.ToString(Math.Round(positionFromIntegral, 4));
            form.commandDepth = Convert.ToString(Math.Abs(Math.Round(positionFromIntegral, 4)));
        }
        // following 2 functions not used
        //private double descentVelocity(double CPS)
        //{
        //    return (CPS+34783)/(-174726);
        //}

        //private double ascentVelocity(double CPS)
        //{
        //    return (CPS-1752)/(-19216);
        //}

        private double weightDisplacedPerSecond(double CPS)
        {


            var countsPerRevolution = encoderCountsPerRevolution * motorInterpolation;


            var seaWaterWeightDensity = 10050.0; // N/m^3


            return (CPS / countsPerRevolution) * pumpEfficiency * seaWaterWeightDensity * pumpDisplacement;
        }

        //Buoyancy lost due to compression at depth
        private double buoyancyLossCompression(double pressure)
        {
            var volumeLoss = 2.29 * Math.Pow(10, -5); //(m^3/m) bottom meter unit is really pressure due to meter of depth
            var seaWaterWeightDensity = 10050.0; // N/m^3

            return pressure * volumeLoss * seaWaterWeightDensity;
        }

        //Drag Force
        private double dragForceCalculation(double velocity)
        {
            var Cd = 1.1;
            var diameter = 0.36; //meters
            var radius = diameter / 2.0;
            var seaWaterMassDensity = 1026.0; // kg/m^3

            var dragConstants = Cd * Math.Pow(radius, 2) * Math.PI * seaWaterMassDensity * 0.5;

            return Math.Sign(velocity) * Math.Pow(velocity, 2) * dragConstants;
        }
    }
}