# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/obs_ctrl/py/obsControlClient.py 473 2018-03-13 17:03:13Z 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 Nice

if __name__ == "__main__":

    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"  : "Close",
        # "INBEAM_TT-DUMMY" : Nice.Point(npr.randint(0, 2475804), npr.randint(0, 2475804)),
        # "SHUTTER-DUMMY"     : args.shutter,
        "APO_W-DUMMY"       : "SP_AS_X_P1",
        "ND_W-DUMMY"        : "Filter 8 blank",
        "CORO_W-DUMMY"      : "APLC_J_F",
        "LYOT_W-DUMMY"      : "APLC_H_P2",
        "SCI1_W-DUMMY"      : "BB_J",
        "SCI2_W-DUMMY"      : "NB_ContJ",
        "LENS_DEP-DUMMY"    : "Close",
        "DB_W-DUMMY"        : "H2-H3"
    }


    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("SimpleObsControlAdapter: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("end_initMotion returned {}".format(st))
        def ex_cb(ex):
            print("Exception!")

        # obsController.begin_initMotion(resp_cb, ex_cb)
        # asynRes = obsController.begin_initMotion()
        # status = obsController.end_initMotion(asynRes)
        # print ("end_initMotion returned {}".format(status))
        # status = obsController.setupMotionDict(dictSetupTest)
        # status = obsController.instrumentSetup(dictSetupTest2)

        # obsController.emergencyAbort()

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

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

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

        dit = 1
        ndit = 2
        FITSname = ""


        status = obsController.scicamExpose(4,3)
        time.sleep(0.1)
        status = obsController.scicamSaveFITS()
        time.sleep(0.1)
        status, FITSname = obsController.scicamGetFITSname()
        print ("The exposure file name is {}".format(FITSname))
    except:
        traceback.print_exc()
        status = 1

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

    sys.exit(status)
