# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/SHARKNIR_di_mon_DetectorGain.py 3952 2022-05-19 08:30:53Z depa $

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

from SHARKNIR_gen_base import SHARKNIR_gen_base

class SHARKNIR_gen_tiptilt_offset_and_scicam(SHINSApp):
    """calibration
    """

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

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

        gen_base = SHARKNIR_gen_base()

        logger.info("Prepare Instrument Setup")
        gen_base.main(ins_s=ins_s)

        ###########################
        # CHECK IF LOOP IS CLOSED #
        ###########################
        status, is_loop_closed = obsController.RTCTTRead(uvars.TTControlLoopEna, 1, "dsp")
        is_loop_closed = is_loop_closed[0]

        if("X_DISPLACEMENT_MAS" and "Y_DISPLACEMENT_MAS" in rtc_s):
            
            x_disp = float(rtc_s["X_DISPLACEMENT_MAS"])
            y_disp = float(rtc_s["Y_DISPLACEMENT_MAS"])
            logger.info("Requested X displacement =           {} mas".format(x_disp))
            logger.info("Requested Y displacement =           {} mas".format(y_disp))
            
            deg = float(rtc_s["SLOPE_Y"])*(np.pi/180) if("SLOPE_Y" in rtc_s) else 0
            platescale = 14.5
            
            delta_x = ( x_disp*np.cos(deg) + y_disp*np.sin(deg) )/platescale
            delta_y = ( x_disp*np.sin(deg) + y_disp*np.cos(deg) )/platescale
            logger.info("Computed displacements to apply in (x,y) =      ({} , {}) [mas] ".format(delta_x, delta_y))
            
        else:
            delta_x, delta_y = [0, 0]
            logger.info("No additional displacements required")

        logger.info("Is loop closed? {}".format(is_loop_closed))
        
        if is_loop_closed:
            # Loop closed
            
            ####################################
            # IF LOOP IS CLOSED: MOVE CENTROID #
            ####################################
            if self.paragraph("MOVE CENTROID"):

                import time
                diag_rtc_s = {
                    "TTPIXELENABLED"    : "1",
                    "TTPIXELDECIMATION" : "0",
                    "TTDIAGENABLED"     : "1",
                    "TTDIAGDECIMATION"  : "0",
                    "TTTESTTIME"        : "5000000"
                }
                asyncDiag = obsController.begin_RTCTTTeccamExpose(diag_rtc_s)
                time.sleep(1)
                
                ##################################
                # 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.info("Retrieved center: {} {}".format(cx, cy))
            
                ############################
                # Pass centroid to the RTC #
                ############################

                delta_x, delta_y     = ufun.to_teccam(delta_x, delta_y)
                delta_tip, delta_tilt = ufun.mode_to_teccam(float(rtc_s["VECTOR_3"]), float(rtc_s["VECTOR_4"]))
                                
                cx = cx_orig + delta_x + delta_tip
                cy = cy_orig + delta_y + delta_tilt

                logger.info("New centroid origin to apply :  ({} , {})".format(cx, cy))
                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))
                self.status = obsController.end_RTCTTTeccamExpose(asyncDiag)
        else:
            # Loop open    
                
            ############################
            # IF LOOP IS OPEN: MOVE DM #
            ############################
            if self.paragraph("MOVE DM"):
                
                delta_x, delta_y = ufun.transform_offset(disp=[delta_x, delta_y], thetaRot=0, conv='PSF')
                print("qui 1")
                if("VECTOR_3" in rtc_s):
                    rtc_s["VECTOR_3"] = str(float(rtc_s["VECTOR_3"]) + delta_x)
                    
                if("VECTOR_4" in rtc_s):
                    rtc_s["VECTOR_4"] = str(float(rtc_s["VECTOR_4"]) + delta_y)
                print("qui 2")
                ##################
                # SHOW RTC SETUP #
                ##################
                # print(rtc_s)
                logger.debug("Enumerating RTC setup")
                for element, value in rtc_s.items():
                    logger.debug("Element : % 15s  -  Value % 15s    " % (element, value))
                
                ##############
                # SAVE MODES #
                ##############
                
                modes = [ float(rtc_s["VECTOR_{}".format(i)]) for i in range(1, 39) ]
                
                logger.warning("Modes: {}".format(modes))
                
                #############################
                # APPLY THE DM DISPLACEMENT #
                #############################
                
                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
                
                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")
            
            
        #################
        # Setup + image #
        #################
        # gen base already have paragraphs
        
        gen_base.main(det_s=det_s)


        if is_loop_closed:
            # Loop closed

            ###############################################
            # IF LOOP IS CLOSED: PUT BACK CENTROID ORIGIN #
            ###############################################
            if self.paragraph("PUT CENTROID BACK, PASS CENTROID TO RTC"): 
            
                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")

                
        else:
            # Loop open    
                        
            #################################
            # IF LOOP IS OPEN: MOVE DM BACK #
            #################################
            if self.paragraph("MOVE DM BACK"):

                modes = [-m for m in modes]
        
                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")
                
                
