# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/SHARKNIR_gen_cal_spiral.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 alignment.allineamento import Align_the_coronagraph as aly


from SHARKNIR_gen_base import SHARKNIR_gen_base

class SHARKNIR_gen_cal_spiral_with_pupil_and_centroidorigin(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

        fitsDir = uvars.fitsDir + obsController.getCurrentDataDirectory() +'/'

        # taly = aly(camera="CRED2")
        
        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
        
        ##################################
        # Retrieving RTC centroid origin #
        ##################################
        
        logger.info("Read centroid origin on RTC...")
        self.status, [cx, cy] = obsController.RTCTTReadSingle(uvars.CentroidOrigin, 2, 'dsp')
        logger.info("Read centroid origin on RTC... Done")
        
        # self.status, rtc_filenames, self.other = gen_tec_CloseLoop.main()
        # cx,cy = self.other

        cx_orig = cx
        cy_orig = cy
        
        logger.warning("RETRIEVED: {} {}".format(cx, cy))


        # ##### check check check #####
        
        # ########################
        # # Take an image on RTC #
        # ########################

        # logger.info("TT take image")
        # self.status, rtc_fits_file = obsController.RTCTTTeccamExpose(rtc_s)
        # ufun.error_handler(self.status, "RTCTTTeccamExpose")
        # logger.info("TT take image... DONE. fits cube is {}".format(rtc_fits_file))
        
        # #############################
        # # Calculate centroid on RTC #
        # #############################

        # #taly.detector.setCrop(0)
        # taly.detector.setStackType('mean')
        # taly.PSFNoCoro.LoadImage(fitsDir+rtc_fits_file)
        # taly.PSFNoCoro.Center(model='Gaussian2D', box=20, display=False)
        # teccam_psfCX = taly.PSFNoCoro.xc
        # teccam_psfCY = taly.PSFNoCoro.yc
        # logger.info("CHECK: PSF centers are {} {}".format(teccam_psfCX, teccam_psfCY))

        # ################################
        # # Normalize values to [-1, +1] #
        # ################################

        # xsize, ysize = (taly.PSFNoCoro.image).shape
        # print(xsize, ysize)

        # # Scale to [-1, scaled_value, 1] and take scaled_value
        # cx = ( (xsize/2) - (teccam_psfCX + 0.5) ) / (xsize/2)
        # cy = ( (teccam_psfCY + 0.5) - (ysize/2) ) / (ysize/2)

        # logger.warning("FITTED AT BEGINNING {} {}".format(cx, cy))

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

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

                to_go -= 1
                
                logger.info("---------- "+str(int(to_go))+" to go -----------------")
                
                if is_absolute:
                    cx = step_x
                    cy = step_y
                else:
                    cx = cx + step_x
                    cy = cy + step_y                
                    
                ############################
                # Pass centroid to the RTC #
                ############################
                
                logger.info("Change centroid origin on the technical camera")
                self.status = obsController.RTCTTSetCentroidPos(cx, cy)
                ufun.error_handler(self.status, "RTCTTSetCentroidPos")
                
                logger.info("TECCAM CentroidOrigin is now {}, {}".format(cx, cy))
                
                ########################
                # Take an image on RTC #
                ########################
                
                logger.info("TT take image")
                self.status, rtc_fits_file = obsController.RTCTTTeccamExpose(rtc_s)
                ufun.error_handler(self.status, "RTCTTTeccamExpose")
                logger.info("TT take image... DONE. fits cube is {}".format(rtc_fits_file))

            #######################
            # FOCUS SETUP + IMAGE #
            #######################

            logger.info("****Start focus setup and image")
            self.status, self.filename, self.other = gen_base.main(ins_s_1, det_s_1)
            logger.info("****End focus setup and image")

            #########################
            # DEFOCUS SETUP + IMAGE #
            #########################

            logger.info("****Start defocus setup and image")
            self.status, self.filename, self.other = gen_base.main(ins_s_2, det_s_2)
            logger.info("****End defocus setup and image")
            
            # ##################
            # # EXPOSING SASHA #
            # ##################
    
            # logger.info("Waiting camera exposure to complete ...")
            # self.status, self.filenames = obsController.sashaExpose()
            # logger.info("Waiting camera exposure to complete ... DONE")
            # logger.info("FITS files are "+str(self.filenames) )        

            
        logger.info("End of the loop: this was the spiral, composed by {} points:".format(len(spiral)))
        logger.info(str(spiral))
                                    
        ############################
        # PUT BACK CENTROID ORIGIN #
        ############################
        if self.paragraph("PUT BACK CENTROID ORIGIN"): 
        
            logger.info("Putting back the DM to {}, {}".format(cx_orig, cy_orig))
            
            ############################
            # Pass centroid to the RTC #
            ############################
            
            logger.info("Change centroid origin on the technical camera")
            self.status = obsController.RTCTTSetCentroidPos(cx_orig, cy_orig)
            ufun.error_handler(self.status, "RTCTTSetCentroidPos")
            
            ##### check check check #####
            
            ########################
            # Take an image on RTC #
            ########################
            
            logger.info("TT take image")
            self.status, rtc_fits_file = obsController.RTCTTTeccamExpose(rtc_s)
            ufun.error_handler(self.status, "RTCTTTeccamExpose")
            logger.info("TT take image... DONE. fits cube is {}".format(rtc_fits_file))
            
            # #############################
            # # Calculate centroid on RTC #
            # #############################
            
            # #taly.detector.setCrop(0)
            # taly.detector.setStackType('mean')
            # taly.PSFNoCoro.LoadImage(fitsDir+rtc_fits_file)
            # taly.PSFNoCoro.Center(model='Gaussian2D', box=20, display=False)
            # teccam_psfCX = taly.PSFNoCoro.xc
            # teccam_psfCY = taly.PSFNoCoro.yc
            # logger.info("CHECK: PSF centers are {} {}".format(teccam_psfCX, teccam_psfCY))
            
            # ################################
            # # Normalize values to [-1, +1] #
            # ################################
            
            # xsize, ysize = (taly.PSFNoCoro.image).shape
            # print(xsize, ysize)
            
            # # Scale to [-1, scaled_value, 1] and take scaled_value
            # cx = ( (xsize/2) - (teccam_psfCX + 0.5) ) / (xsize/2)
            # cy = ( (teccam_psfCY + 0.5) - (ysize/2) ) / (ysize/2)
            
            # logger.warning("FITTED AT END OF SPIRAL {} {}".format(cx, cy))

