
import os
import time
import numpy as np
from lxml import etree
import easysnmp
from itertools import cycle
from util import variables as uvars
from util.shinsLogger import logger
from util import exception
from astropy.coordinates import SkyCoord
from astropy.time import Time
import astropy.units as u
import csv
import json
from alignment.rotate_NCPA import Deformable_Mirror
from alignment.allineamento import Align_the_coronagraph as aly

###############
# Miscellanea #
###############

def calc_ramp_settings(dit, readout, ngmin=uvars.Default_NGMIN, ngmax=None, keep_ngmax=False, diff_threshold=0.05):
    """ Determine ramp settings for SCICAM acquisitions

    This script proposes a strategy to set SCICAM acquisition parameters given an input
    targeted exposure time and a given setting of the camera (type of window and clock time).
    Created on Mon Aug 16 18:46:58 2021
    @author: DV


    This function computes the SCICAM ramp settings (NGROUPS, NDROPS) that best match
    a targeted integration time. NREADS is always 1.

    Parameters
    ==========
    dit            : the targeted integration time in seconds
    ftime_ms       : the frame time in milliseconds (readout mode dependent)
    ngmin          : the minimum number of groups
    ngmax          : the maximum number of groups
    keep_ngmax     : set True if you want to force the ramp to have exactly ngmax groups. If False, ngroups
                     is optimized so to get as close as possible to the targeted integration time.
    diff_threshold : difference ratio between dit and calculated. 0.05 = 5%
    """

    dit = float(dit)*1000 # transform to milliseconds

    frame_type = {
        "128x128"                   : 71.76,
        "coro_stripe"               : 128.322,
        "256x256"                   : 274.5,
        "Center"                    : 2675.1,
        "Full_Image"                : 4278.3,
        "1000x1000"                 : 4048.08,
        "64x64"                     : 19.0,
        "32x32"                     : 5.4,
        "PD_wollaston_stripe_small" : 170.073,
        "PD_wollaston_stripe_large" : 295.324,
        "obs_stripe_200px"          : 420.6,
        "obs_stripe_512px"          : 1071.9
    }

    ftime_ms = frame_type[readout]

    # Case 0. Single frame
    if dit < ftime_ms:
        num_groups = ngmin if ngmin else 1
        num_reads  = 1
        num_drops  = 0

        return { "NGROUPS": str(num_groups),
                 "NREADS": str(num_reads),
                 "NDROPS": str(num_drops),
                }

    # Total number of frames requested to match integration time
    nftot = int(np.ceil(dit/ftime_ms))

    if ngmax is None:
        keep_ngmax = False
        ngmax=nftot

    # Case 1.
    # Requested frames are more than groups. Need to insert drop frames into each group.
    # Optimize ndrops AND NGROUPS to get close to the requested itime
    if nftot >= ngmax:
        num_reads = 1
        if keep_ngmax:
            ngmin = ngmax
        #else:
        #    ngmin = ngmax - 1 if ngmax <= 3 else 2

        nf_diff_prev = 50.0
        frac_prev = 1.0

        for i in range(ngmax, ngmin-1, -1):
            num_groups = i
            num_drops  = int(np.rint((dit - num_groups * num_reads * ftime_ms) / (num_groups - 1) / ftime_ms))
            nftot_res = num_groups * num_reads + (num_groups - 1) * num_drops

            # Break conditions
            nf_diff = nftot_res - (dit / ftime_ms)
            frac = nf_diff * ftime_ms / dit

            # This keeps the highest ngroups if same values for multiple combinations
            if (nf_diff < nf_diff_prev or frac < frac_prev):
                ng_best = num_groups
                nd_best = num_drops
                nf_diff_prev = nf_diff
                frac_prev = frac

            if nf_diff <= 0.5 or frac <= diff_threshold:
                break

        num_groups = ng_best;
        num_drops = max(0,nd_best);

        '''
        num_groups = ngmax
        num_reads  = 1
        num_drops  = (ExpTime - num_groups*FrameTime) / (num_groups - 1) / FrameTime
        num_drops_array = np.array([np.floor(num_drops), np.ceil(num_drops)])
        EffExpTime = FrameTime * (num_groups*num_reads + (num_groups - 1)*num_drops_array)
        Delta = np.abs(ExpTime-EffExpTime)
        num_drops = int(num_drops_array[np.argmin(Delta)])
        '''

    # Case 2.
    # Requested frames are less than groups. Save all of them in separated groups.
    else:
        num_groups = nftot
        num_reads  = 1
        num_drops = 0

    EffExpTime = ftime_ms * (num_groups*num_reads + (num_groups-1)*num_drops)
    logger.debug("setINDI sasha.num_groups.value={}".format(num_groups))
    logger.debug("setINDI sasha.num_reads.value={}".format(num_reads))
    logger.debug("setINDI sasha.num_drops.value={}".format(num_drops))
    logger.debug("Effective exposure time [ms]: {:.2f}".format(EffExpTime))
    logger.debug("Delta [ms]: {:.2f}".format(dit-EffExpTime))


    return { "NGROUPS": str(num_groups),
             "NREADS": str(num_reads),
             "NDROPS": str(num_drops),
            }


