/*---
  WGS84の直交座標(ECEF)から水平線座標(ENU)の変換
  2002.8.27: S.Fukushima(ENRI)
  ---*/

#include "str01.h"
#include "MathUtils.h"
#include <cstdio>
#include <cstdlib>

using namespace qrk;


#define SQR(x) ((x)*(x))

extern vector blh2ecef(double, double, double);
extern vector ecef2blh(vector);
extern matrix matmat(matrix *, matrix *);
extern vector matvec(matrix, vector);


matrix rotx(double sita)
  /* x軸回りのsita度の回転変換：右ねじの方向 */
{
  matrix rota;
  rota.n = rota.m = 3;

  rota.a[0][0] = 1;
  rota.a[0][1] = 0;
  rota.a[0][2] = 0;

  rota.a[1][0] = 0;
  rota.a[1][1] = cos(sita * M_PI / 180.0);
  rota.a[1][2] = sin(sita * M_PI / 180.0);

  rota.a[2][0] = 0;
  rota.a[2][1] = -sin(sita * M_PI / 180.0);
  rota.a[2][2] = cos(sita * M_PI / 180.0);

  return rota;
}

matrix roty(double sita)
{
  matrix rota;
  rota.n = rota.m = 3;

  rota.a[0][0] = cos(sita * M_PI / 180.0);
  rota.a[0][1] = 0;
  rota.a[0][2] = -sin(sita * M_PI / 180.0);

  rota.a[1][0] = 0;
  rota.a[1][1] = 1;
  rota.a[1][2] = 0;

  rota.a[2][0] = sin(sita * M_PI / 180.0);
  rota.a[2][1] = 0;
  rota.a[2][2] = cos(sita * M_PI / 180.0);

  return rota;
}

matrix rotz(double sita)
{
  matrix rota;
  rota.n = rota.m = 3;

  rota.a[0][0] = cos(sita * M_PI / 180.0);
  rota.a[0][1] = sin(sita * M_PI / 180.0);
  rota.a[0][2] = 0;

  rota.a[1][0] = -sin(sita * M_PI / 180.0);
  rota.a[1][1] = cos(sita * M_PI / 180.0);
  rota.a[1][2] = 0;

  rota.a[2][0] = 0;
  rota.a[2][1] = 0;
  rota.a[2][2] = 1;

  return rota;
}
vector ecef2enu(vector dest, vector origin)
  /*--- ECEF座標を水平線座標(ENU)へ変換する */
{
  int i;
  vector mov, ret, blh;
  matrix rotyp, rotzp1, rotzp2;
  matrix mat_conv1, mat_conv2;

  origin.n  = 3;
  origin.err  = 0;
  mov.n = 3;
  mov.err = 0;
  ret.n = 3;
  ret.err = 0;

  blh = ecef2blh(origin);

  rotzp1 = rotz(90.0);
  rotyp  = roty(90.0 - blh.a[0]);
  rotzp2 = rotz(blh.a[1]);

  mat_conv1 = matmat(&rotzp1, &rotyp);
  mat_conv2 = matmat(&mat_conv1, &rotzp2);

  for(i=0;i<3;i++) mov.a[i] = dest.a[i] - origin.a[i];
  ret = matvec(mat_conv2, mov);
  return ret;
}


#if 0
/*--- チェック用 */
int main()
{
  double lat, lon, hig, lat_o, lon_o, hig_o;
  vector ecef, ecef_o, enu;

  lat = 38.14227288;            /*--- 変換する位置座標(B27海側)*/
  lon = 140.93265738;
  hig =  45.664;

  lat_o = 38.13877338;          /*--- 原点の座標(B09山側)*/
  lon_o = 140.89872429;
  hig_o = 44.512;

  ecef = blh2ecef(lat, lon, hig);
  ecef_o = blh2ecef(lat_o, lon_o, hig_o);
  enu = ecef2enu(ecef, ecef_o);

  printf("result= %.3f %.3f %.3f\n", enu.a[0], enu.a[1], enu.a[2]);
  printf("length= %.3f, angle(deg)= %.3f\n",
  sqrt(SQR(enu.a[0])+SQR(enu.a[1])), (180.0/PI)*atan(enu.a[1]/ enu.a[0]));
}
/*---
  結果: result= 2974.681 388.988 0.447
  length= 3000.006, angle(deg)= 7.450
  ---*/
#endif
