# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/testOB_tcs_command.py 3808 2022-01-25 12:43:04Z depa $

import sys
import Ice
from util.shinsLogger import logger
from util import exception
from util import variables as uvars
from util import functions as ufun
from shinsapp import SHINSApp
import Basda

# cp /home/shinsmgr/shins/src/scripts/templates/tpl/testOB_tcs_command.py /opt/tan/lib/python/site-packages/shins/templates/tpl/

class testOB_tcs_command(SHINSApp):

    @exception.handler_for_run
    def run(self, ins_s=None, det_s=None, tel_s=None, rtc_s=None, **kwargs):
        '''Try some tcs command
        '''
        logger.info(__name__)

        self.status = uvars.EXIT_SUCCESS
            
        obsController = self.obsController
        TEST = 5
        
        if TEST == 1:
            ###############################################
            ############### OFFSET POINTING ###############
            ###############################################
            tel_s = {"OFFSETY":"0.017453292",        # in radians
                     "OFFSETX":"0.0",                # in radians
                     "CoordinateSystem":"RADEC",     # "RADEC", "AZALT", "DETXY", "GALACTIC" ("AZALT" not supported on our TCS sim)
                     "MoveType":"REL",               # "REL", "RELCS", "ABS", "ABSCS"
                     "TelescopeSide":"LEFT"} 

            print("Start Offset Pointing ....")
            # asyncres = obsController.begin_OffsetPointing(tel_s)
            # status = obsController.end_OffsetPointing(asyncres)
            self.status = obsController.OffsetPointing(tel_s)
            ufun.error_handler(self.status, "OffsetPointing")

        # #############################################
        # ############## ApplyWaffle AO################
        # #############################################

        # modes = ["NORMAL", "REPOINT", "SKIPREF", "SKIPSTART", "SKIPSTARTREPOINT", "SKIPALL"]

        # for mode in modes:
        #     tel_s = {
        #         "AOMODE": mode,
        #         "TelescopeSide":"LEFT"
        #     }

        #     try:
        #         logger.info("Start Waffle in mode: {}".format(mode))
        #         asyncres = obsController.begin_ApplyWaffleAO(tel_s)
        #         status = obsController.end_ApplyWaffleAO(asyncres)
        #         print(status)
        #     except:
        #         logger.error("Error trying mode {}, but trying next one".format(mode))
        #         pass
        
        
        # ###############################################
        # ############# Binocular Control ###############
        # ###############################################
        # tel_s = "ciccio"
        # print("Start Binocular Control")
        # status = obsController.begin_BinocularControl(tel_s)
        # status = obsController.end_BinocularControl(status)
        # ufun.error_handler(self.status, "BinocularControl")
        # print(status)
        
        # #############################################
        # # ############# RUN AO2 #####################
        # ############################################

        # modes = ["NORMAL", "REPOINT", "SKIPREF", "SKIPSTART", "SKIPSTARTREPOINT", "SKIPALL"]

        # for mode in modes:
        #     tel_s = {
        #         "AOMODE": mode,
        #         "TelescopeSide":"LEFT"
        #     }

        #     try:
        #         logger.info("Start Run AO2 in mode: {}".format(mode))
        #         asyncres = obsController.begin_RunAO2(tel_s)
        #         status = obsController.end_RunAO2(asyncres)
        #         print(status)
        #     except:
        #         logger.error("Error trying mode {}, but trying next one".format(mode))
        #         pass

            
        # #######################################################
        # # ############## Preset Telescope No AO2  ########
        # #######################################################
        # tel_s = {"OBJRA":"0", "OBJDEC":"0",
        #          "PMRA":"0", "PMDEC":"0",
        #          "GSRA":"0", "GSDEC":"0",
        #          "PMGSRA":"0", "PMGSDEC":"0",
        #          "CoordinateSystem":"RADEC",
        #          "Epoch":"2018.3",
        #          "Equinox":"J2000",
        #          "OBJEQUIN":"2000",
        #          "OBJECTNAME":"41 Dra",
        #          "TelescopeMode":"STATIC", # he wants BOTH, but only LEFT is allowed
        #          "TelescopeSide":"LEFT"}  # status =1
        
        # print("Start Preset Telescope No AO2")
        # status = obsController.begin_PresetTelescopeNoAO2(tel_s)
        # status = obsController.end_PresetTelescopeNoAO2(status)
        # print(status)                 
        
        
        # ins_s = {
        #     "DROT_Mode" : "FIELD"
        # }

                
        # #########################
        # # WAIT INSTRUMENT SETUP #
        # #########################
            
        # logger.info("Waiting Instrument setup to complete...")
        # self.status = obsController.setupInstrument(ins_s)
        # ufun.error_handler(self.status, "setupInstrument")
        # logger.info("Instrument setup... DONE")            

        
        if TEST == 5:
            #######################################################
            ############## Preset Telescope No AO2 Start ########
            #######################################################

            tel_s = {
                "CoordinateSystem":"RADEC",
                "Epoch":"2022.5",
                "Equinox": "j2000",
                "OBJEQUIN":"2000",

                # "OBJRA":"0.41",
                # "GSRA":"0.402",
                # "AORA":"0.43",
                # "OBJDEC":"1.51",
                # "GSDEC":"1.502",
                # "AODEC":"1.53",

                # "OBJECTNAME":"M31",
                # "OBJRA":"310.3579",
                # "OBJDEC":"45.2803",

                "PMRA":"0",
                "PMDEC":"0",
                "PMGSRA":"0",
                "PMGSDEC":"0",
                "PMAORA":"0",
                "PMAODEC":"0",
                "BinocularFlag":"OFF",
                #"TelescopeMode": "TRACK",
                "TelescopeMode": "ADAPTIVEACE_TRACK",
                "TelescopeSide": "LEFT",
                "AOMODE":"REPOINT"
            }

            tel_s["OBJECTNAME"] = "Deneb"
            objra_degree = 15*(20 + 41/60. + 27.41/3600.)
            tel_s["OBJRA"] = str(objra_degree*3.141592653/180)
            objdec_degree = 45 + 16/60. + 40.4/3600.
            tel_s["OBJDEC"] = str(objdec_degree*3.141592653/180)
            
            tel_s["OBJECTNAME"] = "M31"
            objra_degree = 15*(0 + 42/60. + 44.05/3600.)
            tel_s["OBJRA"] = str(objra_degree*3.141592653/180)
            objdec_degree = 41 + 15/60. + 54.5/3600.
            tel_s["OBJDEC"] = str(objdec_degree*3.141592653/180)

            logger.info("Running: PresetTelescope")
            status = obsController.PresetTelescope(tel_s)
            logger.info("PresetTelescope returned : %d" % status)

            # logger.info("Running: trackingStartHere")
            # status = obsController.trackingStartHere()
            # logger.info("trackingStartHere returned : %d" % status)

        
        # #######################################################
        # # ############## Preset Telescope With AO ########
        # #######################################################
        # tel_s = ('41 Drac', '', '', 'STATIC', 'REPOINT')
        
        # logger.info("Start preset telescope with AO")
        # asyncres = obsController.begin_PresetTelescopeWithAO(tel_s)
        # status = obsController.end_PresetTelescopeWithAO(asyncres)
        # print(status)
