using System;

using System.Linq;

public class Program

{

    public static void Main()

    {

        Console.WriteLine("t_offset,p_offset,t,p,z,offsetBflMm,offsetBflMicrons,offsetBflSteps,result");

        CalculateFocusOffsetInSteps(null, null, 20, 1, 0);

        CalculateFocusOffsetInSteps(null, null, 20, 1, 5000);

        CalculateFocusOffsetInSteps(null, null, 20, 1, 10000);

        CalculateFocusOffsetInSteps(null, null, 20, 1, 15000);

        CalculateFocusOffsetInSteps(null, null, 20, 1, 20000);

        CalculateFocusOffsetInSteps(19, null, 20, 1, 0);

        CalculateFocusOffsetInSteps(19, null, 20, 1, 5000);

        CalculateFocusOffsetInSteps(19, null, 20, 1, 10000);

        CalculateFocusOffsetInSteps(19, null, 20, 1, 15000);

        CalculateFocusOffsetInSteps(19, null, 20, 1, 20000);

        CalculateFocusOffsetInSteps(null, 0.95, 20, 1, 0);

        CalculateFocusOffsetInSteps(null, 0.95, 20, 1, 5000);

        CalculateFocusOffsetInSteps(null, 0.95, 20, 1, 10000);

        CalculateFocusOffsetInSteps(null, 0.95, 20, 1, 15000);

        CalculateFocusOffsetInSteps(null, 0.95, 20, 1, 20000);

        CalculateFocusOffsetInSteps(19, 0.95, 20, 1, 0);

        CalculateFocusOffsetInSteps(19, 0.95, 20, 1, 5000);

        CalculateFocusOffsetInSteps(19, 0.95, 20, 1, 10000);

        CalculateFocusOffsetInSteps(19, 0.95, 20, 1, 15000);

        CalculateFocusOffsetInSteps(19, 0.95, 20, 1, 20000);

    }

    internal static int CalculateFocusOffsetInSteps(double? labTemperature, double? labPressure, double t, double p, double z)

    {

        const double standardTemperature = 20;

        const double standardPressure = 1;

        // If lab temperature or pressure is not null then calculate the offsets to apply, otherwise we use 0

        var t_offset = standardTemperature - labTemperature ?? 0;

        var p_offset = labPressure - standardPressure ?? 0;

        var offsetBflMm = PerformCalculation(t + t_offset, p + p_offset, z);

        var offsetBflMicrons = 1000 * offsetBflMm;

        // 1 encoder step per 0.375 microns of point of best focus movement

        var offsetBflSteps = offsetBflMicrons / 0.375;

        var result = Convert.ToInt32(-1 * offsetBflSteps);

        Console.WriteLine($"{t_offset}, {p_offset}, {t}, {p}, {z}, {offsetBflMm}, {offsetBflMicrons}, {offsetBflSteps}, {result}");

        return result;

    }

​

    private static double PerformCalculation(double t, double p, double z)

    {

        double z1 = 7.64996491070144E-18;

        double z2 = -4.01789696458072E-13;

        double z3 = 7.69869874512883E-09;

        double z4 = -0.0000655644999700243;

        double p1 = 0.505327516724414;

        double t1 = 0.0142788606011759;

        double tp = -0.00156678499023222;

        double c = -0.513537762335459;

        return

            z1 * Math.Pow(z, 4) +

            z2 * Math.Pow(z, 3) +

            z3 * Math.Pow(z, 2) +

            z4 * z +

            p1 * p +

            t1 * t +

            tp * t * p +

            c;

    }

}