#!/usr/bin/env python
# -*- coding: utf-8 -*-

import json
import sys
import Ice
import sharknir
import time
from pathlib import Path
import datetime

import numpy as np
from astropy.time import Time, TimeDelta
# from astropy.time import Time
# from astropy.time import TimeDelta
from tpl.shinsapp import SHINSApp
from util import functions as ufun
from util import variables as uvars
from util import master_listener_davide


actuators = np.array([
                                                  [0.0, 0.3], [0.0, 0.4], [0.0, 0.5], [0.0, 0.6], [0.0, 0.7],
                                      [0.1, 0.2], [0.1, 0.3], [0.1, 0.4], [0.1, 0.5], [0.1, 0.6], [0.1, 0.7], [0.1, 0.8],
                          [0.2, 0.1], [0.2, 0.2], [0.2, 0.3], [0.2, 0.4], [0.2, 0.5], [0.2, 0.6], [0.2, 0.7], [0.2, 0.8], [0.2, 0.9],
              [0.3, 0.0], [0.3, 0.1], [0.3, 0.2], [0.3, 0.3], [0.3, 0.4], [0.3, 0.5], [0.3, 0.6], [0.3, 0.7], [0.3, 0.8], [0.3, 0.9], [0.3, 1.0 ],
              [0.4, 0.0], [0.4, 0.1], [0.4, 0.2], [0.4, 0.3], [0.4, 0.4], [0.4, 0.5], [0.4, 0.6], [0.4, 0.7], [0.4, 0.8], [0.4, 0.9], [0.4, 1.0 ],
              [0.5, 0.0], [0.5, 0.1], [0.5, 0.2], [0.5, 0.3], [0.5, 0.4], [0.5, 0.5], [0.5, 0.6], [0.5, 0.7], [0.5, 0.8], [0.5, 0.9], [0.5, 1.0 ],
              [0.6, 0.0], [0.6, 0.1], [0.6, 0.2], [0.6, 0.3], [0.6, 0.4], [0.6, 0.5], [0.6, 0.6], [0.6, 0.7], [0.6, 0.8], [0.6, 0.9], [0.6, 1.0 ],
              [0.7, 0.0], [0.7, 0.1], [0.7, 0.2], [0.7, 0.3], [0.7, 0.4], [0.7, 0.5], [0.7, 0.6], [0.7, 0.7], [0.7, 0.8], [0.7, 0.9], [0.7, 1.0 ],
                          [0.8, 0.1], [0.8, 0.2], [0.8, 0.3], [0.8, 0.4], [0.8, 0.5], [0.8, 0.6], [0.8, 0.7], [0.8, 0.8], [0.8, 0.9],
                                      [0.9, 0.2], [0.9, 0.3], [0.9, 0.4], [0.9, 0.5], [0.9, 0.6], [0.9, 0.7], [0.9, 0.8],
                                                  [1.0, 0.3], [1.0, 0.4], [1.0, 0.5], [1.0, 0.6], [1.0, 0.7]
             ])

dummy_64 = np.zeros(shape=(64,64))
dummy_patrol = np.zeros(shape=(640,512))

dark_path = Path("/opt/tan/etc/tiptilt_ctrl/rtc/darks/")
flat_path = Path("/opt/tan/etc/tiptilt_ctrl/rtc/dm_flats/")
gain_path = Path("/opt/tan/etc/tiptilt_ctrl/rtc/gains/")
ncpa_path = Path("/opt/tan/etc/tiptilt_ctrl/rtc/ncpa_modes/")

signs_list = []
with open(u'/opt/tan/etc/tiptilt_ctrl/rtc/BAX479-DM97-sign.dat', "r") as f:
    for line in f:
        for x in line.split():
            signs_list.append(int(x))
signs = np.array(signs_list)


