import astropy.coordinates as coord
from astropy import units as u
from pylab import *
import numpy as np
import math as m
# test

def transRaDec2D(ra, dec):
    x1 = np.cos(dec / 57.2957795) * np.cos(ra / 57.2957795);
    y1 = np.cos(dec / 57.2957795) * np.sin(ra / 57.2957795);
    z1 = np.sin(dec / 57.2957795);
    return np.array([x1, y1, z1])

def getobsPA(ra, dec):
    l1 = np.array([0,0,1])
    l2 = transRaDec2D(ra, dec)
    polar_ec = coord.SkyCoord(0*u.degree, 90*u.degree,frame='barycentrictrueecliptic')
    polar_eq = polar_ec.transform_to('icrs')

    # print(polar_eq.ra.value,polar_eq.dec.value)
    polar_d = transRaDec2D(polar_eq.ra.value, polar_eq.dec.value)
    l1l2cross = np.cross(l2,l1)
    pdl2cross = np.cross(l2,polar_d)
    angle = math.acos(np.dot(l1l2cross,pdl2cross)/(np.linalg.norm(l1l2cross)*np.linalg.norm(pdl2cross)))

    angle = (angle)/math.pi*180

    # if (ra>90 and ra< 270):
    #     angle=-angle
    return angle



def loadSatOrbitDat(datDir='',fileNum=50):
    oData = loadtxt(datDir+'1.txt')

    for i in arange(2, fileNum + 1, 1):
        tdat = loadtxt(datDir+str(i)+'.txt')
        oData = np.vstack((oData,tdat))
    
    return oData

def transRaDec2D(ra, dec):
    x1 = np.cos(dec / 57.2957795) * np.cos(ra / 57.2957795);
    y1 = np.cos(dec / 57.2957795) * np.sin(ra / 57.2957795);
    z1 = np.sin(dec / 57.2957795);
    return np.array([x1, y1, z1])


def locateSat(time=2459766., OrbitData = None, startId = 0, orbDataLen = 10000):

    satPos = np.zeros(3)
    nextSid = startId
    for i in np.arange(startId, orbDataLen-1, 1):
        t1 = OrbitData[i,0]
        t2 =  OrbitData[i+1,0]
        if time>= t1 and time<t2:
            if((t2-t1)>130.0/86400.0): # should be 120s, for error, set 120+10s
                     break

            x1 = OrbitData[i,1];
            y1 = OrbitData[i,2];
            z1 = OrbitData[i,3];
            x2 = OrbitData[i+1,1];
            y2 = OrbitData[i+1,2];
            z2 = OrbitData[i+1,3];
			
            l1 = np.sqrt(x1*x1+y1*y1+z1*z1);
            l2 = np.sqrt(x2*x2+y2*y2+z2*z2);
            theta = np.arccos((x1*x2+y1*y2+z1*z2)/(l1*l2));
            theta1 = (time-t1)/(t2-t1)*theta;
            theta2 = theta-theta1;
            l = (t2-time)/(t2-t1)*l1+(time-t1)/(t2-t1)*l2;
			# // double ef = sin(theta2)/sin(theta1);
            
            x0 = np.sin(theta2)*x1/l1+np.sin(theta1)*x2/l2;
            y0 = np.sin(theta2)*y1/l1+np.sin(theta1)*y2/l2;
            z0 = np.sin(theta2)*z1/l1+np.sin(theta1)*z2/l2;
            l_ = np.sqrt(x0*x0+y0*y0+z0*z0);
            
            satPos[0] = x0*l/l_;
            satPos[1] = y0*l/l_;
            satPos[2] = z0*l/l_;
            nextSid = i
            break;	
    return satPos, nextSid




# @jit()
def producePointingList(out = '' , center = [60,-40], radius = 5):
    points = np.loadtxt('skyMapOrSurveyList/sky.dat')

    num = points.shape[0]
    center = center#ra dec
    radius = radius # degree
    c_eclip = coord.SkyCoord(points[:,2]*u.degree, points[:,1]*u.degree,frame='barycentrictrueecliptic')
    c_equtor = c_eclip.transform_to('icrs')

    ids1 = (c_equtor.ra*u.degree).value > center[0]-radius
    ids2 = (c_equtor[ids1].ra*u.degree).value < center[0]+radius
    ids3 = (c_equtor[ids1][ids2].dec*u.degree).value > center[1]-radius
    ids4 = (c_equtor[ids1][ids2][ids3].dec*u.degree).value < center[1]+radius

    elip_txt = '# ra    dec    lon    lat    angle \n'

    for p,p_ in zip(points[ids1][ids2][ids3][ids4],c_equtor[ids1][ids2][ids3][ids4]):
        ra = (p_.ra*u.degree).value
        dec = (p_.dec*u.degree).value
        pa = getobsPA(ra, dec) + 90
        # print(ra, dec)
        lon = p[2]
        lat = p[1]
        elip_txt = elip_txt + str(round(ra,6)) + '    ' + str(round(dec,6)) + '    ' + str(round(lon,6)) + '    ' + str(round(lat,6)) + '    ' + str(pa)
        elip_txt = elip_txt + '\n'

    # for i in np.arange(0,num,1):
    #     # lon,lat
    
    #     ra = (c_equtor[i].ra*u.degree).value
    #     dec = (c_equtor[i].dec*u.degree).value

    #     if center[0]-radius <ra < center[0]+radius and center[1]-radius<dec<center[1]+radius:
    #         elip_txt = elip_txt + str(round(ra,6)) + '    ' + str(round(dec,6)) + '    ' + str(round(points[i,2],6)) + '    ' + str(round(points[i,1],6)) + '    -113.4333\n' 

    pointfn = open(out,'w')
    pointfn.write(elip_txt)
    pointfn.flush()
    pointfn.close()


