#!/usr/bin/python

#
#   Get sequence of focussing images and
#   compute the best focus value
#
#   This variant of the script uses Eddy Emelianov's
#   'fli_control' executable
#


import OBSUTILS as obsutil
import argparse as ap
import sys
import numpy as np
import subprocess as sp


# --- FLI-hardware related (ROBOTEL variant)


def set_focus(foc_val):
    if foc_val < 0:
        print("INVALID FLI-FOCUSER VALUE! IT MUST BE NON-NEGATIVE VALUE!")
        return -1
    cmd = ["fli_control", "-g", str(int(foc_val))]
    ret = sp.run(cmd, stdout=sp.PIPE, stderr=sp.PIPE)
    return ret.returncode


def get_image(filename, exp_time):
    #
    # Eddy's 'fli_control' related stub:
    #     filename is expected in form 'rootname_DDDD.ext'
    #     convert it to 'rootname'
    #

    fname = str(filename).split("_")
    fname = fname[0]

    cmd = [
        "fli_control",
        "-r",
        "/tmp/10micron.fitsheader",
        "-x",
        "{:d}".format(np.round(exp_time * 1000)),  # to microseconds
        fname,
    ]

    ret = sp.run(cmd, stdout=sp.PIPE, stderr=sp.PIPE)
    return ret.returncode


if __name__ == "__main__":
    parser = ap.ArgumentParser(
        prog=sys.argv[0],
        description="FLI CCD and focuser hardware: focussing sequence implementation. It is assumed that 'fli_control' software is installed in the OS.",
    )

    parser.add_argument("focus_start", help="focus start value", type=float)
    parser.add_argument("focus_stop", help="focus stop value", type=float)
    parser.add_argument(
        "focus_step", help="focus step value", type=float, nargs="?", default=np.nan
    )

    parser.add_argument(
        "--guess",
        help="Use of two-steps focussing strategy. Input focus start and stop values are interpretated"
        + "as a guess range while step value is one for precise measurements.",
        action="store_true",
        default=False,
    )

    parser.add_argument(
        "-N",
        "--num",
        help="Number of points for precise stage measurement. Default is 7. Ignored if no '--guess'.",
        type=np.uint64,
        default=7,
    )

    parser.add_argument(
        "-v", "--verbose", help="Verbose output", action="store_true", default=False
    )

    parser = obsutil.getFocussingSequenceCmdlinePars(parser)

    args = parser.parse_args()

    if (args.num < 3) and (args.guess):
        if args.verbose:
            print("Number of focus values must be greater than 2! Abort!")
        sys.exit(-1)

    focus_start = float(args.focus_start)
    focus_stop = float(args.focus_stop)
    focus_step = float(args.focus_step)

    if np.isfinite(focus_step):
        if np.isclose(focus_step, 0.0):
            if args.verbose:
                print("Invalid (zero) focus step value! Abort!")
            sys.exit(1)

        if np.isclose(focus_start, focus_stop):
            if args.verbose:
                print("Invalid focus range parameters (an empty range)! Abort!")
            sys.exit(1)

        if (focus_stop - focus_start) * focus_step < 0:
            if args.verbose:
                print("Invalid focus range parameters! Abort!")
            sys.exit(1)

        # if (focus_start > focus_stop) and (focus_step > 0):
        #     if args.verbose:
        #         print("Invalid focus range parameters! Abort!")
        #     sys.exit(1)

        # if (focus_start < focus_stop) and (focus_step < 0):
        #     if args.verbose:
        #         print("Invalid focus range parameters! Abort!")
        #     sys.exit(1)
    else:
        if args.guess:
            if args.verbose:
                print("If '--guess' is set then step value must be given! Abort!")
            exit(1)

    log = []

    seq_kwds = obsutil.parsFocussingSequenceCmdlinePars(args, log)

    if args.verbose:
        if len(log):
            for log_str in log:
                print(log_str)

    if args.verbose:
        print("START FOCUSSING SEQUENCE ...")

    if args.guess:  # rough focus estimation
        if args.verbose:
            print("\trough focus estimation ...")
        result = obsutil.focussingSequence(
            [focus_start, focus_stop],
            set_focus,
            get_image,
            None,
            sys.stdout,
            **seq_kwds
        )

        if result["ret_code"] != 0:
            print("\n\tERROR: Cannot perform rought focus estimation! Abort!")
            print("\nFAIL!")
            sys.exit(result["ret_code"])

        # compute precise range start and stop values
        r = args.num * focus_step / 2.0
        focus_start = result["focus_value"] - r
        focus_stop = result["focus_value"] + r

        if args.verbose:
            print(
                "\testimate guess focus position in range [{:g},{:g}]".format(
                    focus_start, focus_stop
                )
            )

    if np.isfinite(args.focus_step):
        result = obsutil.focussingSequence(
            [focus_start, focus_stop, focus_step],
            set_focus,
            get_image,
            None,
            sys.stdout,
            **seq_kwds
        )
    else:
        result = obsutil.focussingSequence(
            [focus_start, focus_stop],
            set_focus,
            get_image,
            None,
            sys.stdout,
            **seq_kwds
        )

    if args.verbose:
        print("\tThe best focus value: {:g}".format(result["focus_value"]))

    if not args.do_not_set:
        if args.verbose:
            print("\n\tSet focus to the best value ...", end="")
            set_focus(result["focus_value"])
            print("\tOK")

    if args.verbose:
        print("DONE.")

    sys.exit(0)