class DevicesObject(object):
    def __init__(self):
        self.mini_os = SHINSApp()
        self.obsController = self.mini_os.os()
        self.json_state = {}
        self.soul_deltas = [-999,-999] 
        
    def readall(self):
        json_string = "{}"
        try:
            status, json_string = self.obsController.getJsonStatus()
            if ', , ' in json_string:
                index = json_string.index(', ,')
                json_elements = len(json_string.split("},"))
                json_string = ufun.filterString(main_string = json_string, substring = ", , ", new_substring = ", ")
                print(str(datetime.datetime.now())+"Received Json had {} elements. Missing element at char {} : '{}'".format(json_elements,index, json_string[index-10:index+20]))
        except Exception as e: #CommunicatorDestroyedException: or ConnectionLostException:
            print(str(datetime.datetime.now())+" : ERROR ______GETTING JSON________ Found the following exception: {}".format(e))
                        
        try:
            json_dict = json.loads(json_string)

            drot_pos_deg = ufun.drot_step2deg(json_dict["DROT_PosAng"]["encoder"])
            json_dict["DROT_PosAng"]["encoder"] = np.round(drot_pos_deg,3)
            json_dict["soul-deltas"] = self.soul_deltas
            self.json_state = json_dict
            return json_dict
        except Exception as e:
            print(str(datetime.datetime.now())+" : ERROR _________Void Json exception, no DROT_PosAng specified. Exception is: {}".format(e))
            self.json_state = json_dict
            return json_dict

    def read(self, dev):
        outDict = self.readall()
        return outDict[dev]

    def move(self, ins_s):

        status = self.obsController.setupInstrument(ins_s)

        return ins_s

    def absolute(self, ins_s):
        status = self.obsController.setupInstrumentAbsolutePosition(ins_s)
        return status

    def relative(self, ins_s):
        status = self.obsController.setupInstrumentRelativePosition(ins_s)
        return status

    def home(self, ins_s):

        lamps_s = dict((k, "OFF") for k in  ["CAL_FF_LAMP", "CAL_FIBER_FOCUS_LAMP", "CAL_FIBER_DEFOCUS_LAMP"] if k in ins_s)
        if lamps_s:
            self.obsController.setupInstrument(lamp_s)

        status = self.obsController.initMotion(ins_s.keys())
        status = self.obsController.initCalibUnit([k for k in ["CAL_FILTER_DEP", "CAL_MIRROR_DEP", "CAL_FIBER_DEP"] if k in ins_s])

        return status

    def stop(self, ins_s):
        status = self.obsController.stopMotion(ins_s.keys())
        return status

    def abort(self):
        status = self.mini_os.abort()
        return status