def producePointingList2(out = '' , center = [60,-40], radius = 5, survey_file = 'skyMapOrSurveyList/E17.5_b17.5_beta_11.6_opt_transtime_1_CMG_1_dp_2_0.25_da_10_Texp_1.5_DEC60_500_0.1_800_1000_+5deg.dat'):
    points = np.loadtxt(survey_file)

    num = points.shape[0]
    center = center#ra dec
    radius = radius # degree
    c_eclip = coord.SkyCoord(points[:,2]*u.degree, points[:,1]*u.degree,frame='barycentrictrueecliptic')
    c_equtor = c_eclip.transform_to('icrs')
    ids1 = (c_equtor.ra*u.degree).value > center[0]-radius
    ids2 = (c_equtor[ids1].ra*u.degree).value < center[0]+radius
    ids3 = (c_equtor[ids1][ids2].dec*u.degree).value > center[1]-radius
    ids4 = (c_equtor[ids1][ids2][ids3].dec*u.degree).value < center[1]+radius

    obDataDir = 'orbit20160925/'
    orbitDat = loadSatOrbitDat(obDataDir,50)
    sOrbitId = 0

    elip_txt = '# ra    dec    lon(ecliptic)    lat(ecliptic)    pos_angle    time(julian)    sat_x    sat_y    sat_z    sun_x    sun_y    sun_z    moon_x    moon_y    moon_z    sat_vx    sat_vy    sat_vz    exp_time    isDeep\n'

    for p,p_ in zip(points[ids1][ids2][ids3][ids4],c_equtor[ids1][ids2][ids3][ids4]):
        ra = (p_.ra*u.degree).value
        dec = (p_.dec*u.degree).value
        pa = getobsPA(ra, dec) + 90
        # print(ra, dec)
        lon = p[2]
        lat = p[1]
        elip_txt = elip_txt + str(round(ra,6)) + '    ' + str(round(dec,6)) + '    ' + str(round(lon,6)) + '    ' + str(round(lat,6)) + '    ' + str(pa)

        oTime = p[0]
        satPos, sOrbitId =locateSat(time=oTime, OrbitData = orbitDat, startId = sOrbitId, orbDataLen = orbitDat.shape[0])
        tempOrbitId = sOrbitId
        deltT = 0.1 # unit s
        nTime = p[0]+deltT/86400.
        satPosN, _ =locateSat(time=nTime, OrbitData = orbitDat, startId = sOrbitId, orbDataLen = orbitDat.shape[0])
        sat_v = (satPosN - satPos)/deltT
        mm1 = np.sqrt(np.sum(sat_v*sat_v))
        mm2 = np.sqrt(np.sum(satPos*satPos))
        # print(np.sum(sat_v*satPos)/(mm1*mm2))
        # print(sat_v,p[6:9])
        if(np.abs(np.sum(sat_v*satPos)/(mm1*mm2))>0.01):
            print(oTime)
            break


        
        # checkMod = np.sqrt((satPos[0]-p[3])*(satPos[0]-p[3]) + (satPos[1]-p[4])*(satPos[1]-p[4]) + (satPos[2]-p[5])* (satPos[2]-p[5]))

        # if (checkMod > 0.01):
        #     print(oTime, checkMod)
        #     break

        # print(satPos[0]-p[3], satPos[1]-p[4], satPos[2]-p[5])

        # if oTime > 2459768:
        #     break
        
        dat_col = [0,3,4,5,6,7,8,9,10,11,16,14]
        for col in dat_col[:-2]:
            elip_txt = elip_txt + '    ' + str(p[col])
        elip_txt = elip_txt + '    ' + str(sat_v[0]) + '    ' + str(sat_v[1]) + '    ' + str(sat_v[2])

        for col in dat_col[-2:]:
            elip_txt = elip_txt + '    ' + str(p[col])


        elip_txt = elip_txt + '\n'
        

    pointfn = open(out,'w')
    pointfn.write(elip_txt)
    pointfn.flush()
    pointfn.close()


