import numpy as np
import pandas as pd
import os
from scipy import interpolate
import Get_real_position_axis
from scipy import signal


def get_calibrated_position_axis(position_axis, scale):
    items = os.listdir(".")
    for names in items:
        if names.endswith("parameters_int.txt"):
            filename = names

    ref = np.genfromtxt(filename, delimiter='\t')
    position_ref = (ref[0, :])
    amplitude_ref = (ref[1, :])

    items = os.listdir(".")
    for names in items:
        if names.endswith("parameters_scale.txt"):
            filename = names

    ref = pd.read_csv(filename, sep="\t", header=None)
    first_row = (ref.iloc[0])
    scale_read = first_row.to_numpy(dtype='float64')

    calibrated_position_axis=[]

    rel_scale = (scale - scale_read) / 1000000

    position_axis = position_axis + rel_scale   # shift to reference of the motor
    position_axis = np.asarray(position_axis).squeeze()

    factor = np.ceil((np.mean(np.diff(position_axis))) / (np.mean(np.diff(position_ref))))  # ratio between position axes

    f = interpolate.interp1d(position_axis, position_axis, kind='cubic')
    oversmp_pos = f(np.linspace(position_axis[0], position_axis[-1], int(factor) * np.shape(position_axis)[0]))     #oversampled position axis

    f1 = interpolate.interp1d(position_ref, amplitude_ref, kind='cubic')
    ref_interp = f1(oversmp_pos)        #interpolated reference on the position axis

    # Find the peaks of the interpolated reference and it gets their indexes
    locs = signal.find_peaks(ref_interp)
    indexes = locs[0]
    #left_index = indexes[0]
    #right_index = indexes[-1]

    calib_pos_axis_overs = Get_real_position_axis.get_real_position_axis(ref_interp) * (
            position_axis[-1] - position_axis[0]) + position_axis[0] - rel_scale        #oversampled calibrated position axis

    calibrated_position_axis = calib_pos_axis_overs[0:-1:int(factor)]
    calibrated_position_axis = np.array(calibrated_position_axis)

    # Compute the indexes factorized
    left_index = int(np.ceil(indexes[0] / factor))
    right_index = int(left_index + len(calibrated_position_axis) - 1)

    return calibrated_position_axis

    '''
    calibrated_position_axis.append(calib_pos_axis_overs[1])
    downsample_index=int(len(calib_pos_axis_overs)/factor)
    for i in range(1, downsample_index):
        index_value = calib_pos_axis_overs[i * int(factor)]
        calibrated_position_axis.append(index_value)

    calibrated_position_axis = np.array(calibrated_position_axis)

    scale_fact = np.mean(np.diff(position_axis)) / np.mean(np.diff(calibrated_position_axis))   #normalize differentials

    calibrated_position_axis = calibrated_position_axis * scale_fact

    return calibrated_position_axis
    '''
