# $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/scripts/templates/tpl/SHARKNIR_gen_tec_TargetPos.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

class SHARKNIR_gen_tec_TargetPos(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
        
        ##################################
        # Retrieving RTC centroid origin #
        ##################################
        if self.paragraph("READ, CHANGE, READ AGAIN RTC CENTROID"):
        
            logger.info("Read centroid origin on RTC...")
            self.status, [cx_orig, cy_orig] = obsController.RTCTTReadSingle(uvars.CentroidOrigin, 2, 'dsp')
            logger.info("Read centroid origin on RTC... Done")
            logger.info("Centroid origin is: {}, {}".format(cx_orig, cy_orig))
            
            ############################
            # Pass centroid to the RTC #
            ############################
            
            cx = float(rtc_s["TTCENTROIDORIGX"])
            cy = float(rtc_s["TTCENTROIDORIGY"])
            
            logger.info("Change centroid origin on the technical camera")
            self.status = obsController.RTCTTSetCentroidPos(cx, cy)
            ufun.error_handler(self.status, "RTCTTSetCentroidPos")
            logger.debug(self.status)
            
            logger.info("Centroids changed on TECCAM to {}, {}".format(cx, cy))
            
            ##################################
            # Retrieving RTC centroid origin #
            ##################################
            
            logger.info("Read centroid origin on RTC...")
            self.status, [cx_orig, cy_orig] = obsController.RTCTTReadSingle(uvars.CentroidOrigin, 2, 'dsp')
            logger.info("Read centroid origin on RTC... Done")
            logger.info("Centroid origin is: {}, {}".format(cx_orig, cy_orig))
        
