# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/SHARKNIR_gen_tiptilt_init.py 3974 2022-06-10 13:01:05Z depa $

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

from SHARKNIR_gen_base import SHARKNIR_gen_base

class SHARKNIR_gen_tec_PD_wollaston(SHINSApp):

    @exception.handler_for_run
    def run(self, ins_s=None, det_s=None, tel_s=None, rtc_s=None, **kwargs):
        '''Calculate reconstructor PD with n images
        '''

        logger.info(__name__)
        logger.info('Test tiptilt system initialize ... ')

        self.status = uvars.EXIT_SUCCESS
        obsController = self.obsController

        self.gen_base = SHARKNIR_gen_base();        gen_base = self.gen_base

        #####################
        # START SASHA SETUP #
        #####################

        gen_base.main(ins_s=ins_s, det_s=det_s, expose=False)

        ##################
        # SHOW RTC SETUP #
        ##################

        logger.debug("Enumerating RTC setup")
        for element, value in rtc_s.items():
            logger.debug("Element : % 15s  -  Value % 15s    " % (element, value))

        # ##########################
        # # Apply RTC shape (flat) #
        # ##########################
        # #        0, 0, tilt, tip, other modes
        # flatten = [ 0 ]*23
        # logger.info("Flattening the DM")
        # self.status = obsController.RTCTTModeUpload(flatten, absolute=True)
        # logger.info("Waiting setup to apply displacement... DONE")

        # ###############################
        # # Call custom template method #
        # ###############################

        self.pd_reconstructor(gen_base)

        ##############
        # SAVE MODES #
        ##############

        modes = [ float(rtc_s["VECTOR_{}".format(i)]) for i in range(1, 39) ]
        base_original = [0]*len(modes)

        logger.warning("Modes: {}".format(modes))

        #######################
        # LOOP FOR EVERY MODE #
        #######################

        for idx, val in enumerate(modes):
            if val != 0:

                ##################################################
                # First we appy positive val, then negative val  #
                ##################################################

                for v in [+val, -val]:

                    logger.info("****************************")
                    logger.info("Applying mode {}, value {}".format(idx, v))

                    ####################
                    # Apply RTC shape  #
                    ####################
                    new_base = list(base_original) # copy
                    new_base[idx] = v
                    
                    logger.info(new_base)

                    self.status = obsController.RTCTTModeUpload(new_base, absolute=True)
                    logger.info("Waiting setup to apply displacement... DONE")

                    ###############################
                    # Call custom template method #
                    ###############################

                    # After applying specific shape
                    self.pd_reconstructor(gen_base)
                    

    def pd_reconstructor(self, gen_base):
        """
        Helper method to be run first with flat mirror, and then in the mode loop
        """
        
        
        # Apo positions are shared between focus and defocus loops
        several_apos = ["HOLE3", "Lens_PD_neg", "Lens_PD_pos"]
    
        ###################################
        # LOOP FOCUS FIBER: SETUP + IMAGE #
        ###################################
    
        several_groups = ["3",  "3",  "3"]
        several_reads =  ["2",  "2",  "1"]
        several_drops =  ["70", "28", "74"]
        several_readouts = ["PD_wollaston_stripe_small", "PD_wollaston_stripe_small", "PD_wollaston_stripe_large"]
    
        logger.info("***Starting Focus fiber loop")
        for apo, group, read, drop, readout in zip(several_apos, several_groups, several_reads, several_drops, several_readouts):
    
            ins_s = {
                "CAL_FIBER_DEP": "FOCUS_IN",
                "CAL_FIBER_FOCUS_LAMP": "ON",
                "CAL_FIBER_DEFOCUS_LAMP": "OFF",
                "APODIZER_W": apo,
            }
    
            det_s = {"NGROUPS": group, 
                     "NREADS": read,
                     "NDROPS": drop,
                     "READOUT": readout,
                     }
    
            logger.info("****Start focus setup and image")
            self.status, self.filename, self.other = gen_base.main(ins_s, det_s)
            logger.info("****End focus setup and image")
    
        #####################################
        # LOOP DEFOCUS FIBER: SETUP + IMAGE #
        #####################################
    
        several_groups = ["3",  "3",  "7"]
        several_reads =  ["1",  "3",  "1"]
        several_drops =  ["6", "35", "11"]
    
        logger.warning("Starting Defocus fiber loop")
        for apo, group, read, drop in zip(several_apos, several_groups, several_reads, several_drops):
    
            ins_s = {
                "CAL_FIBER_DEP": "DEFOCUS_IN",
                "CAL_FIBER_FOCUS_LAMP": "OFF",
                "CAL_FIBER_DEFOCUS_LAMP": "ON",
                "APODIZER_W": apo,
            }
        
            det_s = {"NGROUPS": group, 
                     "NREADS": read,
                     "NDROPS": drop,
                     }
    
            logger.info("****Start defocus setup and image")
            self.status, self.filename, self.other = gen_base.main(ins_s, det_s)
            logger.info("****End defocus setup and image")
