add search obs pointings and fix bug

'''

Author: Zhang Xin zhangx@bao.ac.cn

Date: 2024-11-08 15:12:55

LastEditors: Zhang Xin zhangx@bao.ac.cn

LastEditTime: 2024-11-11 10:01:35

FilePath: /CSST_Survey/survey_sim/config/infooutput.py

Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE

'''

import os

import logging

class InfoOutput(object):

    def __init__(self, dir=None, logger_filename=None, info_out_filename=None):

        self.outDir = dir

        self.info_out_filename = info_out_filename

        self.logger = logging.getLogger()

        fh = logging.FileHandler(os.path.join(

            self.outDir, logger_filename), mode='w+', encoding='utf-8')

        fh.setLevel(logging.DEBUG)

        self.logger.setLevel(logging.DEBUG)

        logging.getLogger('numba').setLevel(logging.WARNING)

        formatter = logging.Formatter(

            '%(asctime)s - %(msecs)d - %(levelname)-8s - [%(filename)s:%(lineno)d] - %(message)s')

        fh.setFormatter(formatter)

        self.logger.addHandler(fh)

        hdr1 = "# JDTime lonitude(ecliptic) latitude(ecliptic) RA Dec sun_x sun_y sun_z moon_x moon_y moon_z sat_x sat_y sat_z sat_vel_x sat_vel_y sat_vel_z isInDeep "

        fmt1 = "%15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %15.8f %4d"

        self.hdr = hdr1

        self.fmt = fmt1

        self.create_output_file()

    def Log_info(self, message):

        print(message)

        self.logger.info(message)

    def Log_error(self, message):

        print(message)

        self.logger.error(message)

    def update_output_header(self, additional_column_names=""):

        self.hdr += additional_column_names

    def create_output_file(self):

        self.outInfo = open(os.path.join(

            self.outDir, self.info_out_filename), "w")

        self.logger.info("Creating catalog file %s ...\n" %

                         (os.path.join(self.outDir, self.info_out_filename)))

        if not self.hdr.endswith("\n"):

            self.hdr += "\n"

        self.outInfo.write(self.hdr)

    def outInfo_add_obj(self, jdTime=2459766., p_lon_ecl=0., p_lat_ecl=0., p_ra=0, p_dec=0., sun=[0, 0, 0], moon=[0, 0, 0], sat=[0, 0, 0], sat_vel=[0, 0, 0], isInDeep=0):

        line = self.fmt % (

            jdTime, p_lon_ecl, p_lat_ecl, p_ra, p_dec, sun[0], sun[1], sun[2], moon[0], moon[1], moon[2], sat[0], sat[1], sat[2], sat_vel[0], sat_vel[1], sat_vel[2], isInDeep)

        # if not line.endswith("\n"):

        line += "\n"

        self.outInfo.write(line)

from astropy.time import Time

import math

import numpy as np

from scipy import interpolate

from numba import jit, njit

