#
#   Focusing application skeleton
#


import OBSUTILS as obsutil
import sys
import numpy as np


def focussing_app(parser, set_focus, get_image):
    """
    Focussing sequence standard application skeleton

        parser - argparse.ArgumentParser
        set_focus_func: a function of signature:
                        set_focus_func(foc_val)
        get_image_func: a function of signature:
                          get_image_func(image_filename, exp_time)
                        exp_time in seconds

        Returns:
            0 - OK
            non-zero - ERROR
    """

    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!")
        return -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!")
            return 1

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

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

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

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

    return 0