#include "coordinates.h"

/* 基本旋转矩阵 */
void rot_x(demat* M, double alf) {
	//第1行元素
	M->v[0] = 1;
	M->v[1] = 0;
	M->v[2] = 0;
	//第2行元素
	M->v[3] = 0;
	M->v[4] = cos(alf);
	M->v[5] = sin(alf);
	//第3行元素
	M->v[6] = 0;
	M->v[7] = -sin(alf);
	M->v[8] = cos(alf);
}

void rot_y(demat* M, double alf) {

	//第1行元素
	M->v[0] = cos(alf);
	M->v[1] = 0;
	M->v[2] = -sin(alf);
	//第2行元素
	M->v[3] = 0;
	M->v[4] = 1;
	M->v[5] = 0;
	//第3行元素
	M->v[6] = sin(alf);
	M->v[7] = 0;
	M->v[8] = cos(alf);
}

void rot_z(demat* M, double alf) {

	//第1行元素
	M->v[0] = cos(alf);
	M->v[1] = sin(alf);
	M->v[2] = 0;
	//第2行元素
	M->v[3] = -sin(alf);
	M->v[4] = cos(alf);
	M->v[5] = 0;
	//第3行元素
	M->v[6] = 0;
	M->v[7] = 0;
	M->v[8] = 1;
}

/* 由基本旋转矩阵衍生的各个坐标系直接的旋转矩阵 */
void C_A2B(demat* M, double phi_T, double psi_T, double gamma_T) {
	// 返回或者发射惯性系到飞行器体坐标系
	// phi_T, psi_T, gamma_T: 321转序，返回惯性坐标系到体坐标系的姿态角;
	// 转序：321
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_z(M1, phi_T);
	rot_y(M, psi_T);
	denseMM(M2, M, M1);
	rot_x(M1, gamma_T);
	denseMM(M, M1, M2);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void C_A2O(demat* M, double A0, double phi0, double T) {
	// 返回惯性系到返回系
	// A0： 初始方位角
	// phi0： 初始地心或者地理纬度
	// T：从建立再入坐标系的时刻起的飞行时间
	double we;
	demat* M1, * M2;
	we = 7.272205216643040e-05; // 地球自转角速度，rad / s
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	C_O2E(M1, A0, phi0);
	rot_z(M, we * T);
	denseMM(M2, M, M1);
	denseMtM(M, M1, M2);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void C_En2T(demat* M, double phi0, double phi, double dlambda) {
	// 再入坐标系到地理坐标系之间的转换
	// phi0 ： 再入坐标系原点的地心或者地理纬度
	// phi ： 地理坐标系原点的地心纬度
	// dlambda ： 经度差
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_z(M1, phi0);
	rot_x(M, dlambda);
	denseMM(M2, M, M1);
	rot_z(M1, -phi);
	denseMM(M, M1, M2);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void C_H2B(demat* M, double nv, double beta, double alpha) {
	// 半速度坐标系到飞行器体坐标系之间的转换
	// 转序：123转序
	// nv：倾侧角
	// beta：侧滑角
	// alpha: 攻角
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_x(M1, nv);
	rot_y(M, beta);
	denseMM(M2, M, M1);
	rot_z(M1, alpha);
	denseMM(M, M1, M2);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void C_O2E(demat* M, double A0, double phi0) {
	// 返回系到地心坐标系
	// A0： 初始方位角
	// phi0： 初始地心或者地理纬度
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_y(M1, A0 + PI / 2);
	rot_x(M, -phi0);
	denseMM(M2, M, M1);
	rot_z(M1, PI / 2);
	denseMM(M, M1, M2);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void C_O2En(demat* M, double A0) {
	// 返回系到再入坐标系之间的转换
	// A0：初始方位角
	rot_y(M, A0);
}

void C_T2H(demat* M, double sigma, double theta) {
	// 地理坐标系到半速度坐标系之间的转换
	// 转序：23转序
	// sigma：负当地航迹方位角
	// theta：当地速度倾角
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_y(M1, sigma);
	rot_z(M2, theta);
	denseMM(M, M2, M1);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}
void C_V2B(demat* M, double beta, double alpha) {
	// 速度坐标系到飞行器体坐标系之间的转换
	// 转序：23
	// beta: 侧滑
	// alpha : 攻角
	demat* M1, * M2;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	rot_y(M1, alpha);
	rot_z(M2, beta);
	denseMM(M, M2, M1);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
}

void A_H2B1(double* alpha, double* beta, double* nu,
	double sigma, double theta,
	double lat, double lon,
	double phi_T, double psi_T, double gamma_T,
	double A0, double lon0, double phi0, double T) {
	// alpha, beta, nu: 侧滑角, 攻角, 倾侧角
	//用方式1获得半速度坐标系H到飞行器体坐标系B之间的欧拉角
	//phi_T, psi_T, gamma_T: 321转序，返回惯性坐标系到体坐标系的姿态角;
	//theta:当地速度倾角;
	//sigma: 负的当地航迹方位
	//lon : 当前经度
	//lat : 当前纬度
	//A0 : 发射或者返回系的方位角
	//lon0 : 发射或者返回系原点经度、取决于坐标系怎么建立的
	//phi0 : 发射或者返回系原点地理或者地心纬度、取决于坐标系怎么建立的
	//T : 从建立返回系起的飞行时间
	demat* M1, * M2, * M3;
	M1 = createDeMat(3, 3);
	M2 = createDeMat(3, 3);
	M3 = createDeMat(3, 3);
	C_A2O(M1, A0, phi0, T);
	C_A2B(M3, phi_T, psi_T, gamma_T);
	denseMMt(M2, M3, M1); // M2 = C_O2B
	C_O2En(M1, A0);
	denseMMt(M3, M1, M2); // M3 = C_B2En
	C_En2T(M1, phi0, lat, lon - lon0);
	denseMM(M2, M1, M3); // M2 = C_B2T
	C_T2H(M1, sigma, theta);
	denseMM(M3, M1, M2); // M3 = C_B2H
	beta[0] = asin(M3->v[2]);
	alpha[0] = atan2(-M3->v[1], M3->v[0]);
	nu[0] = atan2(-M3->v[5], M3->v[8]);
	freeDeMat(M1, 2);
	freeDeMat(M2, 2);
	freeDeMat(M3, 2);
}