"""

description:

    z = satPos[2]

    # J2000, 地球赤道坐标系

    satellite_position = SkyCoord(

        x=x, y=y, z=z, unit="km", representation_type="cartesian", frame="gcrs"

        x=x,

        y=y,

        z=z,

        unit="km",

        representation_type="cartesian",

        frame="gcrs",

        equinox="J2000",

        obstime=Time(t, format="jd")

    )

    itrs_position = satellite_position.transform_to(ITRS(obstime=Time(t, format="jd")))

    itrs_position = satellite_position.transform_to(

        ITRS(obstime=Time(t, format="jd")))

    # 获取地理坐标 (经纬度和高度)

    earth_location = EarthLocation.from_geocentric(

    return [longitude, latitude]

@jit

def calculateAngle(ra1, dec1, ra2, dec2):

    # double x1, y1, z1, x2, y2, z2, angle, cosValue;

    # 返回值为角度（以度数为单位，不是以弧度为单位）

@jit

def getAngle132(x1, y1, z1, x2, y2, z2, x3, y3, z3):

    x11 = x1 - x3

    z22 = z2 - z3

    tt = np.sqrt(

        (x11 * x11 + y11 * y11 + z11 * z11) * (x22 * x22 + y22 * y22 + z22 * z22)

        (x11 * x11 + y11 * y11 + z11 * z11) *

        (x22 * x22 + y22 * y22 + z22 * z22)

    )

    cosValue = (x11 * x22 + y11 * y22 + z11 * z22) / tt

    return angle * 180 * M_1_PI

@jit

def cross_product_3d(vector_a, vector_b):

    # 检查输入向量是否为三维

    if len(vector_a) != 3 or len(vector_b) != 3:

        raise ValueError("两个向量都必须是三维的")

    # 计算叉乘的分量

    c1 = vector_a[1] * vector_b[2] - vector_a[2] * vector_b[1]

    c2 = vector_a[2] * vector_b[0] - vector_a[0] * vector_b[2]

    c3 = vector_a[0] * vector_b[1] - vector_a[1] * vector_b[0]

    # 返回结果向量

    return np.array([c1, c2, c3])

@njit

def dot_product_vector(a, b):

    if len(a) != len(b):

        raise ValueError("两个矩阵的维度必须相同")

    result = 0.0

    for i in range(len(a)):

        result += a[i] * b[i]

    return result

@jit

def dot_product_matrix(A, B):

    # 获取矩阵 A 的行数和矩阵 B 的列数

    if A.ndim == 2 and B.ndim == 2:

        rows_A = len(A)

        cols_A = len(A[0])

        rows_B = len(B)

        cols_B = len(B[0])

        # 检查矩阵是否可以相乘（A 的列数应该等于 B 的行数）

        if cols_A != rows_B:

            raise ValueError("矩阵 A 的列数必须等于矩阵 B 的行数")

        # 初始化结果矩阵 C，所有元素为 0

        C = [[0 for _ in range(cols_B)] for _ in range(rows_A)]

        # 执行矩阵点乘

        for i in range(rows_A):

            for j in range(cols_B):

                for k in range(cols_A):  # 或者 rows_B，因为 cols_A == rows_B

                    C[i][j] += A[i][k] * B[k][j]

    elif A.ndim == 2 and B.ndim == 1:

        rows_A = len(A)

        cols_A = len(A[0])

        rows_B = len(B)

        # 检查矩阵是否可以相乘（A 的列数应该等于 B 的行数）

        if cols_A != rows_B:

            raise ValueError("矩阵 A 的列数必须等于矩阵 B 的行数")

        # 初始化结果矩阵 C，所有元素为 0

        C = [0 for _ in range(rows_A)]

        # 执行矩阵点乘

        for i in range(rows_A):

            for j in range(rows_B):

                C[i] += A[i][j] * B[j]

    else:

        raise ValueError("数据不正确")

    return np.array(C)

@njit

def norm_(arr):

    norm_val = 0.

    for i in np.arange(len(arr)):

        norm_val += arr[i]*arr[i]

    return np.sqrt(norm_val)

# 根据指定的转动轴和转动角度生成旋转矩阵

def GenRotationMatrix(u=np.array([0, 0, 0]), angle_deg=10.0):

@jit

def GenRotationMatrix(u=np.array([0., 0., 0.]), angle_deg=10.0):

    theta = angle_deg * PI_180

    cos_theta = math.cos(theta)

    sin_theta = math.sin(theta)

    One_cos_theta = 1 - cos_theta

    u_norm = np.linalg.norm(u)

    u = u / u_norm

    ux = u[0]

    uy = u[1]

    uz = u[2]

    u_norm = norm_(u)

    # u = u / u_norm

    ux = u[0] / u_norm

    uy = u[1] / u_norm

    uz = u[2] / u_norm

    if np.fabs(u_norm - 1.0) > 1e-5:

        print("in GenRotationMatrix: u_norm differs too much from 1.0!")

    R = np.zeros([3, 3])

    R = np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])

    R[0, 0] = cos_theta + ux * ux * One_cos_theta

    R[0, 1] = ux * uy * One_cos_theta - uz * sin_theta

    R[0, 2] = ux * uz * One_cos_theta + uy * sin_theta

    return R

@jit

def rodrigues_rotation_formula(axis, theta):

    """根据旋转轴向量和旋转角度生成旋转矩阵, 与GenRotationMatrix()结果一样"""

    theta = theta * PI_180

    axis = axis / np.linalg.norm(axis)  # 确保轴向量归一化

    axis = axis / norm_(axis)  # 确保轴向量归一化

    cos_theta, sin_theta = np.cos(theta), np.sin(theta)

    # 构造斜对称矩阵

    )

    # 构造旋转矩阵

    R = cos_theta * np.eye(3) + (1 - cos_theta) * np.outer(axis, axis) + sin_theta * S

    R = cos_theta * np.eye(3) + (1 - cos_theta) * \

        np.outer(axis, axis) + sin_theta * S

    return R

@jit

def get_rotation_axis(mat):

    """根据旋转轴旋转矩阵,生成旋转轴"""

    assert np.allclose(np.dot(mat.T, mat), np.eye(3)), "R must be orthogonal"

    assert np.isclose(np.linalg.det(mat), 1.0), "R must have a determinant of 1"

    # assert np.allclose(dot_product_matrix(mat.T, mat),

    #                    np.eye(3)), "R must be orthogonal"

    # assert np.isclose(np.linalg.det(

    #     mat), 1.0), "R must have a determinant of 1"

    axis = np.zeros(3)

    axis = np.array([0., 0., 0.])

    # axis[0] = mat[7]-mat[5];

    # axis[1] = mat[2]-mat[6];

    # axis[2] = mat[3]-mat[1];

    axis[2] = mat[1, 0] - mat[0, 1]

    # axis_norm = np.sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);

    axis_norm = np.linalg.norm(axis)

    axis_norm = norm_(axis)

    if axis_norm == 0:

        return np.array([0., 0., 0.])

    axis[0] /= axis_norm

    axis[1] /= axis_norm

    axis[2] /= axis_norm

    return axis

@jit

def calculate_trace(matrix):

    # 确保输入是一个方阵

    if len(matrix) != len(matrix[0]):

        raise ValueError("输入的矩阵不是方阵")

    # 初始化迹的总和

    trace_sum = 0

    # 遍历主对角线上的元素并累加

    for i in range(len(matrix)):

        trace_sum += matrix[i][i]

    # 返回迹的总和

    return trace_sum

@jit

def get_RotationAngleFromMatrix(mat):

    assert np.allclose(np.dot(mat.T, mat), np.eye(3)), "R must be orthogonal"

    assert np.isclose(np.linalg.det(mat), 1.0), "R must have a determinant of 1"

    trace = np.trace(mat)

    cos_theta = np.clip((trace - 1) / 2, -1, 1)

    # assert np.allclose(np.dot(mat.T, mat), np.eye(3)), "R must be orthogonal"

    # assert np.isclose(np.linalg.det(

    #     mat), 1.0), "R must have a determinant of 1"

    # trace = np.trace(mat)

    trace = calculate_trace(mat)

    # cos_theta = np.clip((trace - 1) / 2, -1, 1)

    cos_theta = (trace - 1) / 2

    if cos_theta < -1:

        cos_theta = -1

    if cos_theta > 1:

        cos_theta = 1

    angle = 180.0 / math.pi * math.acos(cos_theta)

    return angle

# //  注意：新版本返回的角度单位是“度数”,不再使用“弧度”！

# //

# //  另外，目前还没有优化这个函数。

# @jit

def Get_RotationAngle(

    ra_old=60.0,

    dec_old=-40.0,

    dec_new=-43.0,

):

    angle_rot = 0

    rot_axis = np.zeros(3)

    angle_rot = 0.0

    rot_axis = np.array([1., 0., 0.])

    dec_old = 90 - dec_old

    dec_new = 90 - dec_new

    # dec_old = 90 - dec_old

    # dec_new = 90 - dec_new

    sin_ra_old = math.sin(ra_old * PI_180)

    cos_ra_old = math.cos(ra_old * PI_180)

    sin_dec_old = math.sin(dec_old * PI_180)

    #   2021-07-03:这里采用的是数学上标准的球坐标与直角坐标之间的转换，与天文上的定义不一样，所以在使用该

    #              函数时，需要对dec做一个变换: "dec" --> "90-dec"

    # TODO 为什么不用天文的坐标系？？？？？？？？？？？？

    p_old = np.array([sin_dec_old * cos_ra_old, sin_dec_old * sin_ra_old, cos_dec_old])

    p_new = np.array([sin_dec_new * cos_ra_new, sin_dec_new * sin_ra_new, cos_dec_new])

    p_tmp = np.ones(3) * -999

    p_old = np.array(

        [sin_dec_old * cos_ra_old, sin_dec_old * sin_ra_old, cos_dec_old])

    p_new = np.array(

        [sin_dec_new * cos_ra_new, sin_dec_new * sin_ra_new, cos_dec_new])

    # p_tmp = np.ones(3) * -999

    p_tmp = np.array([-999., -999., -999.])

    # ====================================================================

    # step 1:绕z轴旋转，将旧指向旋转到新指向所在的子午圈，并计算出旋转后的“临时”指向矢量

        if np.fabs(dec_new - dec_old) < 1e-5:

            # // printf("==> getting rotation axis : 1\n");

            rot_axis[0] = 0

            rot_axis[1] = 0

            rot_axis[2] = 1

            return (

                angle_rot,

                rot_axis,

                0,

            )  # 两个指向相同的情况下不需要进行任何转动,可直接返回

        delta_alpha = 0

        delta_alpha1 = 0  # 等价于不做任何转动。

        delta_alpha2 = 180  # 这种情况应该予以排除，因为会导致帆板面的指向反转，造成无法接受太阳光照进行发电。

            rot_axis[0] = 0.

            rot_axis[1] = 0.

            rot_axis[2] = 1.

            return angle_rot, rot_axis, np.array([[-999., -999., -999.], [-999., -999., -999.], [-999., -999., -999.]])

            # 两个指向相同的情况下不需要进行任何转动,可直接返回

        delta_alpha = 0.

        delta_alpha1 = 0.  # 等价于不做任何转动。

        delta_alpha2 = 180.  # 这种情况应该予以排除，因为会导致帆板面的指向反转，造成无法接受太阳光照进行发电。

        p_tmp[0] = p_old[0]

        p_tmp[1] = p_old[1]

        p_tmp[2] = p_old[2]

        alpha_n = np.array([0, 0, 1])

        Rz = GenRotationMatrix(alpha_n, 0)

        alpha_n = np.array([0., 0., 1.])

        Rz = GenRotationMatrix(alpha_n, 0.)

    else:

        # alpha_old,alpha_new是在XY平面内的单位向量

        alpha_old = np.array([cos_ra_old, sin_ra_old, 0])

        alpha_new = [cos_ra_new, sin_ra_new, 0]

        alpha_old = np.array([cos_ra_old, sin_ra_old, 0.])

        alpha_new = np.array([cos_ra_new, sin_ra_new, 0.])

        # 通过ＸＹ平面内的两个（单位长度）指向的叉乘获取旋转轴以及相应的旋转方向(用于调整黄经)

        alpha_n = np.cross(alpha_old, alpha_new)

        # alpha_n = np.cross(alpha_old, alpha_new)

        alpha_n = cross_product_3d(alpha_old, alpha_new)

        alpha_n_norm = np.linalg.norm(alpha_n)

        alpha_n_norm = norm_(alpha_n)

        if np.fabs(alpha_n_norm) < 1e-6:  # 原先是1e-8,导致在台式机上可能出现NaN问题

            # 	说明 alpha_old[] 与 alpha_new[] 指向相反的方向，就直接绕z轴旋转180度即可????? 应该不需要转，要不帆板就转了

            alpha_n[0] = 0

            alpha_n[1] = 0

            alpha_n[2] = 1

            alpha_n[0] = 0.

            alpha_n[1] = 0.

            alpha_n[2] = 1.

            delta_alpha = 0.0

            # delta_alpha = 180.

            # 根据alpha_old,alpha_new来确定如何绕z-轴旋转

            # TODO cosval 难道不是经度的夹角？？？？？？？？

            cosval = np.dot(alpha_old, alpha_new)

            cosval = dot_product_vector(

                alpha_old, alpha_new)/(norm_(alpha_old)*norm_(alpha_new))

            # csst_test( fabs(cosval) < -1.0 || fabs(cosval) > 1.0, errmsg, "cosval is out of range [-1,1]!" );

            # 确保不会出现数值计算错误（之前在台式机上运行仿真时，总在这个模块内出现问题）

                # 转动的方向由转动轴的指向决定，因此传入的角度值都是正数

                # 这里需要注意的是需要将alpha_n反号

                alpha_n[0] *= -1

                alpha_n[1] *= -1

                alpha_n[2] *= -1

                alpha_n[0] *= -1.

                alpha_n[1] *= -1.

                alpha_n[2] *= -1.

                Rz = GenRotationMatrix(alpha_n, delta_alpha2)

            else:

                return -999, -999, -999

                return -999.,  np.array([-999., -999., -999.]),  np.array([[-999., -999., -999.], [-999., -999., -999.], [-999., -999., -999.]])

        # MatrixVecProduct(Rz,p_old,p_tmp);

        p_tmp = np.dot(Rz, p_old)

        p_tmp = dot_product_matrix(Rz, p_old)

    #  ====================================================================

    #  step 2:根据 p_tmp 和 p_new 的叉乘来计算第二次旋转的旋转轴；虽然确定了旋转轴，

    #  但是依旧存在两种旋转方式。

    axis = np.zeros(3)

    axis = np.array([1.0, 1.0, 1.0])

    # 首先还是先判断“临时指向”与目标指向是否共线(包括同向与反向)

    p_tmp_dot_p_new = np.dot(p_tmp, p_new)

    norm_po_pn = (norm_(p_tmp)*norm_(p_new))

    if norm_po_pn == 0:

        return -999., np.array([-999., -999., -999.]),  np.array([[-999., -999., -999.], [-999., -999., -999.], [-999., -999., -999.]])

    p_tmp_dot_p_new = dot_product_vector(

        p_tmp, p_new)/norm_po_pn

    if np.fabs(p_tmp_dot_p_new - 1) < 1e-8:  # p_tmp与p_new重合

        angle_rot = delta_alpha

        rot_axis = get_rotation_axis(Rz)

        #  旋转轴的指向不影响结果

        axis[0] = math.cos(0.5 * math.pi + ra_new * PI_180)

        axis[1] = math.sin(0.5 * math.pi + ra_new * PI_180)

        axis[2] = 0

        axis[2] = 0.

        Rn = GenRotationMatrix(axis, 180)

        R = np.dot(Rn, Rz)  # 此处需要注意旋转矩阵乘积的顺序

        Rn = GenRotationMatrix(axis, 180.)

        R = dot_product_matrix(Rn, Rz)  # 此处需要注意旋转矩阵乘积的顺序

        # MatrixMultiplication(Rn,Rz);  //  此处需要注意旋转矩阵乘积的顺序

        angle_rot = get_RotationAngleFromMatrix(R)

        # printf("==> getting rotation axis : 3\n");

        get_rotation_axis(R, rot_axis)

        rot_axis = get_rotation_axis(R)

        return angle_rot, rot_axis, R

    #  给存贮绕n-轴的两个旋转矩阵分配内存（n轴由两个矢量的叉积得到）

    cosval = np.clip(np.dot(p_tmp, p_new), -1, 1)

    # cosval = np.clip(p_tmp_dot_p_new, -1, 1)

    cosval = p_tmp_dot_p_new

    if cosval > 1:

        cosval = 1

    if cosval < -1:

        cosval = -1

    delta_alpha1 = math.acos(cosval) * 180 / math.pi  # 这个值始终是大于0的

    # delta_alpha2 = 180-delta_alpha1; //这个值始终也是大于0的，但表示反方向旋转

    axis = np.cross(p_tmp, p_new)

    axis_norm = np.linalg.norm(axis)

    # axis = np.cross(p_tmp, p_new)

    axis = cross_product_3d(p_tmp, p_new)

    axis_norm = norm_(axis)

    axis[0] /= axis_norm

    axis[1] /= axis_norm

    axis[2] /= axis_norm

    R1 = GenRotationMatrix(axis, delta_alpha1)

    R = np.dot(R1, Rz)

    R = dot_product_matrix(R1, Rz)

    angle_rot = get_RotationAngleFromMatrix(R)

    rot_axis = get_rotation_axis(R)

    ppNorm = np.cross(p1, p2)

    pn1 = np.cross(p1, ppNorm)

    pn2 = np.cross(p2, ppNorm)

    angle = getAngle132(pn1[0], pn1[1], pn1[2], pn2[0], pn2[1], pn2[2], 0, 0, 0)

    angle = getAngle132(pn1[0], pn1[1], pn1[2], pn2[0],

                        pn2[1], pn2[2], 0, 0, 0)

    R = GenRotationMatrix(u=ppNorm, angle_deg=angle)

    return angle, ppNorm, R

# @jit

def calculateTransTime(transAngle=1.0, surveyCons=None):

    # //double data[9][2] = { {0.5, 70},{1, 80}, {5, 95},

    # //{ 10, 105 }, { 15, 115 }, { 20, 120 }, { 30, 135 }, { 45, 150 },{180,200} };

    # //double data[4][2] = { {0.1, 70},{1, 80}, {45, 161},{180,200} };

    # double data[4][2] = { {1, 80}, {20,127},{45, 196},{180,581} };

    # // double data[4][2] = { {1, 45}, {20,92},{45, 196},{180,581} };  // 减少稳定时间

    # // double data[4][2] = { {1, 76}, {20,123},{45, 192},{180,577} };

    # // double data[3][2] = { {1, 80}, {45,170},{180,445} };

    angleVStime = np.array([[1, 20, 45, 180], [80, 127, 196, 581]])

    # if transAngle < angleVStime[0, 0]:

    #     tTime = 70

    # else:

    #     angleVStime_i = interpolate.interp1d(

    #         angleVStime[0], angleVStime[1], kind="linear"

    #     )

    #     tTime = angleVStime_i(transAngle)

    # print(tTime)

    if transAngle < angleVStime[0, 0]:

        tTime = 70

    elif transAngle == angleVStime[0, 0]:

        tTime = angleVStime[1, 0]

    else:

        for i in np.arange(1, 4, 1):

            if (transAngle > angleVStime[0, i-1] and transAngle <= angleVStime[0, i]):

                tTime = angleVStime[1, i-1] * ((transAngle - angleVStime[0, i])) / (((angleVStime[0, i-1] - angleVStime[0, i]))) + \

                    angleVStime[1, i] * ((transAngle - angleVStime[0, i-1])) / \

                    (((angleVStime[0, i] - angleVStime[0, i-1])))

                break

    return tTime + surveyCons.SHUTTER_TIME * 2.0

    # int i = 0;

    # double tTime = 0;

    # // if(transAngle > 180) {

    # //     printf("%f \n",transAngle);

    # // }

    # if(transAngle < 1) {

    #     tTime = 70;

    # } else if(transAngle == 1) {

    #     tTime = 80;

    # } else {

    #     for(i = 0; i < 3 ; i ++) {

    #         if(transAngle>data[i][0] && transAngle <= data[i + 1][0] ) {

    #             tTime = data[i][1] * ((transAngle - data[i+1][0])) / (((data[i][0] - data[i+1][0])))

    #                   + data[i+1][1] * ((transAngle - data[i][0])) / (((data[i+1][0] - data[i][0])));

    #             break;

    #         }

    #     }

    # }

    # return tTime + SHUTTER_TIME*2.0; // 此处增加了快门打开和关闭所需要的时间 @2018-11-06

# endif

        self.orbitData = orbitData

        self.ephLib = ephLib