# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/obs_ctrl/obsControlClient.py 578 2018-07-31 07:17:38Z depa $

# obsControlClient is actually the Sequencer component.

import sys, traceback, Ice
import argparse
import sharknir
import lbto
import numpy.random as npr
import time
import timer
import Nice

if __name__ == "__main__":
    T = timer.timer()
    T.begin()
    parser = argparse.ArgumentParser()
    parser.add_argument("--inbeam_dep"  , type=int,
                        help="Commanded position for input beam deployable arm (in steps)")
    parser.add_argument("--inbeam_tt_x" , type=int,
                        help="Commanded X position for input beam \"tip/tilt\" mirror (in steps)")
    parser.add_argument("--inbeam_tt_y" , type=int,
                        help="Commanded Y position for input beam \"tip/tilt\" mirror (in steps)")
    parser.add_argument("--shutter"     , type=int,
                        help="Commanded X position for entrance shutter (in steps)")
    #parser.add_argument("adc1", type=int, help="...")
    #parser.add_argument("adc2", type=int, help="...")
    parser.add_argument("--apo"         , type=int,
                        help="Commanded named position for Apodizer wheel")
    parser.add_argument("--nd"          , type=int,
                        help="Commanded named position for  wheel ND wheel")
    parser.add_argument("--coro"        , type=int,
                        help="Commanded named position for Coro-Slit wheel")
    parser.add_argument("--lyot"        , type=int,
                        help="Commanded named position for Lyot-Grism wheel")
    parser.add_argument("--sci1"        , type=int,
                        help="Commanded named position for Scientific wheel 1")
    parser.add_argument("--sci2"        , type=int,
                        help="Commanded named position for Scientific wheel 2")
    parser.add_argument("--lens_dep"    , type=int,
                        help="Commanded named position for pupil lens deployer")
    parser.add_argument("--db"          , type=int,
                        help="Commanded named position for Dual Band wheel")
    #parser.add_argument("derot", type=int, help="...")
    # NOTE: all missing devices will be added once code to control them had been developedcrosso.pl/en/cyclling-tips/what-take/

    args = parser.parse_args()

    status = 0
    ic = None # Ice Communicator

    dictNonSci = {
        1 : sharknir.NonSciAssembly("Direct Imaging Field Stabilized", 1, 1, 1, True),
        2 : sharknir.NonSciAssembly("Direct Imaging Pupil Stabilized", 1, 1, 1, False),
        3 : sharknir.NonSciAssembly("Gaussian Lyot Coronograph", 1, 2, 2, True)
    }

    # target = lbto.position2()
    # guide = lbto.position2()

    dictSetup = {
        "INBEAM_DEP"  : args.inbeam_dep,
        # "INBEAM_TT_X" : args.inbeam_tt_x,
        # "INBEAM_TT_Y" : args.inbeam_tt_y,
        # "SHUTTER"     : args.shutter,
        "APO_W"       : args.apo,
        "ND_W"        : args.nd,
        "CORO_W"      : args.coro,
        "LYOT_W"      : args.lyot,
        "SCI1_W"      : args.sci1,
        "SCI2_W"      : args.sci2,
        # "LENS_DEP"    : args.lens_dep,
        "DB_W"        : args.db
    }

    dictSetupTest = {
        "INBEAM_DEP-DUMMY"  : npr.randint(-4292060, 4292060),
        # "INBEAM_TT-DUMMY" : Nice.Point(npr.randint(0, 2475804), npr.randint(0, 2475804)),
        # "SHUTTER-DUMMY"     : args.shutter,
        # "APO_W-DUMMY"       : npr.randint(0,1985482),
        # "ND_W-DUMMY"        : npr.randint(0,1985482),
        # "CORO_W-DUMMY"      : npr.randint(0,1985482),
        # "LYOT_W-DUMMY"      : npr.randint(0,1985482),
        # "SCI1_W-DUMMY"      : npr.randint(0,1985482),
        # "SCI2_W-DUMMY"      : npr.randint(0,1985482),
        "LENS_DEP-DUMMY"    : 2639342,# 675053 npr.randint(0, 3605244),
        # "DB_W-DUMMY"        : npr.randint(0,1985482)
    }

    dictSetupTest2 = {
        "INBEAM_DEP-DUMMY"   : "IN",
        # "INBEAM_TT-DUMMY"  : Nice.Point(npr.randint(0, 2475804), npr.randint(0, 2475804)),
        # "SHUTTER-DUMMY"    : args.shutter,
        "APODIZER_W-DUMMY"   : "SP_AS_X_P1",
        "ND_FILT_W-DUMMY"    : "Filter 7 blank",
        "CORO_SLIT_W-DUMMY"  : "GAUSS_F",
        "LYOT_GRISM_W-DUMMY" : "APLC_H_P2",
        "SCI_FILT_W1-DUMMY"  : "BB_J",
        "SCI_FILT_W2-DUMMY"  : "NB_ContJ",
        "PUPIL_LENS-DUMMY"   : "IN",
        "DB_FILT_W-DUMMY"    : "ContJ-Pa Beta"
    }


    dictCalibUnitSetupTest = {
        "FIBER_DEP-DUMMY"      : npr.randint(-3640883, 0)#,
        # "CAL_MIRROR_DEP-DUMMY" : npr.randint(-3640883, 0)
    }

    # for key in dictSetup.keys():
    #     if dictSetup[key] != None:
    #         dictSetupTest[key] = dictSetup[key];

    # print(dictSetupTest)
    # print(dictCalibUnitSetupTest)


    strSep = " "
    strSetupMotion = "APO_W={}".format(args.apo) +\
                     strSep +\
                     "ND_W={}".format(args.nd) +\
                     strSep +\
                     "CORO_W={}".format(args.coro) +\
                     strSep +\
                     "LYOT_W={}".format(args.lyot) +\
                     strSep +\
                     "SCI1_W={}".format(args.sci1) +\
                     strSep +\
                     "SCI2_W={}".format(args.sci2) +\
                     strSep +\
                     "DB_W={}".format(args.db)
    # NOTE: all missing devices will be added once code to control them had been developed
    strSetupTracking = ""
    strSetupCalibration = ""



    try:
        ic = Ice.initialize(sys.argv)
        base = ic.stringToProxy("ObsControlAdapterSeq:default -p 10500")
        obsController = sharknir.obsControl.SequencerPrx.checkedCast(base)
        if not obsController:
            raise RuntimeError("Invalid Proxy")

        # status = obsController.setupMotion(dictNonSci, strSetupMotion)
        # print "Wheel configuration string is: {:s}".format(strSetupMotion)
        # status = obsController.setupCalibUnitDict(dictCalibUnitSetupTest)
        # status = obsController.initMotion()
        def resp_cb(st):
            print("Response callback {}".format(st))

        def ex_cb(ex):
            print("Exception is {}".format(ex))

        # asyncRes = obsController.begin_instrumentSetup(dictSetupTest2, resp_cb, ex_cb)
        asyncRes = obsController.begin_setupInstrument(dictSetupTest2)


        print ("Waiting for intrument setup")


        status = obsController.end_setupInstrument(asyncRes)
        print ("end_initMotion returned {}".format(status))
        # status = obsController.setupMotionDict(dictSetupTest)
        # status = obsController.instrumentSetup(dictSetupTest 2)

        # obsController.emergencyAbort()

        # ++++ TCS ++++

        if not(status):
            print (status)
            target    = lbto.position2()
            guide     = lbto.position2()
            AOreference = lbto.position2()
            telescopeMode = "ADAPTIVEACE_TRACK" # ADAPTIVETTM_TRACK, ADAPTIVEACE_TRACK, ADAPTIVEICE_TRACK
            AOmode = "REPOINT" # "NORMAL"
            pathToSubNode = "ltcs"

            binocularControlFlag = "SYNCPRESET"
            status = obsController.BinocularControl(binocularControlFlag)
            print("BinocularControl issued {}".format(status))

            obsController.PresetTelescopeWithAO(target, guide, AOreference, telescopeMode, AOmode);

        # ++++ SCICAM ++++

        dit = 1
        ndit = 2
        FITSname = ""

        dictScicam = {
            "DIT": "4.3",
            "NDIT": str(npr.randint(3600))
        }

        asyncRes = obsController.begin_scicamSetup(dictScicam)
        status = obsController.end_scicamSetup(asyncRes)
        if not(status):
            print ("Scientific camera successfully setup!")
            time.sleep(0.1)
            asyncRes = obsController.begin_scicamSaveFITS()
            status = obsController.end_scicamSaveFITS(asyncRes)
            if not(status):
                print ("Scientific camera set to save to FITS file")
                # time.sleep(0.1)
                asyncRes = obsController.begin_scicamExpose()
                status = obsController.end_scicamExpose(asyncRes)
                if not(status):
                    print ("Success to expose scientific camera")
                    # time.sleep(0.1)
                    asyncRes = obsController.begin_scicamGetFITSname()
                    status, FITSname = obsController.end_scicamGetFITSname(asyncRes)
                    if not(status):
                        print ("The exposure file name is {}".format(FITSname))
                    else:
                        print ("Failed to get filename from scientific camera")
                else:
                    print ("Scientific camera exposure failed")
            else:
                print ("Failed to set scientific camera to save to FITS file!")
        else:
            print("Scientific camera setup failed!")
        print ("Elapsed time {}".format(T.elapsed()))
    except:
        traceback.print_exc()
        status = 1

    if ic:
    # Clean up
        try:
            ic.destroy()
        except:
            traceback.print_exc()
            status = 1

    sys.exit(status)
