'''
Codigo para seleccionar en un rango de +-z
Los datos de filtro estan en el plano z = 0
'''
import orthopoly
import numpy as np
import Codigo2D_CodigoBaseFiltro as c4 
# from Codigo1 import msph_mcart,mcart_msph

def public_c2s(posicionescart):
    '''
    De cartesianas a esfericas (c2s) de forma publica
    '''
    x,y,z       = posicionescart.T
    r,theta,phi = orthopoly.spherical_harmonic.cart2sph(-x,-y,z) #Bug in library
    return np.asarray([r,theta,phi]).T

def public_s2c(posicionesesf):
    '''
    De esfericas a cartesianas (s2c) de forma publica
    '''
    r,theta,phi = posicionesesf.T
    x,y,z       = orthopoly.spherical_harmonic.sph2cart(r,theta,phi)
    return np.asarray([x,y,z]).T

def msph_mcart(matrizsph):
    tiempo = matrizsph.shape[0]
    matrizcart = (public_s2c(matrizsph.reshape(-1, 3))).reshape(tiempo, -1, 3)
    return matrizcart

def mcart_msph(matrizcart):
    tiempo = matrizcart.shape[0]
    matrizsph = (public_c2s(matrizcart.reshape(-1, 3))).reshape(tiempo, -1, 3)
    return matrizsph

def funciondefiltro(Datoscart,selectz,cte):
    
    '''
    # cte es para ajustar las unidades, el codigo da del orden de 10**1
    '''
    
    Datosesf = mcart_msph(Datoscart)

    Datosesf[:,:,0] = Datosesf[:,:,0]/(cte) # Normalizamos
    Datoscart = msph_mcart(Datosesf)
    
    contador = c4.contarpuntos(selectz,Datoscart) # Contamos el numero de puntos que cumplen la condicion en cada tiempo
    filtro1 = c4.datosfiltrados(selectz,Datoscart) # Filtramos los puntos (nota: la matriz contiene filas de ceros, el numero de filas de ceros varian en el tiempo)
    filtroesf1 = c4.mcart_msph_filtrado(filtro1)

    return filtro1,filtroesf1

def filtroz(Datoscart,selectz,cte):
    '''
    Codigo para hacer la proyeccion (sigue manteniendo 0's  pero interpolar ignora los 0's)
    '''
    f1,esf1 = funciondefiltro(Datoscart,selectz,cte)
    
    FiltroCart = np.zeros_like(f1)
    FiltroEsf = np.zeros_like(f1)
    
    FiltroEsf[:,:,0] = np.sqrt(f1[:,:,0]**2 + f1[:,:,1]**2)
    FiltroEsf[:,:,1][esf1[:,:,1] != 0] = np.pi/2 # da igual poner el esf1[1]
    FiltroEsf[:,:,2] = esf1[:,:,2]
    
    FiltroCart = c4.msph_mcart_filtrado(FiltroEsf)
    FiltroCart[:,:,2] = 0
    
    return FiltroCart,FiltroEsf