# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/testOB_tiptilt_close_loop.py 3790 2022-01-17 19:18:52Z depa $

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

class SHARKNIR_gen_tec_CloseLoop(SHINSApp):

    @exception.handler_for_run
    def run(self, ins_s=None, det_s=None, tel_s=None, rtc_s=None, **kwargs):
        '''Close tiptilt loop
        '''

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

        self.status = uvars.EXIT_SUCCESS
        obsController = self.obsController
        
        if (kwargs and "centroid_from_psf" in kwargs and type(kwargs["centroid_from_psf"])==bool ):
            centroid_from_psf = kwargs["centroid_from_psf"]
        else:
            centroid_from_psf = False

        if (kwargs and "diag_test_enabled" in kwargs and type(kwargs["diag_test_enabled"])==bool ):
            diag_test_enabled = kwargs["diag_test_enabled"]
        else:
            diag_test_enabled = False

        if diag_test_enabled:
            try:
                self.status, freq = obsController.RTCTTSendCameraCommand("fps raw")
                freq = float(freq)
            except Exception as e:
                logger.error(e)
                freq = 100.0
                logger.warning("Proceding anyway with default value : freq = {} Hz".format(freq))
            rtc_dit = 25000000/freq
            rtc_s["TTTESTTIME"]=str(rtc_dit)
            
        if rtc_s is not None:
            logger.debug("Enumerating RTC setup")
            for element, value in rtc_s.items():
                logger.debug("Element : % 20s  -  Value % 20s    " % (element, value))

        if centroid_from_psf:
            
            fitsDir = uvars.fitsDir + obsController.getCurrentDataDirectory() +'/'
            taly = aly(camera="CRED2")
                        
            ########################
            # Take an image on RTC #
            ########################
            if self.paragraph("Take an image on RTC"):
                
                logger.info("TT take image")
                self.status, self.filenames = obsController.RTCTTTeccamExpose(rtc_s)
                ufun.error_handler(self.status, "RTCTTTeccamExpose")
                logger.info("TT take image... DONE. fits cube is {}".format(self.filenames))
                
            #############################
            # Calculate centroid on RTC #
            #############################
            if self.paragraph("Calculate centroid, pass centroid, change gain"):
                
                #taly.detector.setCrop(0)
                taly.detector.setStackType('mean')
                taly.PSFNoCoro.LoadImage(fitsDir+self.filenames)
                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.info("CHECK: PSF normalized are {} {}".format(cx, cy))

        else:
            if self.paragraph("Close Internal Loop on Fixed Coordinates"):
                cx, cy = uvars.fixed_teccam_ref_coords
                logger.info("Closing internal lop on the fixed normalized Teccam coords: [{} , {}]".format(cx, cy))
                # logger.debug("Converting in TECCAM pixels...")
                # teccam_psfCX = (xsize/2)*(1-cx) - 1/2
                # teccam_psfCX = (ysize/2)*(1+cy) - 1/2
                # logger.debug("Converting in TECCAM pixels...DONE")
        
        ############################
        # 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")            
    
        ######################
        # Change Gain on RTC #
        ######################

        tt_test_time = rtc_pop("TTTESTTIME") if (rtc_s is not None and "TTTESTTIME" in rtc_s) else None
        logger.info("TT update gain")
        self.status = obsController.RTCTTExecuteSetup(rtc_s)
        ufun.error_handler(self.status, "RTCTTTeccamExpose")
        logger.info("TT update gain... DONE.")
        
        ###########################
        # CLOSE REAL-TIME TT LOOP #
        ###########################
        if self.paragraph("CLOSE REAL-TIME TT LOOP"):

            logger.info("TT close loop")
            self.status = obsController.RTCTTCloseLoop()
            ufun.error_handler(self.status, "RTCTTCloseLoop")

            if centroid_from_psf:
                logger.info("Tip tilt loop closed on TECCAM {}px, {}px".format(teccam_psfCX, teccam_psfCY))
                logger.info("Corresponding to TECCAM coords {}, {}".format(cx, cy))
            else:
                logger.info("Tip tilt loop closed on TECCAM coords {}, {}".format(cx, cy))
            
        ###############
        # Return this #
        ###############

        self.other = [cx, cy]
