# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/SHARKNIR_gen_cal_spiral_with_pupil.py 3724 2022-03-27 14:26:48Z depa $

from util.shinsLogger import logger
from shinsapp import SHINSApp
from util import variables as uvars
from util import functions as ufun
from util import exception
import numpy as np

from SHARKNIR_gen_base import SHARKNIR_gen_base

class SHARKNIR_gen_cal_spiral_with_pupil(SHINSApp):
    
    @exception.handler_for_run
    def run(self, ins_s=None, det_s=None, tel_s=None, rtc_s=None, **kwargs):
        '''Check that BCU has been initialized, then send specific command
        to CRED2 camera
        '''

        logger.info(__name__)
        obsController = self.obsController
        self.status = uvars.EXIT_SUCCESS

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

        # Setup for focal plane
        
        ins_s_1 = {
            "SCI_FILT_W2" : "ND3",
            "PUPIL_LENS_DEP": "OUT",
        }

        det_s_1 = {
            "READOUT": "128x128",
            "NDROPS": det_s["NDROPS"]
        }

        # Setup for pupil plane

        ins_s_2 = {
            "SCI_FILT_W2" : "HOLE",
            "PUPIL_LENS_DEP": "IN",
        }

        det_s_2 = {
            "READOUT": "Center",
            "NDROPS": "0"
        }


        #############################################
        # START AND WAIT INSTRUMENT AND SASHA SETUP #
        #############################################

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

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

        if "ABSOLUTE" in rtc_s:
            logger.warning("ABSOLUTE keyword found")                
            if (rtc_s["ABSOLUTE"] == "true") : # or rtc_s["ABSOLUTE"]=="0"):
                is_absolute = True       
                logger.warning("Absolute movement")
            else:
                is_absolute = False                
        else:
            is_absolute = False

        ###############
        # SPIRAL LOOP #
        ###############
            
        spiral = ufun.make_spiral(float(rtc_s["STEP"]), float(rtc_s["REPEAT"]))

        to_go = float(rtc_s["REPEAT"])
        
        for tiptilt in spiral:
            
            if self.paragraph("SPIRAL LOOP:"+str(int(to_go))+" to go "): 

                to_go -= 1
                
                logger.info("---------- "+str(int(to_go))+" to go -----------------")
                print(tiptilt)
                                
                #############################
                # APPLY THE DM DISPLACEMENT #
                #############################
                
                modes = [0,0] + tiptilt
                
                attempt = 0
                max_attempts = 3
                while attempt < max_attempts:
                    logger.info("Attempt "+str(int(attempt)))
                    try:
                        logger.info("Upload a mode vector to internal DM")
                        self.status = obsController.RTCTTModeUpload(modes, absolute=is_absolute)
                        logger.info("Waiting setup to apply displacement... DONE")
                        attempt = max_attempts
                    except Exception as e:
                        print(e)
                        attempt += 1
                
            #################
            # Setup + image #
            #################
            
            logger.info("Waiting setup to retrive coro mask center ...")
            self.status, self.filenames, _ = gen_base.main(ins_s=ins_s_1, det_s=det_s_1, **kwargs)
            logger.info("Waiting setup to retrive coro mask center ... DONE")
            
            #################
            # Setup + image #
            #################
            
            logger.info("Waiting setup to retrive coro mask center ...")
            self.status, self.filenames, _ = gen_base.main(ins_s=ins_s_1, det_s=det_s_2, **kwargs)
            logger.info("Waiting setup to retrive coro mask center ... DONE")


        logger.info("End of the loop: this was the spiral:")
        logger.info(str(spiral))
                                    
        #############################
        # APPLY THE DM DISPLACEMENT #
        #############################
        if self.paragraph("APPLY THE DM DISPLACEMENT"): 

            put_back_by = list(-np.sum(spiral, axis=0))
            
            logger.info("Putting back the DM by:")
            logger.info(str(put_back_by))
                
            modes = [0,0] + put_back_by
            
            attempt = 0
            max_attempts = 3
            while attempt < max_attempts:
                logger.info("Attempt "+str(int(attempt)))
                try:
                    logger.info("Upload a mode vector to internal DM")
                    self.status = obsController.RTCTTModeUpload(modes, absolute=is_absolute)
                    logger.info("Waiting setup to apply displacement... DONE")
                    attempt = max_attempts
                except Exception as e:
                    print(e)
                    attempt += 1

