import numpy as np
from scipy.optimize import minimize
import logging

log = logging.getLogger(__name__)

class PointingModel():

    obj_list = [] # object coordinate list
    off_list = [] # offset list

    omega = (360.0 / 86164.099) * (np.pi/180.0) ### rotationrate of equatorial axis

    ##########################
    #### model parameters ####
    ##########################

    ### Encoder error ###
    C_Encoder_tau = 0.0
    C_Encoder_dec = 0.0

    ### Misalignment of Polar Axis ###
    # Azimuth
    C_mAl_tau = 0.0
    C_mAl_dec = 0.0

    # Elevation (Latitude)
    C_mEl_tau = 0.0
    C_mEl_dec = 0.0

    ### Refraction ###
    C_Refraction_tau = 0.0
    C_Refraction_dec = 0.0

    ### Collimation ###
    C_Coll_tau = 0.0
    C_Coll_dec = 0.0

    ### Tube Flexure ###
    C_Flex_tau = 0.0
    C_Flex_dec = 0.0

    ### Skewness of the Axes ###
    C_NP_tau = 0.0

    ### Empirical errors ###
    C_E1_tau = 0.0
    C_E2_tau = 0.0
    #C_E1_dec = 0.0 # test variables for declination for empirical errors.
    #C_E2_dec = 0.0


    def __init__(self, latitude, use_old_model = False):


        self.latitude = latitude

        if use_old_model is True:
            self.LoadPointingModel()


    def add_obj_taudec(self, TAU_obj, DEC_obj, TAU_off, DEC_off):
        self.obj_list.append([TAU_obj, DEC_obj])
        self.off_list.append([TAU_off, DEC_off])


    def ErrorEstimation(self, TAU_obj, DEC_obj, parameter, use_simple_model = False, return_tracking = False):
        '''
            This function calculates the error of the equatorial axis of the telescope.
            Input:
            - TAU_obj: Hour angle value or 1d-list of the object of interest
            - DEC_obj: Declination angle value or 1d-list of the object of interest
            - parameterTau: parameters of the Pointing model for the equatorial axis
            - use_simple_model: parse True for using only the simple Pointing model
        '''
        TAU_obj *= np.pi / 180.0
        DEC_obj *= np.pi / 180.0

        ### pre-calculate repeating sine and cosine functions
        sin_TAU = np.sin(TAU_obj)
        cos_TAU = np.cos(TAU_obj)
        sin_DEC = np.sin(DEC_obj)
        cos_DEC = np.cos(DEC_obj)

        latitude = self.latitude * (np.pi/180.0)
        sin_lat = np.sin(latitude)
        cos_lat = np.cos(latitude)

        x = np.sin(DEC_obj) * sin_lat + np.cos(DEC_obj) * cos_lat * cos_TAU
        altitude = np.arcsin(x)
        alt_diff = -self.omega * (1/np.sqrt(1 - x**2)) * np.cos(DEC_obj) * cos_lat * np.sin(TAU_obj)
        sin_alt = np.sin(altitude)
        cos_alt = np.cos(altitude)

        ### TAU offset ###
        c_mal_tau = parameter[0]
        c_mel_tau = parameter[1]
        c_coll_tau = parameter[2]
        c_refraction_tau = parameter[3]
        c_encoder_tau = parameter[4]
        c_flex_tau = parameter[5]
        c_np_tau = parameter[6]
        c_e1_tau = parameter[7]
        c_e2_tau = parameter[8]

        ### DEC offset ###
        c_mal_dec = parameter[9]
        c_mel_dec = parameter[10]
        c_coll_dec = parameter[11]
        c_refraction_dec = parameter[12]
        c_flex_dec = parameter[13]
        #c_e1_dec = parameter[14]
        #c_e2_dec = parameter[15]

        track_tau_esti = 0.0
        track_dec_esti = 0.0
        off_tau_esti = 0.0 # estimate tau offset from the pointing model
        off_dec_esti = 0.0 # estimated dec offset from the pointing model

        ### TAU offset ###
        off_tau_esti = -c_mal_tau * ((cos_TAU * sin_DEC) / cos_DEC)
        off_tau_esti += c_mel_tau * ((sin_TAU * sin_DEC) / cos_DEC)
        off_tau_esti += c_coll_tau
        if return_tracking is True:
            track_tau_esti += c_mal_tau * self.omega * ((sin_TAU * sin_DEC) / cos_DEC)
            track_tau_esti += c_mel_tau * self.omega * ((cos_TAU * sin_DEC) / cos_DEC)

        ### DEC offset ###
        off_dec_esti =  c_mal_dec * sin_TAU
        off_dec_esti += c_mel_dec * cos_TAU
        off_dec_esti += c_coll_dec
        if return_tracking is True:
            track_dec_esti += c_mal_dec * self.omega * cos_TAU
            track_dec_esti += -c_mel_dec * self.omega * sin_TAU

        if use_simple_model is False:

            ### TAU offset ###
            off_tau_esti += c_refraction_tau * ((sin_TAU * cos_lat) / (sin_alt * cos_DEC))
            off_tau_esti += c_flex_tau * ((sin_TAU * cos_lat) / cos_DEC)
            off_tau_esti += c_encoder_tau / cos_DEC
            off_tau_esti += -c_np_tau * (sin_DEC / cos_DEC)
            off_tau_esti += -c_e1_tau * cos_TAU - c_e2_tau * np.cos(4 * TAU_obj)
            if return_tracking is True:
                track_tau_esti += c_refraction_tau * self.omega * ((cos_TAU * cos_lat) / (sin_alt * cos_DEC)) # 1st derivative term
                track_tau_esti += -c_refraction_tau * alt_diff * cos_alt * ((sin_TAU * cos_lat) / (sin_alt**2 * cos_DEC)) # 2nd term
                track_tau_esti += c_flex_tau * self.omega * ((cos_TAU * cos_lat) / cos_DEC)
                track_tau_esti += self.omega * (c_e1_tau * sin_TAU + 4 * c_e2_tau * np.sin(4 * TAU_obj))

            ### DEC offset ###
            off_dec_esti += c_refraction_dec * ((sin_lat - sin_alt * sin_DEC) / (cos_DEC * sin_alt))
            off_dec_esti += -c_flex_dec * ((sin_lat - sin_alt * sin_DEC) / cos_DEC)
            #off_dec_esti += -c_e1_dec * cos_DEC - c_e2_dec * np.cos(4 * DEC_obj)
            if return_tracking is True:
                track_dec_esti += -c_refraction_dec * alt_diff * ((cos_alt * sin_DEC) / (cos_DEC * sin_alt)) # 1st term
                track_dec_esti += -c_refraction_dec * alt_diff * cos_alt * ((sin_lat - sin_alt * sin_DEC) / (cos_DEC * sin_alt**2)) # 2nd derivative term
                track_dec_esti += c_flex_dec * alt_diff * ((cos_alt * sin_DEC) / cos_DEC)

        if return_tracking is True:
            return off_tau_esti, off_dec_esti, track_tau_esti, track_dec_esti

        else:
            return off_tau_esti, off_dec_esti


    ### TODO change the given coordinate for the correction
    def CorrectPointing(self, TAU_obj, DEC_obj, use_simple_model = False, return_tracking = False):
        '''
            Apply the pointing model to correct the heading of the telescope to calculate the correction to the wished Object.
            Input:
            - TAU_obj: Hour angle coordinate of the object.
            - DEC_obj: Declination coordinate of the object.
            - use_simple_model: True/False if you want to use the full model or only the polar alignment model!

            Return:
            - TAU_correction: Correction of the hour angle
            - DEC_correction: Correction of the declination
        '''
        parameter = [self.C_mAl_tau, self.C_mEl_tau, self.C_Coll_tau, self.C_Refraction_tau, self.C_Encoder_tau, self.C_Flex_tau, self.C_NP_tau, self.C_E1_tau, self.C_E2_tau, self.C_mAl_dec, self.C_mEl_dec, self.C_Coll_dec, self.C_Refraction_dec, self.C_Flex_dec] #, self.C_E1_dec, self.C_E2_dec]

        if return_tracking is False:
            delta_tau_esti, delta_dec_esti = self.ErrorEstimation(TAU_obj, DEC_obj, parameter, use_simple_model, return_tracking)

        else:
            delta_tau_esti, delta_dec_esti, track_tau_esti, track_dec_esti = self.ErrorEstimation(TAU_obj, DEC_obj, parameter, use_simple_model, return_tracking)
            track_tau_esti += self.omega # Base tracking speed for hour angle!
            track_tau_esti *= 180.0/np.pi

            track_dec_esti *= 180.0/np.pi

        TAU_tele = (TAU_obj + delta_tau_esti) % 360.0
        DEC_tele = DEC_obj + delta_dec_esti
        TAU_tele = (TAU_tele + 180) % 360
        if DEC_tele > 90.0:
            DEC_tele = 90.0 - (DEC_tele - 90.0)
            TAU_tele = (TAU_tele + 180) % 360
        if DEC_tele < -90.0:
            DEC_tele = -90.0 + (-90.0 - DEC_tele)
            TAU_tele = (TAU_tele + 180) % 360
        TAU_tele = self.fulltohalfsides(TAU_tele) # convert range [0, 360.0) into (-180.0, 180.0] # monti needs this shape!

        if return_tracking is False:
            return TAU_tele, DEC_tele
        else:
            return TAU_tele, DEC_tele, track_tau_esti, track_dec_esti


    def Inversion(self, TAU_tele, DEC_tele, use_simple_model = False):

        ### Inversion by initial guess (using the correction with telescope coordinates as input)
        ### The solution is very rough
        parameter = [self.C_mAl_tau, self.C_mEl_tau, self.C_Coll_tau, self.C_Refraction_tau, self.C_Encoder_tau, self.C_Flex_tau, self.C_NP_tau, self.C_E1_tau, self.C_E2_tau, self.C_mAl_dec, self.C_mEl_dec, self.C_Coll_dec, self.C_Refraction_dec, self.C_Flex_dec] #, self.C_E1_dec, self.C_E2_dec]
        off_tau_esti, off_dec_esti = self.ErrorEstimation(TAU_tele, DEC_tele, parameter, use_simple_model)
        TAU_obj_init = (TAU_tele - off_tau_esti)%360.0
        DEC_obj_init = DEC_tele - off_dec_esti

        #return TAU_obj_init, DEC_obj_init

        ### Inversion by iterating in 1' steps to find the optimal object coordinate for the given telescope coordinate
        ### Shown to work the best
        TAU_obj_range = np.linspace(TAU_obj_init - 2.0, TAU_obj_init + 2.0, int((4.0 * 3600)/1.0))%360.0  # search coordinate range in 1'' steps
        DEC_obj_range = np.linspace(DEC_obj_init - 2.0, DEC_obj_init + 2.0, int((4.0 * 3600)/1.0))        # search coordinate range in 1'' steps
        indices = DEC_obj_range > 90.0
        TAU_obj_range[indices] = (TAU_obj_range[indices] + 180.0) % 360.0
        DEC_obj_range[indices] = 90.0 - (DEC_obj_range[indices] - 90.0)
        indices = DEC_obj_range < -90.0
        TAU_obj_range[indices] = (TAU_obj_range[indices] + 180.0) % 360.0
        DEC_obj_range[indices] = -90.0 + (-90.0 - DEC_obj_range[indices])


        off_tau_esti, off_dec_esti = self.ErrorEstimation(TAU_obj_range.copy(), DEC_obj_range.copy(), parameter, use_simple_model)

        ### Fix overlaps and cycles
        TAU_tele_esti = (TAU_obj_range + off_tau_esti) % 360.0
        DEC_tele_esti = DEC_obj_range + off_dec_esti
        TAU_tele_esti = (TAU_obj_range + off_tau_esti) % 360.0
        DEC_tele_esti = DEC_obj_range + off_dec_esti
        indices = DEC_tele_esti > 90.0
        TAU_tele_esti[indices] = (TAU_tele_esti[indices] + 180) % 360
        DEC_tele_esti[indices] = 90.0 - (DEC_tele_esti[indices] - 90.0)
        indices = DEC_tele_esti < -90.0
        TAU_tele_esti[indices] = (TAU_tele_esti[indices] + 180) % 360
        DEC_tele_esti[indices] = -90.0 + (-90.0 - DEC_tele_esti[indices])




        ### Calculate estimation minimum to find true difference
        TAU_DIFF = np.abs(TAU_tele_esti - TAU_tele%360.0)
        DEC_DIFF = np.abs(DEC_tele_esti - DEC_tele)
        #print("TAU mini: ", np.min(np.abs(TAU_tele_esti - TAU_tele%360.0)), "DEC mini: ", np.min(np.abs(DEC_tele_esti - DEC_tele)))
        tau_argmin = np.argmin(TAU_DIFF)
        dec_argmin = np.argmin(DEC_DIFF)
        TAU_obj = TAU_obj_range[tau_argmin]
        DEC_obj = DEC_obj_range[dec_argmin]
        return TAU_obj, DEC_obj  ### Return best result

        ### Inversion by using a minimization approach and using the initial guess of the object coordinates
        ### Can give the best results but is slow and prone to errors.
        #args = [TAU_tele, DEC_tele, use_simple_model, with_order]
        #weights = [TAU_obj_init, DEC_obj_init] ### init guess
        #bounds = [(TAU_obj_init - 1.0, TAU_obj_init + 1.0), (DEC_obj_init - 1.0, DEC_obj_init + 1.0)]
        #nms = minimize(self.InversionMinimization, weights, args, method='L-BFGS-B', bounds = bounds)
        #weights = nms['x']
        #return weights[0], weights[1]


    def OptimizeModel(self, use_simple_model = False):
        '''
            Apply a optimization alogorithm to minimize the Mean Squared Root Error of the model to the observation data!
        '''
        weights = [self.C_mAl_tau, self.C_mEl_tau, self.C_Coll_tau, self.C_Refraction_tau, self.C_Encoder_tau, self.C_Flex_tau, self.C_NP_tau, self.C_E1_tau, self.C_E2_tau, self.C_mAl_dec, self.C_mEl_dec, self.C_Coll_dec, self.C_Refraction_dec, self.C_Flex_dec] #, self.C_E1_dec, self.C_E2_dec]
        print("Optimize pointing model, with ", len(self.obj_list), "positions data points!")
        args = [np.array(self.obj_list), np.array(self.off_list), use_simple_model]

        ### minimize pointing model with minimization function and an minimization algorithm
        ### OPTIMIZE WITH SCIPY OPTIMIZE.MINIMIZE
        ### SCIPY has multiple minimization algorithms, though most are no working for this use case
        ### The algorithms here are always working, though some of them have sometimes less optimal results

        #nms = minimize(self.PointingModel, weights, args, method='L-BFGS-B') ### yields consistent good results but not the best
        #nms = minimize(self.PointingModel, weights, args, method='BFGS') ### some time inconsistent and worst results of these three
        nms = minimize(self.PointingModelMinimization, weights, args, method='SLSQP') ### has shown to give best results, but is sometimes inconsistent
        weights = nms['x']
        self.UpdateParams(weights)


    def PointingModelMinimization(self, w, args):
        '''
            Simple two star pointing model. The optimization function measures the Mean Abs (or Mean Squared) of the Sky observations in the argument args for a given set of parameters/weights w.
        '''
        obj_list, off_list, use_simple_model = args

        obj_tau_list = obj_list[:, 0]
        obj_dec_list = obj_list[:, 1]
        off_tau_esti, off_dec_esti = self.ErrorEstimation(obj_tau_list.copy(), obj_dec_list.copy(), w, use_simple_model)

        diff_tau_esti = np.abs(off_tau_esti - off_list[:, 0])
        diff_dec_esti = np.abs(off_dec_esti - off_list[:, 1])
        diff_tau_esti = np.sort(diff_tau_esti) ### prepare order for outlier removal
        diff_dec_esti = np.sort(diff_dec_esti) ### prepare order for outlier removal
        length = len(diff_tau_esti)

        ### Least Mean ###
        TAU_SUM = np.mean(diff_tau_esti[:int(0.9 * length)]) ### [int(0.9 * length]: is sliced cut out outliers from data
        DEC_SUM = np.mean(diff_dec_esti[:int(0.9 * length)]) ### [int(0.9 * length]: is sliced cut out outliers from data

        ### Least Mean Squared ###
        #TAU_SUM = np.mean((diff_tau_esti[:int(0.9 * length)])**2)
        #DEC_SUM = np.mean((diff_dec_esti[:int(0.9 * length)])**2)
        print("ABS MEAN SUM TAU: ", TAU_SUM, "ABS MEAN SUM DEC: ", DEC_SUM)
        #print("ABS MEAN SUM TAU: ", np.sqrt(TAU_SUM), "ABS MEAN SUM DEC: ", np.sqrt(DEC_SUM))
        return TAU_SUM + DEC_SUM


    def PrintParams(self):

        print("WEIGHT PARAMS: {C_mAl_tau: ", self.C_mAl_tau, ", C_mEl_tau: ", self.C_mEl_tau, ", C_Coll_tau: ", self.C_Coll_tau, ", C_Refraction_tau: ", self.C_Refraction_tau, ", C_Encoder_tau: ", self.C_Encoder_tau, ", C_Flex_tau: ", self.C_Flex_tau, ", C_NP_tau: ", self.C_NP_tau, ", C_E1_tau: ", self.C_E1_tau, ", C_E2_tau: ", self.C_E2_tau, "C_mAl_dec: ", self.C_mAl_dec, ", C_mEl_dec: ", self.C_mEl_dec, ", C_Coll_dec: ", self.C_Coll_dec, ", C_Refraction_dec: ", self.C_Refraction_dec, ", C_Flex_dec: ", self.C_Flex_dec, "}") #, ", C_E1_dec: ", self.C_E1_dec, ", C_E2_dec: ", self.C_E2_dec, "}")


    def UpdateParams(self, weights):
        '''
            Rewrite the parameters from a list into the named constants.
        '''
        ### TAU weights ###
        self.C_mAl_tau = weights[0]
        self.C_mEl_tau = weights[1]
        self.C_Coll_tau = weights[2]
        self.C_Refraction_tau = weights[3]
        self.C_Encoder_tau = weights[4]
        self.C_Flex_tau = weights[5]
        self.C_NP_tau = weights[6]
        self.C_E1_tau = weights[7]
        self.C_E2_tau = weights[8]

        ### DEC weights ###
        self.C_mAl_dec = weights[9]
        self.C_mEl_dec = weights[10]
        self.C_Coll_dec = weights[11]
        self.C_Refraction_dec = weights[12]
        self.C_Flex_dec = weights[13]
        #self.C_E1_dec = weights[14]
        #self.C_E2_dec = weights[15]


    def SavePointingModel(self):
        '''
            Save the fitted parameter of the model!
        '''
        weights = [self.C_mAl_tau, self.C_mEl_tau, self.C_Coll_tau, self.C_Refraction_tau, self.C_Encoder_tau, self.C_Flex_tau, self.C_NP_tau, self.C_E1_tau, self.C_E2_tau, self.C_mAl_dec, self.C_mEl_dec, self.C_Coll_dec, self.C_Refraction_dec, self.C_Flex_dec] #, self.C_E1_dec, self.C_E2_dec]
        np.save("./pointing/pointing_model_weights.npy", weights)


    def LoadPointingModel(self):
        '''
            Load the fitted parameter of the model!
            return obj_coord_list, tele_coord_list
        '''
        log.info("Loading last pointing model weights from pointing data folder ...")
        weights = np.load("./pointing/pointing_model_weights.npy")
        self.UpdateParams(weights)


    ###########################################################################
    ############## Simple Conversion and calculation functions ################
    ###########################################################################

    def angleToDecimal(self, angle_tuple):

        sign = np.sign(angle_tuple[0])
        degree = np.abs(angle_tuple[0])%360
        minute = angle_tuple[1]%60
        second = angle_tuple[2]%60
        return sign * (degree + minute * (1/60) + second * (1/3600))

    def fulltohalfsides(self, angle):

        if angle >= 180.0:
            angle = angle % (-180.0)
        return angle

    ###########################################################################
    ########### Close the pointing model when stopping processes ##############
    ###########################################################################

    def __del__(self):

        log.info("Delete pointing model instance from process!")