def make_spiral(step, repeat):

    def move_right(x,y,step):
        return [x+step, y]

    def move_down(x,y,step):
        return [x, y-step]

    def move_left(x,y,step):
        return [x-step, y]

    def move_up(x,y,step):
        return [x, y+step]

    moves = [move_right, move_down, move_left, move_up]

    def gen_points(step, repeat):
        _moves = cycle(moves)
        n = 1
        pos = [0,0]
        times_to_move = 1

        yield pos # initial

        while True:
            for _ in range(2):
                move = next(_moves)
                for _ in range(times_to_move):
                    if n >= repeat:
                        return
                    pos = move(0,0,step) #move(*pos)
                    n+=1
                    yield pos

            times_to_move+=1

    spiral = gen_points(step, repeat)
    return list(spiral)

def filterString(main_string, substring, new_substring):
    '''
    Given a input strung main_string, this function checks if contains 
    substring and changes and overwrite new_substring until original substring is no more present in main_string. 
    Then returns new modified string
    '''
    if substring not in main_string:
        print("main_string does not contain substring... no action needed")
        return main_string
    else:
        while(substring in main_string):
            #print(main_string)
            i = main_string.index(substring)
            new_string = main_string[:i]+str(new_substring)+main_string[i+len(substring):]
            main_string = new_string
        #print(main_string)
        return main_string

def get_status_by_name(os, names):
    status, jstr = os.getJsonStatus()
    print("received json status")
    json_dict = json.loads(jstr)
    dev_status = []
    for dev_name in names:
        if json_dict.has_key(dev_name):
            dev_dict = json_dict[dev_name] 
            if dev_dict.has_key("named"):
                dev_status.append(str(dev_dict["named"]))
            elif dev_dict.has_key("value"):
                dev_status.append(str(dev_dict["value"]))
            else:
                print('type key not found')
                dev_status.append(None)
                                
        else:
            logger.warning('{} is not present in status dictionary'.format(dev_name))
            dev_status.append("")
        
    return dev_status
        
###########################
# Lamps-related functions #
###########################


def switch_on_off_pdu_elements(pdu, aliasDict, onoff="OFF"):

    elems = aliasDict

    # LAMPS SETUP #

    for elem in elems:
        state = onoff
        logger.info("Setup {} to {}".format(elem,state))
        try:
            res  = set_pdu_elem_state(elem, state, pdu=pdu, aliasDict=aliasDict)
            status = uvars.EXIT_SUCCESS if res else uvars.EXIT_FAILURE

        except easysnmp.EasySNMPTimeoutError as e:
            logger.error("Failed setting lamp {}: {}".format(elem, e))
            status = uvars.EXIT_FAILURE

    time.sleep(1)

    # LAMPS STATUS #

    for elem in elems:
        try:
            res  = get_pdu_elem_state(elem, pdu=pdu, aliasDict=aliasDict)
            logger.debug("Lamp {} is now {}".format(elem, res))
        except easysnmp.EasySNMPTimeoutError as e:
            logger.error("Failed getting elem {}: {}".format(elem, e))
            status = uvars.EXIT_FAILURE


def get_pdu_elem_state(elem, getattribute="OutletState",
                       pdu=uvars.ipPDU1, aliasDict=uvars.aliasDict):

    session = easysnmp.Session(hostname=pdu,
                      security_username=uvars.pduUID,
                      version=uvars.snmpVer)
    state = uvars.dictOutletControl[getattribute]
    device = aliasDict[elem]
    res = session.get(state.format(device))
    return uvars.lampStateDict[int(res.value)]