class RtcObject(object):
    def __init__(self):
        self.counter = 0
        self.todos = []
        self.mini_os = SHINSApp()
        self.obsController = self.mini_os.os()
        self.rotating_ncpa = False
        self.ncpa_file = "No file"
        self.json_state = {}

        self.thread = master_listener_davide.PLOT_THREAD()
        try:
            self.thread.start()
            self.thread.RUN = True
            print("plot thread started")
        except KeyboardInterrupt:
            print(" Intercepted Keyboard Interrupt")
            self.thread.CLOSE = True
            self.thread.RUN = False
            del self.obsController
            print("plot thread stopped")
            sys.exit(1)
        except Exception as e:
            print("here")
            print(e)
            self.thread.CLOSE = True
            self.thread.RUN = False
            del self.obsController
            print("plot thread stopped")
            sys.exit(1)

    def reset(self):
        print("reset")
        status = self.obsController.RTCTTResetBCU()
        return status
        
    def resurrect(self):
        print("import")
        from SHINS_SW_TEST.SHINS_TEST_DEBUG import SHINS_TEST_DEBUG
        
        print("init")        
        status = SHINS_TEST_DEBUG(test_s="10", test_s1="true") # init
        time.sleep(1)
        print("setup start")
        status = SHINS_TEST_DEBUG(test_s="11") # setup and start
        print(status)
        return status

    def resurrect2(self):
        print('resurrect2')
        rtc_s = uvars.rtc_s_base.copy()
        time.sleep(10)
        print("init")
        status = self.obsController.RTCTTInit(rtc_s)
        print("Exit status:  {}".format(status))
        ufun.error_handler(status, "RTCTTIsInit")
            
        print("Wait 10 s...")
        time.sleep(10)
        # SETUP REAL-TIME RTCTT #

        print("RTCTT setup... Wait 15 s...")
        status = self.obsController.RTCTTSetup()
        print("Exit status:  {}".format(status))
        ufun.error_handler(status, "RTCTTSetup")
        
        # START REAL-TIME RTCTT #
        print("RTCTT start")
        time.sleep(10)
        status = self.obsController.RTCTTStart()
        print("Exit status:  {}".format(status))
        ufun.error_handler(status, "RTCTTStart")

    def uploadRTC(self, modal=True):
        status = 1
        rtc_s = uvars.rtc_s_base.copy()
        current_flat, current_dark = ufun.get_status_by_name(self.obsController, ['rtc_dm_flat','rtc_cred_dark'])
        rtc_s['TTDMFLATFILE'] = current_flat
        rtc_s['TTBIASFILE'] = current_dark
        if modal:
            print("Enabling Modal Mode")
        else:
            print("Enabling Zonal Mode")
            rtc_s["TTM2CFILE"] = "ZONAL"

        # INIT REAL-TIME RTCTT #
        print("Enumerating RTC setup")
        for element, value in rtc_s.items():
            print("Element : % 20s  -  Value % 20s    " % (element, value))
        
        try:
            status = self.obsController.RTCTTInit(rtc_s)
            print("Exit status:  {}".format(status))
            ufun.error_handler(status, "RTCTTIsInit")
        except Exception as e:
            print(e)
            return status
            
        print("Wait 5 s...")
        time.sleep(5)
        # SETUP REAL-TIME RTCTT #
        
        try:
            print("RTCTT setup... Wait 15 s...")
            status = self.obsController.RTCTTSetup()
            print("Exit status:  {}".format(status))
            ufun.error_handler(status, "RTCTTSetup")
            
            # START REAL-TIME RTCTT #
            print("RTCTT start")
            status = self.obsController.RTCTTStart()
            print("Exit status:  {}".format(status))
            ufun.error_handler(status, "RTCTTStart")
            
        except Exception as e:
            print(e)
            return status
            
        mode = "Modal" if modal else "Zonal"
        print("Enabling {} Mode... DONE".format(mode))
        return status
        
        
    def zonalCommand(self, data):
        print("chiamata effettuata")
        normalize_strain = 1#6.666667
        act = int(data[0])
        strain = float(data[1])
        print(strain)
        strain = strain/normalize_strain
        print("Act on actuator number {} with absolute intensity {}".format(act,strain))
        reserved = [0,0]
        
        actuators = np.zeros(97).tolist()
        actuators[act-1] = strain
        
        print("vector for RTCTTModeUpload: ")
        # print(actuators)
        try:
            status = self.obsController.RTCTTModeUpload(reserved+actuators, absolute=False)
        except Exception as e:
            print(e)
            status = 1
        return status
        
    def move_soul(self, data, apply_movement=False):
        self.soul_deltas = data
        print("Received data: {}".format(data))
        PSFcx  = float(data[0])
        PSFcx  = 0    if PSFcx < 0    else PSFcx
        PSFcx  = 2048 if PSFcx > 2048 else PSFcx 
        PSFcy  = float(data[1])
        PSFcy  = 0    if PSFcy < 0    else PSFcy
        PSFcy  = 2048 if PSFcy > 2048 else PSFcy
        
        deltaX = PSFcx - uvars.SOUL_TARGET_POS[0]
        deltaY = PSFcy - uvars.SOUL_TARGET_POS[1]
        print("Calling Here OffsetXYAO with SCICAM deltas Dx = {} , Dy = {}".format(deltaX , deltaY))

        # GET CURRENT DROT #
        status, drotDict = self.obsController.getPosition("DEGREE", {"DROT":-9999.9})
        drotPos = drotDict["DROT"]
        print("DROT pos is {}".format(drotPos))
        
        # Transform to SOUL delta #
        step_x, step_y = ufun.transform_offset(disp = [deltaX,deltaY], thetaRot = drotPos , conv = "SOUL")
        print("Calculatd steps to apply to SOUL : {} , {}".format(step_x , step_y))
        if apply_movement:
            max_step = 1
            step_x = step_x if (abs(step_x) <= max_step) else max_step*np.sign(step_x)
            step_y = step_y if (abs(step_y) <= max_step) else max_step*np.sign(step_y)
            print("Applying these steps to SOUL : {} , {}".format(step_x , step_y))
            try:
                status = self.obsController.OffsetXYAO(step_x , step_y , "LEFT")
                print("Function is returning NOW")

            except Exception as e:
                print(e)
                status = 1

        else:
            from util.shinsLogger import logger
            resume = "Delta X = {} mm  ,  Delta Y = {} mm".format(step_x,step_y)
            logger.info("--------------------------------------------------------------------------")
            logger.info("| {:^70s} |".format(resume))
            logger.info("--------------------------------------------------------------------------")
            logger.debug("SCICAM deltas = ({},{}), DROT pos = {}, computed SOUL deltas = ({},{})".format(deltaX,deltaY,drotPos,np.round(step_x,3),np.round(step_y,3)))
        return status, [np.round(step_x,4), np.round(step_y,4)] 


    def manageDerotation(self, data):
        action  = str(data[0])
        try:
            mode    = str(data[1])
            pos_ang = float(data[2])
            pos_ang = pos_ang-360 if pos_ang>180 else pos_ang
        except Exception as e:
            # print(e)
            mode, pos_ang = None, None
        
        if(action == "Start_Tracking"):
              print("DAO_______________Derotation mode: {} with angle {}".format(mode,pos_ang))
              ins_s = {"DROT_Mode":mode}
              async = self.obsController.begin_setupInstrument(ins_s)
              status = self.obsController.end_setupInstrument(async)
                      
              if mode == "FIELD_FREE":
                  print("Launching now a {} derotation".format(mode))
                  status = self.obsController.trackingStart2(0,0)
                  
              elif mode == "FIELD_FIXED":
                  print("Launching now a {} derotation".format(mode))
                  dateMjd= Time.now()
                  status = self.obsController.trackingStart2(dateMjd.mjd, pos_ang)
                  
              else:
                  print("{} mode: NO derotation".format(mode))
                  status = self.obsController.trackingStop()
                  
        elif(action == "Stop_Tracking"):
              print("DAO__________________________STOP Derotation!")
              status = self.obsController.trackingStop()
        else:
              print("Something went wrong, no action received")
        print("Reached end of command")
        return status
            
    def read(self, id):
        for todo in self.todos:
            if todo["id"] == id:
                return todo
            
    def create(self, data):
        todo = data
        self.todos.append(todo)
        return todo

    def update(self, id, data):
        todo = self.read(id)
        todo.update(data)
        return todo

    def delete(self, id):
        todo = self.read(id)
        self.todos.remove(todo)

    @property
    def centroid(self):
        status, [cx, cy] = self.obsController.getInternalSeqFloatVariable("RtcCentroidOrigin")
        return [cx, cy]

    @centroid.setter
    def centroid(self, cx_cy):
        cx, cy = cx_cy
        status = self.obsController.RTCTTSetCentroidPos(cx, cy)
        return status

    def flatten(self):
        modes = [0] * 96#39
        status = self.obsController.RTCTTModeUpload(modes, absolute=True)
        status = self.load_ncpa(name="zeri.txt")
        return status
        
    def close_loop(self):
        status = self.obsController.RTCTTCloseLoop()
        return status

    def open_loop(self):
        status = self.obsController.RTCTTOpenLoop()
        return status

    def frequency(self, freq):
        """Set the loop frequency"""

        rtc_s = {"TTFRAMERATE": freq}
        status = self.obsController.RTCTTExecuteSetup(rtc_s)

        return status
  
    def tint(self, t):
        """Set integration time"""

        rtc_s = {
            "TTCAMTINT" : t,
        }
        status = self.obsController.RTCTTExecuteSetup(rtc_s)
        return status

    def patrol(self, b):
        """Set params to enable or disable patrol camera mode"""

        if b:
            print("Switch to patrol mode")
            rtc_patrol = {
                "TTM2CFILE": "20220603-Zernike-FullPupil-Arcetri-1u-rms-4rtc.dat",
                "TTWINCOORDX": "0",
                "TTWINCOORDY": "0",
                "TTWINCOLS": "640",
                "TTWINROWS": "512",
                "TTFRAMERATE": "200", #200
                "TTNSLOPEPIXELS": "16",
                "TTBIASFILE": "",
                "TTPIXELGAINFILE": "",
                "TTCAMTINT": "0.004995574",
                "TTPIXELGAINMODE": "FLATGAIN",
            }
            
        else:
            print("Switch to cropped mode")
            rtc_small = {
                "TTM2CFILE": "20220603-Zernike-FullPupil-Arcetri-1u-rms-4rtc.dat",
                "TTWINCOORDX": "224",
                "TTWINCOORDY": "212",
                "TTWINCOLS": "64",
                "TTWINROWS": "64",
                "TTFRAMERATE": "1000",
                "TTNSLOPEPIXELS": "4096",
                "TTBIASFILE": "2022-02-19-23-45-23_PixelBias_19.95°C_1000Hz-064x064.dat",
                "TTPIXELGAINFILE": "flat_gain_64x64.dat",
                "TTCAMTINT": "0.000994919",
                "TTPIXELGAINMODE": "FLATGAIN",
            }

        rtc_s = rtc_patrol if b else rtc_small
        
        status = self.obsController.RTCTTSwitchCcdMode(rtc_s)

        return status


    def temperature(self, temp):
        """Set the loop temperature"""

        status = self.obsController.RTCTTSendCameraCommand("set temperatures sensor "+temp)

        return status


    def pid(self, p):
        """Set the RTC PID"""
        
        rtc_s = {
            "TTPIDKP": str(p[0]),
            "TTPIDKI": str(p[1]),
            "TTPIDKD": str(p[2]),
            "TTPIDTF": str(p[3]),
            "TTPIDTS": str(p[4]),
            }
        rtc_s["TTPIDTS"]=str(1e-3)
        rtc_s["TTPIDTF"]=rtc_s["TTPIDTS"]
        print(rtc_s)
        status = self.obsController.RTCTTExecuteSetup(rtc_s)
        
        return status

    def threshold(self, t):
        """Set the RTC Threshold"""
        
        status = self.obsController.RTCTTWriteSingle(4416, t, "dsp")
        
        return status

    def get_threshold(self):
        """Get the RTC Threshold"""
        
        status, threshold = self.obsController.RTCTTReadSingle(4416, "dsp")
        
        return threshold

        
    ##############
    # Dark stuff #
    ##############

    def todos_dark(self):
        """List dark files"""

        return [ str(dark.name).decode('utf-8') for dark in sorted(dark_path.glob("*.dat")) ]

    def new_dark(self):
        """Create a new dark"""

        rtc_s = {
            "TTDIAGENABLED" : "true", # or "1"
            "TTDIAGDECIMATION" : "0",
            "TTPIXELENABLED" : "1",
            "TTPIXELDECIMATION" : "0", # if 0 master diagnostic stops
            "TTLOOPENABLED" : "false",
            "TTSAVEASBIAS" : "true",
            "TTUPDATEBIAS" : "true",
            "TTTESTTIME" : "1000000",
        }
        
        if float(ufun.get_status_by_name(self.obsController, ['rtc_cred_freq'])[0]) < 10 :
            rtc_s["TTTESTTIME"]="20000000"
            
        status, filename = self.obsController.RTCTTTeccamExpose(rtc_s)

        return status, filename

    def load_dark(self, name):
        """Load a dark file"""
        rtc_s = {"TTBIASFILE": name}
        status = self.obsController.RTCTTExecuteSetup(rtc_s)

        return status

    def get_current_dark(self):
        pass

    ##############
    # Gain stuff #
    ##############

    def todos_gain(self):
        """List gain files"""

        return [ str(gain.name).decode('utf-8') for gain in sorted(gain_path.glob("*.dat")) ]

    def new_gain(self, gain_type, centroid_size):
        """Create a new gain"""
        rtc_s = {
            "TTPIXELGAINMODE": gain_type,
            "TTPIXELGAINRADIUS": centroid_size,
        }
        status = self.obsController.RTCTTExecuteSetup(rtc_s)
        return status

    def load_gain(self, name):
        """Load a gain file"""
        rtc_s = {
            "TTPIXELGAINMODE": "FILEGAIN",
            "TTPIXELGAINFILE": name,
        }
        status = self.obsController.RTCTTExecuteSetup(rtc_s)
        return status

    ##############
    # Flat stuff #
    ##############

    def todos_flat(self):
        """List flat files"""
        return [ str(flat.name).decode('utf-8') for flat in sorted(flat_path.glob("*.txt")) ]
    
    def save_flat(self):
        """Save the current flat in a file"""

        print("hereeee")
        flat_name = ufun.save_flat(self.obsController, self.thread)
        print("doneee")
        print(flat_name)
        return flat_name
    
    def load_flat(self, name):
        """Load a flat file"""
        rtc_s = {"TTDMFLATFILE": name}
        status = self.obsController.RTCTTExecuteSetup(rtc_s)
        return status

    def tiptilt(self, modes=[0,0,0,0], absolute=False):
        print("Loaded Modes {}".format(modes))
        status = self.obsController.RTCTTModeUpload(modes=modes, absolute=absolute)
        return status
        
    def teccam_http_start(self):
        status, _ = self.obsController.RTCTTSendCameraCommand("exec httpserver start")
        return status

    def teccam_custom_command(self, command):
        status, res = self.obsController.RTCTTSendCameraCommand(command)
        return res

    def cloud_samples(self, s):
        print("prima : samples = {}".format(self.thread.cloud_samples))
        status = self.thread.cloud_samples = s
        print("dopo : samples = {}".format(self.thread.cloud_samples))
        return status

    def get_cloud_samples(self):
        return self.thread.cloud_samples
    
    def save(self):
        data = self.data()
        now = Time.now().isot

        filename = "/data/rtc/rtc-panel-save-{}.json".format(now)
        with open(filename, 'w') as f:
            json.dump(data, f)
        return filename

    ##############
    # NCPA stuff #
    ##############

    def todos_ncpa(self):
        """List ncpa files"""
        return [ str(ncpa.name).decode('utf-8') for ncpa in sorted(ncpa_path.glob("*.txt")) ]

    def load_ncpa(self, name):
        """Load a ncpa file"""
        # load saved file
        ncpa_path = "/opt/tan/etc/tiptilt_ctrl/rtc/ncpa_modes/"
        mode_file = open(ncpa_path+name, 'r')
        data = mode_file.read()
        data_into_list = data.split('\n')
        mode_vector = [float(element) for element in data_into_list[:-1]]

        # edit mode vector obtained from loaded file
        print("v=[{}], len = {}".format(mode_vector, len(mode_vector)))
        angle_where_ncpa_was_optimized = float(mode_vector.pop())
        print("angle read from ncpa file: {}".format(angle_where_ncpa_was_optimized))
        print("v=[{}], len = {}".format(mode_vector, len(mode_vector)))
        mode_vector = mode_vector[2:]
        print("final mode vector=[{}], len = {}".format(mode_vector, len(mode_vector)))

        #apply correct rotation (if necessary)
        actual_drot_pos = float(self.obsController.getPosition("DEGREE", {"DROT":-9999})[1]["DROT"])
        drot_diff = actual_drot_pos -angle_where_ncpa_was_optimized

        if(abs(drot_diff)>0.5):
            ruotati = ufun.rotate_modes(mode_vector, -1*drot_diff)
        else:
            ruotati = mode_vector
        riservati = [0,0]
        try:
            status = self.obsController.RTCTTModeUpload( riservati + list(ruotati), absolute=True)
        except Exception as e:
            print(e)
            status = 1
        self.obsController.setContext({"NCPA_FILE":name});
        print("Finished")
        return status

    def save_ncpa(self):
        """Save the current ncpa in a file"""
        ncpa_name = ufun.save_ncpa(os=self.obsController)
        print(ncpa_name)
        return ncpa_name
    
    def rotateNCPA(self, action):
        
        if action == "start":
            print("START NCPA Rotation")
            self.rotating_ncpa = True
            self.obsController.setContext({"NCPA_ROT_STATUS":"Rotation ON"});
        elif action == "stop":
            print("STOP NCPA Rotation")
            self.rotating_ncpa = False
            self.obsController.setContext({"NCPA_ROT_STATUS":"Rotation OFF"});
        else:
            print("Wrong command in rotateNCPA")

        riservati = [0,0]
        sign = -1 # rotation direction
        sleep = 5#10
        threshold = 0.5

        ins_s = {"DROT":-9999}
        angle_where_ncpa_was_optimized = float(self.obsController.getPosition("DEGREE", ins_s)[1]["DROT"])

        status, total_vector = self.obsController.getInternalSeqFloatVariable("RtcAppliedMode")

        mode_vector = total_vector[2:]
        position_of_last_update = angle_where_ncpa_was_optimized
        
        while self.rotating_ncpa:
            
            bearing = float(self.obsController.getPosition("DEGREE", ins_s)[1]["DROT"])
            diff = bearing - position_of_last_update
            
            print("Bearing: {}; Diff wrt previous update: {}".format(bearing, diff))
            if abs(diff) > threshold:
                print("Above threshold: rotating modes!")
                ruotati = ufun.rotate_modes(mode_vector, sign * (bearing - angle_where_ncpa_was_optimized) )[0:36]
                self.obsController.RTCTTModeUpload( riservati + list(ruotati), absolute=True)
                position_of_last_update = bearing
            else:
                print("Below threshold: no mode rotation")            
                
            time.sleep(sleep)
        return 1

    def getNCPARotationStatus(self):
        print("Status ncpa rotation in arrivo... status is {}".format(self.rotating_ncpa))
        return self.rotating_ncpa


    @property
    def rtc_image(self):

        # Diagostic stuff
        try:
            #print("trying to get the image")
            image = self.thread.img.T # transpose
            #print("there is an image. probably diagnostic is arriving")
        except AttributeError as e:
            print("no image")
            print("if NO image but diagnostic data from os are coming then try reset and resurrect rtc")
            print("can also try to request fps to the console on rtc panel, and then again reset+resurrect")
            image = dummy_64
        except IndexError as e:
            print("too many indices")
            image = dummy_64
        except Exception as e:
            print("Generic error")
            print(e)

        return image


    def data_cred(self, image, centroid):
        # # OS

        status, darkarray = self.obsController.getInternalSeqFloatVariable("RtcPixelBias")

        try:            
            darkimage = np.array(darkarray).reshape(image.shape).T
        except Exception as e:
            darkimage = np.zeros(shape=(image.shape))
        
        try:
            cx,cy = centroid
            image = image[2:-2 , 2:-2]
            darkimage = darkimage[2:-2 , 2:-2]
            darkedData  = image - darkimage
            cred = {
                "data": image.tolist(),
                "dark": darkimage.tolist(),
                #"darkedData" = darkedData,
                "min": image.min(),              #image[1:-1, 1:-1].min(),
                "max": image.max(),              #image[1:-1, 1:-1].max(),
                "minDark" : darkedData.min(),    #darkedData[3:, 3:].min(),
                "maxDark" : darkedData.max(),    #darkedData[3:, 3:].max(),
                "cx": cx,
                "cy": cy,
                "image_shape": list(image.shape),
            }

        except Exception as e:
            print("error building the cred response")
            print(e)
            print("image shape : {} ".format(image.shape))
            #print("image shape : {}  ,  darkedData shape : {}".format(image.shape, darkedData.shape))
            
            cred = None
            
        return cred


    def data_gain(self, image):

        # OS
        status, pixgain = self.obsController.getInternalSeqFloatVariable("RtcPixelGain")

        try:            
            pixgain = np.array(pixgain).reshape(image.shape).T
        except Exception as e:
            pixgain = np.zeros(shape=(image.shape))+1

        try:

            gain = {
                "data": pixgain.reshape(image.shape).tolist(),
                "min": pixgain[3:, 3:].min(),
                "max": pixgain[3:, 3:].max(),
                "image_shape": list(image.shape),
            }

        except Exception as e:
            print("error building the gain response")
            print(e)
            gain = None

        return gain

    def data_dm(self, image):

        # Diagnostics
        shape = self.thread.shape

        # OS
        status, dmflat = self.obsController.getInternalSeqFloatVariable("RtcDmFlat")
        
        try:
            dm_shape_no_flat = (shape*signs) - (np.array(dmflat[0:97]))
            dm = {
                "actuators": actuators.tolist(),
                "shape": (shape*signs).tolist(),
                "flat": (np.array(dmflat[0:97])).tolist(),
                #"dm_shape_no_flat" : dm_shape_no_flat,
                "min": round(shape.min(),2),
                "max": round(shape.max(),2),
                "min_no_flat" : round(min(dm_shape_no_flat),2),
                "max_no_flat" : round(max(dm_shape_no_flat),2),
                "image_shape": list(image.shape),
                "skp_cmd"    : self.thread.tt_cmd[1]
            }

        except Exception as e:
            print("error building the dm response")
            print(e)
            dm = None

        return dm


    def data_cloud(self, image, centroid):

        centroids_x = self.thread.centroid_x
        centroids_y = self.thread.centroid_y
        cloud = np.flip(self.thread.centroid_matrix.T, 1)
        cx,cy = centroid
        rmsX = np.std(centroids_x)*21  # px 2 mas,  plate scale = 21 mas/px
        rmsY = np.std(centroids_y)*21  # px 2 mas
        wx = (np.max(centroids_x)-np.min(centroids_x))*21 # px to mas
        wy = (np.max(centroids_y)-np.min(centroids_y))*21 # px to mas
        
        try:

            cld = {
                "data": cloud.tolist(),
                "min": cloud.min(),
                "max": cloud.max(),
                "samples": len(self.thread.centroid_x),
                "cx": cx - self.thread.cx,
                "cy": cy - self.thread.cy,
                "wx": round(wx, 5), 
                "wy": round(wy, 5), 
                "image_shape": list(image.shape),
                "RMS_X": round(rmsX, 5),
                "RMS_Y": round(rmsY, 5),
                "cloud_samples": self.thread.cloud_samples,
            }

        except Exception as e:
            print("error building the cloud response")
            print(e)
            cld = None

        return cld

    def data(self):

        image = self.rtc_image
        centroid = self.centroid

        data_cloud = self.data_cloud(image, centroid)
        data_cred = self.data_cred(image, centroid)

        ### Focus only on the 8x8 pixel matrix centered on the centroid
        def focus_on_centroid(a, cx, cy, s=4):
            # a is an array-like matrix , s is an Int
            cx = int(cx)*32 + 32   # cx and cy are normalized in [-1,1] -> now are renormalized in [0,64]
            cy = int(cy)*32 + 32

            if(s>cx or s>cy): 
                s = min(cx,cy)
                
            if(s>len(a)-cx or s>len(a)-cy):
                s = min(s , len(a)-cx , len(a)-cy)
                
            a_focused = a[cx-s : cx+s , cy-s : cy+s]
            return a_focused
        
        
        cropped_cred = focus_on_centroid( np.array(data_cred["data"]) , data_cloud["cx"] ,data_cloud["cy"], s=4)


        flux_cred = np.sum(cropped_cred)
                
        data_cred["tot_flux"] = flux_cred   # added data flux to jso

        cropped_cred_darked = focus_on_centroid( np.array(data_cred["data"])-np.array(data_cred["dark"]) , data_cloud["cx"] ,data_cloud["cy"], s=4)

        flux_cred_darked = np.sum(cropped_cred_darked)
        
        data_cred["tot_flux_darked"] = flux_cred_darked  # added darked flux to json
        
        response = {
            "cred": data_cred,
            "gain": self.data_gain(image),
            "dm": self.data_dm(image),
            "cloud": data_cloud,
            "cx": centroid[0],
            "cy": centroid[1],
        }

        #print(response["cred"]["tot_flux"])

        self.json_state = response
        return response
