#include <math.h>
#include <stdio.h>
//---------------------------------------------------------------------------
double A0=6378.1363;  //средний радиус Земли
double Wgeo=360.9856235/86400./57.2957795;  // радиан/сек
double Grav=398600.4415;  //гравитационная постоянная
double th0;
//---------------------------------------------------------------------------
void smpy ( double *a, double r, double *b, int n )   // Умножение на скаляp
{	for ( int i=0; i<n; i++, *b++ = *a++ * r );		}
//------------------------------------------------------------------------
void madd ( double *a, double *b, double *c, int n )  // Сложение вектоpов
{ 	for ( int i=0; i<n; i++, *c++ = *a++ + *b++	);  }
//------------------------------------------------------------------------
double scal ( double *a, double *b, int n )       // Скаляpное пpоизведение
{
    double c=0.;
	for ( int i=0; i<n; c+=a[i]*b[i], i++ );
	return c;

}
//------------------------------------------------------------------------
void mprd ( double *a, double *b, double *c, int ma, int na, int nb )
{                                                  // Умножение матpиц
    int i,j,k;
    double s, *api, *bpj, *cpi;
	for(i=0; i<ma; i++)   {
		api = a+i*na; cpi = c+i*nb;
		for(j=0; j<nb; j++) {
			bpj = b+j;
			for ( k=0,s=0.; k<na; k++)  s += *(api+k) * *(bpj+k*nb);
			*(cpi+j) = s;
		}
	}
}
//------------------------------------------------------------------------
void ort ( double *r, double & fi, double & psi)
{
    double a,cf;
	a = sqrt ( scal ( r , r, 3 ) );
	fi = ( a == 0. )? 0. : asin(r[2]/a);
	psi= ( r[0] == 0. && r[1] == 0. ) ?  0. : atan2(r[1],r[0]);
}
//------------------------------------------------------------------------
void GreenFash(double jd)
{
  double Tu = (jd - 2451545.)/36525.;
  th0 = 24110.54841 + 8640184.812866*Tu + 0.093104*Tu*Tu - 6.2e-6*Tu*Tu*Tu;
  th0 = (th0/60/60)*15/57.29;
}
//---------------------------------------------------------------------------
 void GreenSimple(double t, double *m)
{
  double Th = th0 + Wgeo*t;
  m[0] = cos(Th); m[1] = -sin(Th); m[2] = 0;
  m[3] = -m[1];  m[4] = m[0]; m[5]=0;
  m[6] = 0; m[7] = 0; m[8] = 1;
}
//---------------------------------------------------------------------------
void getAccelJ6(double *r, double *acc)
{
  double J_zonal[5] = {1.08262668355e-3, -2.53265648533e-6,
                       -1.61962159137e-6, -2.27296082869e-7, 5.40681239107e-7}; // коэффициенты зональных гармоник
  double rm[3]; smpy(r,1000.,rm,3);
  double rmod = sqrt(scal(rm,rm,3)), mu = 3.986004418e+14, R = 6.378137e+6;
  double Rd = R/rmod, Rd2=pw(Rd,2), Rd3=pw(Rd,3), Rd4=pw(Rd,4), Rd5=pw(Rd,5), Rd6=pw(Rd,6);
  double mur2 = mu/(rmod*rmod);
  double zR = rm[2]/rmod, zR2 = pw(zR,2), zR3 = pw(zR,3), zR4 = pw(zR,4), zR5 = pw(zR,2), zR6 = pw(zR,6);
  double rx = rm[0]/rmod, ry = rm[1]/rmod, rz = rm[2]/rmod; //ввести исходные данные
  double aJ2[3],aJ3[3],aJ4[3], aJ5[3], aJ6[3], tmp;

  double scal = -1.5*J_zonal[0]*mur2*Rd2; tmp = (1.-5.*zR2);
  double vj2[3] = {tmp*rx, tmp*ry, (3.-5.*zR2)*rz}; //считаем определитель
  smpy(vj2,scal,aJ2,3);

  scal = -0.5*J_zonal[1]*mur2*Rd3; tmp = 5.*(7.*zR3 - 3.*zR);
  double vj3[3] = {tmp*rx, tmp*ry, 3.*(10.*zR2 - (35./3.)*zR4 - 1.)};
  smpy(vj3,scal,aJ3,3);

  scal = -0.625*J_zonal[2]*mur2*Rd4; tmp = (3. - 42.*zR2 + 63.*zR4);
  double vj4[3] = {tmp*rx, tmp*ry, -(15. - 70.*zR2 + 63.*zR4)*rz};
  smpy(vj4,scal,aJ4,3);

  scal = -0.125*J_zonal[3]*mur2*Rd5; tmp = 3.*(35.*zR - 210.*zR3 + 231.*zR5);
  double vj5[3] = {tmp*rx, tmp*ry, (15. - 315.*zR2 + 945.*zR4 - 693.*zR6)};
  smpy(vj5,scal,aJ5,3);

  scal = 0.0625*J_zonal[4]*mur2*Rd6; tmp = (35. - 945.*zR2 + 3465.*zR4 - 3003.*zR6);
  double vj6[3] = {tmp*rx, tmp*ry, (2205.*zR2 - 4851.*zR4 + 3003.*zR6 - 315.)*rz};
  smpy(vj6,scal,aJ6,3);

  madd(aJ2,aJ3,acc,3); madd(acc,aJ4,acc,3); madd(acc,aJ5,acc,3); madd(acc,aJ6,acc,3);
  smpy(acc,1e-3,acc,3); // км/с2
}
//---------------------------------------------------------------------------
void RightPart(double t, double *x, double *dx)
{
     double r2=scal(x,x,3), r=sqrt(r2), r3=r*r2, Acc[3];
     for ( int i=0; i<3; i++)
	 {
		dx[i] = x[i+3];
		dx[3+i]=-Grav*x[i]/r3;
	 }
     double rg[3],mgg[9], mg[9],a[3];
     GreenSimple(t, mgg);
     mprd(x,mgg,rg,1,3,3);       	// переводим радиус-вектор в текущий Гринвич
     getAccelJ6(rg,a);              // получаем ускорение в текущем Гринвиче
     mprd(mgg,a,Acc,3,3,1);      	// переводим ускорение в J2000
     madd(&dx[3],Acc,&dx[3],3);
}
//---------------------------------------------------------------------------
double djul(int day, int mon, int year, int hour, int min, int sec)
{
long int i1=(mon-14)/12,
	i2=year+4800+i1,
	i3=day-32075+1461*i2/4,
	i4=i3+367*(mon-2-i1*12)/12-3*((i2+100)/100)/4;
	return  (i4-.5+hour/24.+min/1440.+sec/86400.);
}
//---------------------------------------------------------------------------
void RK (void (*f)(double, double*, double*), double t,  double *y,  double h )
{
    int i;
     double  y1[6], y2[6], y3[6], y4[6], yy[6], h2;
	h2=h/2;
	f(t,y,y1);   	  for (i=0; i<6; i++)	yy[i]=y[i]+h2*y1[i];
	f(t+h2,yy,y2);    for (i=0; i<6; i++)	yy[i]=y[i]+h2*y2[i];
	f(t+h2,yy,y3);    for (i=0; i<6; i++)	yy[i]=y[i]+h *y3[i];
	f(t+h,yy,y4);
	for ( i=0; i<6; i++) y[i] = y[i]+h*(y1[i]+y4[i]+2*(y2[i]+y3[i]))/6.;
}
//---------------------------------------------------------------------------
void main()
{
   double jd = djul(22, 3, 2018, 0, 0, 0);                 //ввести дату
   double y[6] = {5590.258935,3450.371745,-1756.114989,2.187475,0.188943,7.334642};  // ССО
   GreenFash(jd);
   double t = 3600; // + 3 часа потому что ДМВ
   double tk = t+86400, h = 1;

   f = fopen("res_with.txt", "w");
   int sh = 0;
   while(t<tk)
   {
      RK(RightPart,t,y,h);
      double mg[9], rg[3];
      GreenSimple(t,mg);
      mprd(y,mg,rg,1,3,3);
      double dec, ra, H;
      ort(rg, dec, ra);
      if(sh % 60 == 0)
      {  fprintf(f,"%lf %lf %lf\n",t,ra*RAD, dec*RAD); sh = 0;}
      t+=h;
      sh+=1;
   }
   fclose(f);
}