def set_pdu_elem_state(elem, state, setattribute="OutletCommand", pdu=uvars.ipPDU1,
                       aliasDict=uvars.aliasDict):

    session = easysnmp.Session(hostname=pdu,
                      security_username=uvars.pduUID,
                      version=uvars.snmpVer)
    send = uvars.dictOutletControl[setattribute]
    device = aliasDict[elem]
    command = uvars.outletCmdDict[state]
    res = session.set(send.format(device), command, snmp_type='INTEGER')
    return res


#########################
# XML-related functions #
#########################


def dict_to_xml(tag, d):
    """Transform a dictionary into an xml. Params:
    tag: root tag name, and
    d: dict
    """

    element = etree.Element(tag)
    for key, val in d.iteritems():
        child = etree.Element(key)
        child.text = str(val)
        element.append(child)
    return element


def parse(xml_file):
    """Reads the XML file. It can be a ref setup, or an OB"""

    parser = etree.XMLParser(remove_comments=True)
    tree = etree.parse(xml_file, parser=parser)

    logger.info("Ok parsing {}".format(xml_file))

    valid = tree.getroot()

    return valid

########################
# Conversion functions #
########################

def transform_offset(disp=[0, 0], thetaRot=0.0, conv="Pup"):

    if conv == "Pup":
        thetaRef = np.deg2rad(95.0) # bearing angle
        intMat = [[ 0.0010, 0.0016],
                  [-0.0016, 0.0015]]

    elif conv == "PSF":
        thetaRef = np.deg2rad(0.0)
        intMat = [[-11.092,  0.0   ],
                  [ 0.0,    -10.945]]
        
    elif conv == "SOUL": # Retrurns the opposite signs?!
        thetaRef = np.deg2rad(95.0) # bearing angle
        intMat = [[ -110.268,  +36.704],
                  [ +37.687, +108.299]]
        
    thetaRot = np.deg2rad(thetaRot)

    thetaDiff = thetaRot - thetaRef

    R = np.asarray([[np.cos(thetaDiff), -np.sin(thetaDiff)],
                    [np.sin(thetaDiff),  np.cos(thetaDiff)]])

    RI = np.dot(R, intMat)

    invRI = np.linalg.inv(RI)

    theta = np.dot(invRI, np.asarray(disp)) 

    return theta


    


def to_teccam(x, y):
    '''
    Convert SCICAM coordinates [px],
    into TECCAM coordinates, which are scaled from -1 to 1.
    0.47px on SCICAM corresponds to 0.01 on TECCAM.
    '''

    conversion = (0.47/0.01)

    return [+x/conversion, -y/conversion]

def mode_to_teccam(tip, tilt):
    '''
    Convert tip-tilt modes in teccam coordinates for displacement
    ''' 
    conversion = 0.23
    return [tip*conversion, tilt*conversion]

def guess(value):
    """Guess the type of a variable"""
    
    try:
        val = int(value)
        return val
    except:
        try:
            #float
            val = float(value)
            return val
        except:
            try:
                # list or dict
                val = json.loads(value)
                return val
            except:
                # string
                return value

            
def convert_readout(cx, cy, readout="Full_Image"):
    '''
    This functions receives a pair of coordinates in Full_Image
    and returns the same coordinates in the reference syste specified by "readout"
    '''    
    if cx<0 or cy<0:
        logger.error("Received wrong values for coordinates, cannot be negative")
        return
    
    if readout not in uvars.dictScicamRegionBounds :
        logger.error("Invalid Readout parameter given to function. Received : {}".format(readout))
        return
    
    if readout == "Full_Image":
        return [cx, cy]
    
    x1 = uvars.dictScicamRegionBounds[readout]["x1"]
    x2 = uvars.dictScicamRegionBounds[readout]["x2"]
    y1 = uvars.dictScicamRegionBounds[readout]["y1"]
    y2 = uvars.dictScicamRegionBounds[readout]["y2"]
     
    cx_new = cx - x1 + 1
    cy_new = cy - y1 + 1

    return [cx_new , cy_new]
       
    
##########################
# SOUL Alignment function #
###########################