def producePointingList3(out = '' , center = [60,-40], radius = 5, survey_file = 'skyMapOrSurveyList/E17.5_b17.5_beta_11.6_opt_transtime_1_CMG_1_dp_2_0.25_da_10_Texp_1.5_DEC60_500_0.1_800_1000_+5deg.dat'):
    points = np.loadtxt(survey_file)

    num = points.shape[0]
    center = center#ra dec
    radius = radius # degree
    c_eclip = coord.SkyCoord(points[:,2]*u.degree, points[:,1]*u.degree,frame='barycentrictrueecliptic')
    c_equtor = c_eclip.transform_to('icrs')

    cent_xyz = transRaDec2D(center[0],center[1])
    xyz = transRaDec2D((c_equtor.ra*u.degree).value,(c_equtor.dec*u.degree).value)

    xyz_dp = xyz[0]*cent_xyz[0] + xyz[1]*cent_xyz[1] + xyz[2]*cent_xyz[2]

    ids = xyz_dp>=m.cos(radius*m.pi/180.)

    # ids1 = (c_equtor.ra*u.degree).value > center[0]-radius
    # ids2 = (c_equtor[ids1].ra*u.degree).value < center[0]+radius
    # ids3 = (c_equtor[ids1][ids2].dec*u.degree).value > center[1]-radius
    # ids4 = (c_equtor[ids1][ids2][ids3].dec*u.degree).value < center[1]+radius

    obDataDir = 'orbit20160925/'
    orbitDat = loadSatOrbitDat(obDataDir,50)
    sOrbitId = 0

    elip_txt = '# ra    dec    lon(ecliptic)    lat(ecliptic)    pos_angle    time(julian)    sat_x    sat_y    sat_z    sun_x    sun_y    sun_z    moon_x    moon_y    moon_z    sat_vx    sat_vy    sat_vz    exp_time    isDeep\n'

    for p,p_ in zip(points[ids],c_equtor[ids]):
        ra = (p_.ra*u.degree).value
        dec = (p_.dec*u.degree).value
        # print(ra, dec)
        pa = getobsPA(ra, dec) + 90
        lon = p[2]
        lat = p[1]
        elip_txt = elip_txt + str(round(ra,6)) + '    ' + str(round(dec,6)) + '    ' + str(round(lon,6)) + '    ' + str(round(lat,6)) + '    ' + str(pa)

        oTime = p[0]
        satPos, sOrbitId =locateSat(time=oTime, OrbitData = orbitDat, startId = sOrbitId, orbDataLen = orbitDat.shape[0])
        tempOrbitId = sOrbitId
        deltT = 0.1 # unit s
        nTime = p[0]+deltT/86400.
        satPosN, _ =locateSat(time=nTime, OrbitData = orbitDat, startId = sOrbitId, orbDataLen = orbitDat.shape[0])
        sat_v = (satPosN - satPos)/deltT
        mm1 = np.sqrt(np.sum(sat_v*sat_v))
        mm2 = np.sqrt(np.sum(satPos*satPos))
        # print(np.sum(sat_v*satPos)/(mm1*mm2))
        # print(sat_v,p[6:9])
        if(np.abs(np.sum(sat_v*satPos)/(mm1*mm2))>0.01):
            print(oTime)
            break

        
        dat_col = [0,3,4,5,6,7,8,9,10,11,16,14]
        for col in dat_col[:-2]:
            elip_txt = elip_txt + '    ' + str(p[col])
        elip_txt = elip_txt + '    ' + str(sat_v[0]) + '    ' + str(sat_v[1]) + '    ' + str(sat_v[2])

        for col in dat_col[-2:]:
            elip_txt = elip_txt + '    ' + str(p[col])


        elip_txt = elip_txt + '\n'
        

    pointfn = open(out,'w')
    pointfn.write(elip_txt)
    pointfn.flush()
    pointfn.close()


if __name__ == "__main__":
    isRealSurvey = True
    survey_file = 'skyMapOrSurveyList/E17.5_b17.5_beta_11.6_opt_transtime_1_CMG_1_dp_2_0.25_da_10_Texp_1.5_DEC60_500_0.1_800_1000_+5deg.dat'
    outFileName = 'pointing_test_NGP_246.5_40_1.dat'
    radi = 1.3

    # center_pos = [192.8595, 27.1283]
    center_pos = [246.5, 40]
    if isRealSurvey:
        producePointingList3(out = outFileName,center = center_pos, radius = radi, survey_file = survey_file)
    else:
        producePointingList(out = outFileName,center = center_pos, radius = radi)