def soul_offsetXY_step(os, Dx , Dy, step_size=1):
    """Apply the required offset in X Y on SOUL with steps of "step" mm to avoid loop opening"""
    print("Called Function soul_offsetXY_step from move_soul")
    steps_x = int(Dx//step_size)
    steps_y = int(Dy//step_size)
    d_x, d_y = 0, 0   #cumulative offset for x,y
    for i in range(steps_x):
        d_x =+ step_size
        #status_x = os.OffsetXYAO(step_size, 0, "LEFT")
        logger.debug("Cumulative offset X applied to SOUL = {}/{}mm".format(d_x,Dx))
    for i in range(steps_y):
        d_y =+ step_size
        #status_y = 1os.OffsetXYAO(0, step_size, "LEFT")
        logger.debug("Cumulative offset Y applied to SOUL = {}/{}mm".format(d_y,Dy))

    res_x = Dx % step_size
    res_y = Dy % step_size
    logger.debug("Applying final residual offset of {}mm : Cumulative offset X applied = {}/{}mm".format(res_x, d_x + res_x, Dx))
    #status_x = os.OffsetXYAO(res_x, 0, "LEFT")
    logger.debug("Applying final residual offset of {}mm : Cumulative offset Y applied = {}/{}mm".format(res_y, d_y + res_y, Dy))
    #status_y = os.OffsetXYAO(0, res_y, "LEFT")
    print("Function ended MOVE_SOUL")
    return status_x#, status_y

    

            
##########################
# NCPA-related functions #
##########################

def determine_filter_wavelength(sci1, sci2):
    wvl_sci1 = uvars.filters_wavelength[sci1]
    wvl_sci2 = uvars.filters_wavelength[sci2]
    print("wvl s1= {} , wvl s2= ".format(wvl_sci1, wvl_sci2))
    wvl = wvl_sci2 if wvl_sci1 == None else wvl_sci1

    return wvl
    
def label_coefficients(coefficients):
    labels = ['AstO','AstY','ComY','ComX','TriY','TriO','Sph','AstYII','AstOII','TetY','TetO',
              'ComXII','ComYII','TriOII','TriYII','PenY','PenX','SphII', 'AstOIII', 'AstYIII',
              'TetOII', 'TetYII', 'EsaY', 'EsaX', 'ComYIII', 'ComXIII', 'TriYIII', 'TriOIII',
              'PentaXII', 'PentaYII', 'EptaY', 'EptaX']
    
    l = len(labels)
    c = len(coefficients)

    if c<=l:
        labels = labels[0:c]
    else:
        modes = ['Mode' + str(m) for m in range(l, c)]
        labels = labels + modes

    return list(zip(labels, coefficients))


def ncpa_masterize(fitsDir, filename, master_array):
    '''
    Apply  the CDS techique on the image and mean it over the number of images.
    Returns the master_array with 6 images.
    '''

    from alignment.utils import read_image
    from alignment.utils import remove_hot_pixels

    img_arr = []
    for image in filename:
        img = read_image(fitsDir+image, header=False, stack='', crop=1)
        logger.debug("calculate img minus dark")
        img_minus_dark = img[-1] - img[0] # always 2 slices as NGROUPS=2 in .ref
        img_arr.append(img_minus_dark)

    logger.debug("calculate mean")
    master_img = np.mean(np.array(img_arr), axis=0)
    logger.debug("find and remove hot pixels")
    master_img,_ = remove_hot_pixels(master_img, num_iter=3)

    master_array.append(master_img)

    return master_array


def test_idl_bridges():
    from idlpy import IDL
    IDL.run("print, 'THIS COMES FROM IDL'")
    IDL.run("too='THIS TOO COMES FROM IDL'")    
    idltoo = IDL.too
    del IDL
    return idltoo

def pre_process_PD(master_array, window=[[0,0,128]]):
    '''
    Prepare the images for ONERA. This function is the translation of the IDL routine
    preprocess_sw.pro.
    '''
    
    type_input=type(master_array)
    Nim = len(master_array)
 
    if Nim > 1 and len(window)== 1:
       window = [item for item in window for _ in range(Nim)] 
     
    #if Nim == 1:
    #   imageName=replicate(imageName,2)
    #   Nim++
    
    output_images=[]
    for i,img in enumerate(master_array):       
        if type_input==str:
            #if '.raw' in img:
            img=fits.getdata(img)
        
        # Crop the image
        img=img[window[i][1]:window[i][1]+window[i][2],window[i][0]:window[i][0]+window[i][2]]
        
        # Flux normalization
        Tot=img.sum()
        if Tot < 0:
            print("WARNING!! Image {}: Integrated counts less than 0! Using dynamic flux estimation".format(i))
            s=aly(camera='HAWAII H-2RG')
            s.PSFNoCoro.LoadImage(img)
            s.PSFNoCoro.calculate_bkg()
            Tot = s.PSFNoCoro.flux
        img/=Tot
        
        output_images.append(img)
    
    return output_images

def call_onera(master_array, occultation=0.1, wavelength=1.593):
    from idlpy import IDL
    
    logger.debug("Prepare defocuses")

    # Configured to use all 6 images in master_array
    #          -----focus fiber----    -------defocus fiber -----
    #          hole  pd_neg  pd_pos    hole     pd_neg  pd_pos
    defocuses = [0, -26.378, +26.378, -13.189, -39.568,  +13.189 ]
    n_images = len(defocuses)
    
    # Sort images: focus HOLE3 first and then the others ins ascending order of defocus 
    master_array[1:] = [a for _,a in sorted(zip(defocuses[1:],master_array[1:]))]
    defocuses = [0, -39.568, -26.378, -13.189, +13.189, +26.378]
    
    # # TEST DANIELE : pop out the focus HOLE3 image.
    # master_array = master_array[1:]
    # defocuses = defocuses[1: n_images] 
    # n_images = len(defocuses)

    try:
        # Parameters for ONERA
        logger.debug("Declare parameters in IDL")
        # IDL.master_array = np.array(master_array)
        IDL.Delta_z = np.array(defocuses)
        IDL.Fnum = 32.1
        IDL.wavelength = wavelength # 1.593 Associated to SCI_FILT_W1=NB_H2
        # IDL.wavelength = 1.282 # argos
        IDL.defoc_array_a4 = np.tile([+0.,+0.,+0.],(n_images,1))
        IDL.nModes = 36
        IDL.opt = 'full'
        IDL.windou = np.array([0,0,128])
        IDL.pup_mask = 0
        IDL.visu = 0 # don't display
        IDL.occultation = occultation
    except Exception as e:
        logger.error(e)
        
    # Calling Daniele's run_pd IDL routine that calls ONERA code.
    #logger.debug("Compliling wrapper run_pd")
    #IDL.run("cd, '{}'".format("/usr/local/harris/idl87/lib/PD_IDL"))
    #IDL.run(".compile run_pd")

    # Prepare images for ONERA (old IDL function pre_process_sw.pro)
    logger.debug('IDL Initialization Completed ')
    images_input = pre_process_PD(master_array)
    IDL.images_input = np.array(images_input)

    logger.debug("Calling wrapper run_pd")
    IDL.run("run_pd, images_input, Delta_z, Fnum, wavelength, nModes, opt, defoc_array_a4, pup_mask=pup_mask, occultation=occultation, Zernikes=Zernikes, Niter=Niter, diversities=diversities, pup_diam=pup_diam, oversample=oversample, visu=visu")


    # Print outputs on the screen
    np.set_printoptions(precision=2,suppress=True)
    logger.debug("\nNumber of iterations to reach convergence: {}".format(IDL.Niter))
    
    logger.debug("\nReconstructed defoci in lambda ptv:")#logger.debug("\nDiversities in rad rms (first mode is tip):")
    logger.debug(IDL.diversities[:,2]/2./np.pi*2*np.sqrt(3))
    
    logger.debug("\nReconstructed relative Tip/Tilt in pixels:")
    logger.debug(4/np.pi*IDL.oversample*IDL.diversities[:,0:2])    
    
    logger.debug("Enumerating coefficients in nm:")
    zernikes = IDL.Zernikes[1:]*1e3*IDL.wavelength/2./np.pi #[nm]
    zernikes_names = label_coefficients(zernikes)
    
    for element, value in zernikes_names:
        logger.debug("Element : % 8s  -  Value %6.2f    " % (element, value))
        
    logger.debug("\nTotal WFE in nm: %6.2f" %(np.sqrt(np.sum(zernikes**2))))

    logger.debug([round(value, 2) for (element, value) in zernikes_names])

    #IDL.exit()
    del IDL

    # #######################
    # # FUCKING IDL modifies the SIGINT catch. I have to restore it here
    # from sequencer import Sequencer
    # import signal
    # sequencer = Sequencer(embedded=False)
    # # Calling here instead of in Sequencer constructor due to Flask bug
    # signal.signal(signal.SIGINT, sequencer.interrupt)
    # #######################
    
    return zernikes.tolist()

def saveFitFast(data, namefile = "fits_file.fits"):
    from datetime import date
    fitsDir = uvars.fitsDir + str(date.today()) + "/"
    from astropy.io import fits
    hdu = fits.PrimaryHDU(np.array(data))
    hdul = fits.HDUList([hdu])
    hdul.writeto(fitsDir + namefile)
    
def zernikes_to_modes(zern, filename="221104_fibers.txt"):
    '''Trasform zernikes coefficients to DM modes using a reconstructor matrix in a file'''

    base_path = "/opt/tan/lib/python/site-packages/shins/templates/alignment/PD_IDL/recmats/"

    logger.debug("function zernike_to_modes() is using the reconstructor :\"{}\" ".format(filename))
    
    zernikes = [i for j, i in enumerate(zern) if j not in [6,30,31]]
    with open(base_path + filename, 'rb') as f:
        reader = csv.reader(f)
        reconstructor = list(reader)
        
    reconstructor = np.array(reconstructor, dtype=float)
    modes = - np.dot(zernikes, reconstructor)
    modes = np.round(modes,3)
    return modes.tolist()

    

def rotate_modes(vec, theta):
    # if_filename = r"/home/shinsmgr/Downloads/BAX479-IF.mat"
    if_filename = r"/opt/tan/etc/tiptilt_ctrl/rtc/BAX479-IF.mat"
    m2c_filename   = r"/opt/tan/etc/tiptilt_ctrl/rtc/m2cs/20220603-Zernike-FullPupil-Arcetri-1u-rms-4rtc.dat"
    #m2c_filename   = r"/home/shinsmgr/Downloads/BAX479-Z2C.mat"

    vec = np.array(vec)
    
    dm = Deformable_Mirror(if_filename, m2c_filename)
    vec_new = dm.rotate_NCPA(vec, theta, display=False)

    return vec_new


def rotate_modes_with_drot(os, sleep=10, threshold=0.5):
    
    riservati = [0,0]

    sign = -1 # rotation direction
    
    ins_s = {"DROT":-9999}
    angle_where_ncpa_was_optimized = float(os.getPosition("DEGREE", ins_s)[1]["DROT"])

    status, total_vector = os.getInternalSeqFloatVariable("RtcAppliedMode")
    mode_vector = total_vector[2:]
    
    #os.RTCTTModeUpload( riservati + mode_vector, absolute=True)
    
    position_of_last_update = angle_where_ncpa_was_optimized
    
    while True:
        
        bearing = float(os.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 = rotate_modes(mode_vector, sign * (bearing - angle_where_ncpa_was_optimized) )[0:36]
            os.RTCTTModeUpload( riservati + list(ruotati), absolute=True)
            position_of_last_update = bearing
        else:
            print("Below threshold: no mode rotation")            

        time.sleep(sleep)

################################    
# Derotation-related functions #
################################

def drot_step2deg(step_pos):
    drot_deg = step_pos*(8.7890625e-06) -132.6219
    return drot_deg

def set_fieldfixed_derotator_parameters(os, ref_pos=25, slit="narrow"):
    if slit=="narrow":
        slit_offset = 0 #1.8
    elif slit == "large":
        slit_offset = 0 #2.5
        

    ref_pos += slit_offset
    absencPos = int(65536 - (float(ref_pos) + 132.6219)*(65536.0/360.0) )
    # os.setDerotationParameters3(debug1 = 1, debug2 = -1, debug3=0)
    # os.setDerotationParameters4(debug4 = absencPos, debug5=0, debug6=0)
    os.setDerotationParameters3(1,-1,0)
    os.setDerotationParameters4(absencPos,0,0)
    print("Derotation Parameters modified")

#################################
# Coordinates-related functions #
#################################

    
def to_radians(tel_s):
    """Marco version for coordinate to radian conversion"""
    
    # print(tel_s)

    logger.info("Converting coordinates to radian")

    tel_s["Epoch"] = str(Time.now().jyear)
    
    if ('OBJRA' in tel_s) and ('OBJDEC' in tel_s):
        obsCoord = SkyCoord(tel_s['OBJRA'], tel_s['OBJDEC'], unit=("hourangle", "deg"))
        tel_s['OBJRA'] = '{}'.format(obsCoord.ra.radian)
        tel_s['OBJDEC'] = '{}'.format(obsCoord.dec.radian)

    if ('PMRA' in tel_s) and ('PMDEC' in tel_s):
        obsCoord = SkyCoord(tel_s['PMRA'], tel_s['PMDEC'], unit=("mas", "mas"))
        tel_s['PMRA'] = '{}'.format(obsCoord.ra.wrap_at(np.pi*u.rad).radian ) 
        tel_s['PMDEC'] = '{}'.format(obsCoord.dec.wrap_at(np.pi*u.rad).radian)

    if ('GSRA' in tel_s) and ('GSDEC' in tel_s):
        obsCoord = SkyCoord(tel_s['GSRA'], tel_s['GSDEC'], unit=(u.hourangle, u.degree))
        tel_s['GSRA'] = '{}'.format(obsCoord.ra.radian)
        tel_s['GSDEC'] = '{}'.format(obsCoord.dec.radian)

    if ('PMGSRA' in tel_s) and ('PMGSDEC' in tel_s):
        obsCoord = SkyCoord(tel_s['PMGSRA'], tel_s['PMGSDEC'], unit=("mas", "mas"))
        tel_s['PMGSRA'] = '{}'.format(obsCoord.ra.wrap_at(np.pi*u.rad).radian ) 
        tel_s['PMGSDEC'] = '{}'.format(obsCoord.dec.wrap_at(np.pi*u.rad).radian)
                
    if ('AORA' in tel_s) and ('AODEC' in tel_s):
        obsCoord = SkyCoord(tel_s['AORA'], tel_s['AODEC'], unit=(u.hourangle, u.degree))
        tel_s['AORA'] = '{}'.format(obsCoord.ra.radian)
        tel_s['AODEC'] = '{}'.format(obsCoord.dec.radian)

    if ('PMAORA' in tel_s) and ('PMAODEC' in tel_s):
        obsCoord = SkyCoord(tel_s['PMAORA'], tel_s['PMAODEC'], unit=("mas", "mas"))
        tel_s['PMAORA'] = '{}'.format(obsCoord.ra.wrap_at(np.pi*u.rad).radian ) 
        tel_s['PMAODEC'] = '{}'.format(obsCoord.dec.wrap_at(np.pi*u.rad).radian)

    if ('OFFSETX' in tel_s) and ('OFFSETY' in tel_s):
        obsCoord = SkyCoord(tel_s['OFFSETX'], tel_s['OFFSETY'], unit=("arcsec", "arcsec"))
        tel_s['OFFSETX'] = '{}'.format(obsCoord.ra.wrap_at(np.pi*u.rad).radian ) 
        tel_s['OFFSETY'] = '{}'.format(obsCoord.dec.wrap_at(np.pi*u.rad).radian)
    
    return tel_s


def save_flat(os, thread):
    """Save the current flat in a file"""

    # Time
    now = Time.now().datetime.strftime("%Y-%m-%d-%H-%M-%S")

    # T, seeing, el, dome
    try:
        params = ["LBTTemp", "DIMMSeeing", "ELPosition", "L_ShutterOpen", "L_SWA_Argos_Deployed"]
        dd_params = os.getDdParameters(params)[1][1:]
        temp, seeing, el, dome_open, swing_arm = [int(round(float(p))) for p in dd_params]
        el /= 3600

    except Exception as e:
        print("Cannot get some dd param")
        print(e)
        dd_params = range(5)
        temp, seeing, el, dome_open, swing_arm = ["err" for p in dd_params]

    # int ext
    try:
        status, json_string = os.getJsonStatus()
        all_devs = json.loads(json_string)
        
        shutter_in = all_devs["SHUTTER"]["named"] == "IN"
        some_fiber_on = all_devs["CAL_FIBER_FOCUS_LAMP"]["named"] == "ON" or all_devs["CAL_FIBER_DEFOCUS_LAMP"]["named"] == "ON"

        if some_fiber_on:
            source = "int"
        elif (not some_fiber_on) and (not dome_open) and (not swing_arm):
            source = "bar"
        elif (not some_fiber_on) and swing_arm:
            source = "arg"
        elif (not some_fiber_on) and dome_open:
            source = "sky"
        else:
            source = "boh"
    except Exception as e:
        print("Cannot get JSON")
        print(e)
        source = "err"

    # b
    try:
        status, bearing = os.getPosition("DEGREE", {"DROT":-9999})
        bearing = int(round(float(bearing.values()[0])))
    except Exception as e:
        print("Cannot get derot position")
        print(e)
        bearing = "err"

    # Example name: dm_shape_2022-11-24-19-22-33_arg_95b_90el_-4C_1as.txt
    flat_path = "/opt/tan/etc/tiptilt_ctrl/rtc/dm_flats/"
    flat_name = "dm_shape_{}_{}_{}b_{}el_T{}C_{}as.txt".format(now, source, bearing, el, temp, seeing)
    flat_total = flat_path + flat_name

    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)
    
    with open(str(flat_total), "w") as flat_file:
        for i in range(len(thread.shape)):
            flat_file.write("{}\n".format(thread.shape[i]*signs[i]))

    return flat_name

def save_ncpa(os):
    """Save the currentncpa modes in a file"""

    # Time
    now = Time.now().datetime.strftime("%Y-%m-%d-%H-%M-%S")

    # T, seeing, el, dome
    try:
        params = ["LBTTemp", "DIMMSeeing", "ELPosition", "L_ShutterOpen", "L_SWA_Argos_Deployed"]
        dd_params = os.getDdParameters(params)[1][1:]
        temp, seeing, el, dome_open, swing_arm = [int(round(float(p))) for p in dd_params]
        el /= 3600

    except Exception as e:
        print("Cannot get some dd param")
        print(e)
        dd_params = range(5)
        temp, seeing, el, dome_open, swing_arm = ["err" for p in dd_params]

    # int ext
    try:
        status, json_string = os.getJsonStatus()
        all_devs = json.loads(json_string)
        
        shutter_in = all_devs["SHUTTER"]["named"] == "IN"
        some_fiber_on = all_devs["CAL_FIBER_FOCUS_LAMP"]["named"] == "ON" or all_devs["CAL_FIBER_DEFOCUS_LAMP"]["named"] == "ON"

        if some_fiber_on:
            source = "int"
        elif (not some_fiber_on) and (not dome_open) and (not swing_arm):
            source = "bar"
        elif (not some_fiber_on) and swing_arm:
            source = "arg"
        elif (not some_fiber_on) and dome_open:
            source = "sky"
        else:
            source = "boh"
    except Exception as e:
        print("Cannot get JSON")
        print(e)
        source = "err"

    # b
    try:
        status, bearing = os.getPosition("DEGREE", {"DROT":-9999})
        bearing = round(float(bearing.values()[0]))
    except Exception as e:
        print("Cannot get derot position")
        print(e)
        bearing = "err"

    # Example name: ncpa_2022-11-24-19-22-33_arg_95b_90el_-4C_1as.txt
    ncpa_path = "/opt/tan/etc/tiptilt_ctrl/rtc/ncpa_modes/"
    ncpa_name = "ncpa_{}_{}_{}b_{}el_T{}C_{}as.txt".format(now, source, int(bearing), el, temp, seeing)
    ncpa_total = ncpa_path + ncpa_name

    status, total_vector = os.getInternalSeqFloatVariable("RtcAppliedMode")
    with open(str(ncpa_total), "w") as ncpa_file:
        for i in range(len(total_vector[0:36])):
            ncpa_file.write("{}\n".format(total_vector[i]))
        ncpa_file.write("{}\n".format(bearing))

    return ncpa_name


def checkLBTAuthorization(os):
    try:
        status, instruments = os.getDdParameters(["L_Instrument","R_Instrument"])
        if instruments:
            context_ins = {uvars.lbt_L_ins:instruments[1] , uvars.lbt_R_ins:instruments[2]}
            status = os.setContext(context_ins)
            logger.info("LBT Authorized {} on LEFT side, {} on RIGHT side".format(instruments[1],instruments[2]))
            return instruments[1:]
        else:
            logger.warning("Unable to receive info from LBT DataDictionary")
            logger.warning("Probably there is a problem with TCS Service, try to restart tcs_ctrl-svr")
            return [None,None]
    except Exception as e:
        logger.warning(e)
        logger.warning("Unable to receive info from LBT DataDictionary")
        if "Worker is not online" in e:
            logger.warning("Probably there is a problem with TCS Service, try to restart tcs_ctrl-svr")
        return [None,None]

def error_handler(status, method=""):
    if status != uvars.EXIT_SUCCESS:
        msg = "{:s} returned EXIT_FAILURE".format(method)
        raise exception.TemplateException(msg)
    
