// File: /workspace/github_cpp_makefile_star2_O3/jkingan[P]ladyheather/heathgps.cpp
// Function: calc_sat_posn(int)
void calc_sat_posn(int prn)
{
int j;
double mu;
double omega_dot_earth;
double A;
double cmm;
double tk;
double n;
double mk;
double Ek;
double nu;
double Phi;
double du,dr,di;
double u;
double r;
double i;
double x_prime;
double y_prime;
double sat_x, sat_y, sat_z;
double omega;
int compute_harmonic_correction;
double E;
double pr;

int iii;
int cp_wrap;
double p1,p2;
double pr0;
double pr1, pr2, pr3;
static double last_pr = 0.0;

   // calculate sat position from ephermeris data, then convert distance to
   // sat into a pseudorange value.
if(0 && log_file) fprintf(log_file, "# prn:%d  eph_flag:%d  eph_valid:%d\n", prn, sat[prn].eph_flag,sat[prn].eph_valid); // rnx3

   if(prn < 1) return;
   if(prn > 32) return;

   pr = 0.0;
   if(sat[prn].eph_flag == 0) {
      sat[prn].iii = (-1);
      goto no_range;
   }
   if(sat[prn].eph_valid == 0) {
      sat[prn].iii = (-1);
      goto no_range;
   }

   compute_harmonic_correction = 1;

   mu = 3.986005e14;
   omega_dot_earth = 7.2921151467e-5; //(rad/sec)

   // Now follow ICD table 20-IV
   A = sat[prn].sqrtA;
   A *= A;

   cmm = sqrt(mu/(A*A*A)); // computed mean motion
   tk = sat[prn].raw_time - sat[prn].eph_toe;

   // account for beginning of end of week crossover
   if(tk > 302400.0)  tk = tk - 604800.0;
   if(tk < -302400.0) tk = tk + 604800.0;

   // apply mean motion correction
   n = cmm + sat[prn].delta_n;

   // Mean anomaly
   mk = sat[prn].M0 + n*tk;

   // solve Kepler's equation for eccentric anomaly
   E = mk;
   Ek = 0.0;
   for(j=0; j<30; j++) {  // Kepler's equation
      Ek = E;
      E -= (E - sat[prn].e*sin(E) - mk) / (1.0-sat[prn].e*cos(E));
      if(fabs(E - Ek) < 1.0E-14) break;
   }

   // True anomaly:
   nu = atan2( (sqrt(1.0-(sat[prn].e*sat[prn].e))*sin(Ek)/(1.0-sat[prn].e*cos(Ek))), 
               ((cos(Ek)-sat[prn].e)/(1.0-sat[prn].e*cos(Ek))) );
   // Ek = acos((e + cos(nu)) / (1.0+e*cos(nu)));

   Phi = nu + sat[prn].omega;
   du = 0.0;
   dr = 0.0;
   di = 0.0;
   if(compute_harmonic_correction == 1) {    // compute harmonic corrections
      du = sat[prn].Cus*sin(2.0*Phi) + sat[prn].Cuc*cos(2.0*Phi);
      dr = sat[prn].Crs*sin(2.0*Phi) + sat[prn].Crc*cos(2.0*Phi);
      di = sat[prn].Cis*sin(2.0*Phi) + sat[prn].Cic*cos(2.0*Phi);
   }

   u = Phi + du;
   r = A*(1.0-sat[prn].e*cos(Ek)) + dr;

   // inclination angle at reference time
   i = sat[prn].io + sat[prn].i_dot*tk + di;
   x_prime = r*cos(u);
   y_prime = r*sin(u);
   omega = sat[prn].omega_0 + (sat[prn].omega_dot - omega_dot_earth)*tk - omega_dot_earth*sat[prn].toe;

   sat_x = x_prime*cos(omega) - y_prime*cos(i)*sin(omega);
   sat_y = x_prime*sin(omega) + y_prime*cos(i)*cos(omega);
   sat_z = y_prime*sin(i);
   if(sat_x != sat_x) goto no_range;  // rnx - kludgy NAN check
   if(sat_y != sat_y) goto no_range;
   if(sat_z != sat_z) goto no_range;

   lla_to_ecef(lat,lon,alt);
   du = (ecef_x - sat_x);
   dr = (ecef_y - sat_y);
   di = (ecef_z - sat_z);
   pr = sqrt(du*du + dr*dr + di*di);

   iii = (-1);
   if(1) { // piss - adjust raw pseudorange for code_phase
      pr0 = pr;
if(CP_WRAP == 0) sat[prn].iii = (-1);
      if(sat[prn].iii < 0) {
         iii = (int) (pr / (LIGHTSPEED*1000.0 * 1.0e-3));
         p1 = p2 = sat[prn].code_phase/(16.0*1023.0)*1e-3;
         p1 += (double) iii * 1.0e-3;
         p2 += (double) (iii+1) * 1.0e-3;
         if(fabs(pr - p1) > fabs(pr - p2)) ++iii;
         sat[prn].iii = iii;
      }
      iii = sat[prn].iii;

      cp_wrap = 0;
      if(CP_WRAP) { 
         if((sat[prn].code_phase > 15000.0) && (sat[prn].last_cp < 1000.0)) cp_wrap = (-1);
         else if((sat[prn].code_phase < 1000.0) && (sat[prn].last_cp > 15000.0)) cp_wrap = (1);
         iii += cp_wrap;
         sat[prn].iii = iii;
      }

if(0 && (prn == plot_prn) && debug_file) {  //piss
   if(cp_wrap) fprintf(debug_file, "code phase wrap %d!\n", cp_wrap);
   fprintf(debug_file, "prn:%-2d iii:%d  pr0:%.4f ", 
                        prn,     iii,    pr0);
}

      /* Check which is closest to pseudorange */
      pr1 = ((double) (iii-1) * 1.0e-3) + sat[prn].code_phase/(16.0*1023.0)*1e-3;
      pr1 *= (LIGHTSPEED*1000.0);
      pr2 = ((double) iii * 1.0e-3) + sat[prn].code_phase/(16.0*1023.0)*1e-3;
      pr2 *= (LIGHTSPEED*1000.0);
      pr3 = ((double) (iii+1) * 1.0e-3) + sat[prn].code_phase/(16.0*1023.0)*1e-3;
      pr3 *= (LIGHTSPEED*1000.0);

////  if(fabs(pr - p1) > fabs(pr - p2)) ++iii;
      pr = ((double) iii * 1.0e-3) + sat[prn].code_phase/(16.0*1023.0)*1e-3;
      pr *= (LIGHTSPEED*1000.0);
   }

if(0 && (prn == plot_prn) && debug_file) {  //piss
   fprintf(debug_file, "iii2:%d  iiix:%d pr1:%.3f pr2:%.3f pr3:%.3f pr:%.3f  cp:%g  dpr:%.3f\n", 
      iii, sat[prn].iii, pr1,pr2,pr3, pr, sat[prn].code_phase, pr-last_pr);
   last_pr = pr;
}


if(0 && debug_file) {
   fprintf(debug_file, "ex:%.9f ey:%.9f  ez:%.9f\n", ecef_x,ecef_y,ecef_z);
   fprintf(debug_file, "sx:%.9f sy:%.9f  sz:%.9f\n", sat_x,sat_y,sat_z);
   fprintf(debug_file, "dx:%.9f dy:%.9f  dz:%.9f\n", du,dr,di);
   fprintf(debug_file, "prn:%d  flag:%d  valid:%d  prange:%.9f  (pr0:%.9f)   time:%.9f\n", prn, sat[prn].eph_flag, sat[prn].eph_valid, pr, pr0, sat[prn].raw_time);
   fprintf(debug_file, "\n");
}
   
   if(pr) {
      have_range |= rinex_gnss(prn);
   }

   no_range:
   sat[prn].range = pr;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0x88, %rsp
movl %edi, %ebx
leal -0x21(%rbx), %eax
cmpl $-0x20, %eax
jb 0x738d6
movl %ebx, %eax
imulq $0x348, %rax, %rax      # imm = 0x348
leaq 0x1cb73e(%rip), %rcx    # 0x23efb0
leaq (%rcx,%rax), %r14
cmpb $0x0, 0x202(%rcx,%rax)
je 0x738c4
cmpl $0x0, 0x244(%r14)
je 0x738c4
movsd 0x2d0(%r14), %xmm0
mulsd %xmm0, %xmm0
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd %xmm0, -0x40(%rbp)
mulsd %xmm0, %xmm1
movsd 0xaa958(%rip), %xmm0    # 0x11e208
divsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x738e4
sqrtsd %xmm0, %xmm0
jmp 0x738e9
movl $0xffffffff, 0x50(%r14) # imm = 0xFFFFFFFF
xorpd %xmm1, %xmm1
movsd %xmm1, 0x38(%r14)
addq $0x88, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq
callq 0x9270
movsd 0x10(%r14), %xmm3
subsd 0x2d8(%r14), %xmm3
movsd 0xaa910(%rip), %xmm1    # 0x11e210
addsd %xmm3, %xmm1
movsd 0xaa90c(%rip), %xmm2    # 0x11e218
cmpltsd %xmm3, %xmm2
andpd %xmm2, %xmm1
andnpd %xmm3, %xmm2
orpd %xmm1, %xmm2
movsd 0x97f53(%rip), %xmm3    # 0x10b878
addsd %xmm2, %xmm3
movapd %xmm2, %xmm1
cmpltsd 0xaa8ea(%rip), %xmm1    # 0x11e220
andpd %xmm1, %xmm3
andnpd %xmm2, %xmm1
orpd %xmm3, %xmm1
addsd 0x2a8(%r14), %xmm0
movapd %xmm1, -0x60(%rbp)
mulsd %xmm1, %xmm0
addsd 0x2b0(%r14), %xmm0
movl $0x1d, %r15d
movsd %xmm0, -0x48(%rbp)
movapd %xmm0, %xmm2
movsd %xmm2, -0x20(%rbp)
movapd %xmm2, %xmm0
movsd %xmm2, -0x30(%rbp)
movsd 0x2c0(%r14), %xmm1
movsd %xmm1, -0x28(%rbp)
callq 0x9080
mulsd -0x28(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
movapd %xmm1, %xmm2
subsd %xmm0, %xmm2
subsd -0x48(%rbp), %xmm2
movsd %xmm2, -0x28(%rbp)
movsd 0x2c0(%r14), %xmm0
movsd %xmm0, -0x38(%rbp)
movapd %xmm1, %xmm0
callq 0x9710
movsd -0x20(%rbp), %xmm2
mulsd -0x38(%rbp), %xmm0
movsd 0x9569e(%rip), %xmm1    # 0x109070
subsd %xmm0, %xmm1
movsd -0x28(%rbp), %xmm0
divsd %xmm1, %xmm0
subsd %xmm0, %xmm2
movapd %xmm2, %xmm0
subsd -0x30(%rbp), %xmm0
andpd 0x9579c(%rip), %xmm0    # 0x109190
subl $0x1, %r15d
setb %al
movsd 0xaa825(%rip), %xmm1    # 0x11e228
ucomisd %xmm0, %xmm1
ja 0x73a11
testb %al, %al
je 0x7396c
movsd 0x2c0(%r14), %xmm1
mulsd %xmm1, %xmm1
movsd 0x9564a(%rip), %xmm0    # 0x109070
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x73a3a
sqrtsd %xmm0, %xmm0
jmp 0x73a3f
callq 0x9270
movsd %xmm0, -0x20(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9080
mulsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x20(%rbp)
movsd 0x2c0(%r14), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
mulsd -0x28(%rbp), %xmm0
movsd 0x955f3(%rip), %xmm1    # 0x109070
subsd %xmm0, %xmm1
movsd -0x20(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
movsd 0x2c0(%r14), %xmm1
movsd %xmm1, -0x38(%rbp)
subsd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
mulsd -0x38(%rbp), %xmm0
movsd 0x955a9(%rip), %xmm2    # 0x109070
subsd %xmm0, %xmm2
movsd -0x28(%rbp), %xmm1
divsd %xmm2, %xmm1
movsd -0x20(%rbp), %xmm0
callq 0x9050
addsd 0x308(%r14), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd 0x2c8(%r14), %xmm1
movsd %xmm1, -0x50(%rbp)
addsd %xmm0, %xmm0
movsd %xmm0, -0x20(%rbp)
callq 0x9080
movsd %xmm0, -0x28(%rbp)
movsd 0x2b8(%r14), %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x98(%rbp)
movsd 0x2a0(%r14), %xmm0
movsd %xmm0, -0x90(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9080
movsd %xmm0, -0x38(%rbp)
movsd 0x300(%r14), %xmm0
movsd %xmm0, -0x88(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x80(%rbp)
movsd 0x2f0(%r14), %xmm0
movsd %xmm0, -0x78(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9080
movsd %xmm0, -0x70(%rbp)
movsd 0x2e0(%r14), %xmm0
movsd %xmm0, -0x68(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
mulsd -0x68(%rbp), %xmm0
movsd -0x70(%rbp), %xmm1
mulsd -0x78(%rbp), %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x80(%rbp), %xmm1
mulsd -0x88(%rbp), %xmm1
movsd -0x38(%rbp), %xmm0
mulsd -0x90(%rbp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x98(%rbp), %xmm1
mulsd -0xa0(%rbp), %xmm1
movsd -0x28(%rbp), %xmm0
mulsd -0x50(%rbp), %xmm0
addsd %xmm1, %xmm0
addsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd 0x2c0(%r14), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
mulsd -0x48(%rbp), %xmm0
movsd 0x95443(%rip), %xmm1    # 0x109070
subsd %xmm0, %xmm1
movsd -0x40(%rbp), %xmm0
mulsd %xmm1, %xmm0
addsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x318(%r14), %xmm0
mulsd -0x60(%rbp), %xmm0
addsd 0x2f8(%r14), %xmm0
addsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9710
mulsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
mulsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd 0xaa59b(%rip), %xmm2    # 0x11e230
movsd 0x310(%r14), %xmm1
addsd %xmm2, %xmm1
mulsd -0x60(%rbp), %xmm1
addsd 0x2e8(%r14), %xmm1
xorps %xmm0, %xmm0
cvtss2sd 0x1f4(%r14), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
callq 0x9710
movsd %xmm0, -0x30(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x40(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
movsd -0x40(%rbp), %xmm1
mulsd -0x48(%rbp), %xmm1
mulsd %xmm0, %xmm1
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
movsd %xmm0, -0x40(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x60(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x28(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9080
movsd -0x30(%rbp), %xmm1
ucomisd %xmm1, %xmm1
jp 0x738cc
movsd -0x48(%rbp), %xmm3
movsd -0x60(%rbp), %xmm1
mulsd %xmm3, %xmm1
mulsd -0x28(%rbp), %xmm1
movsd -0x38(%rbp), %xmm2
mulsd -0x40(%rbp), %xmm2
addsd %xmm1, %xmm2
ucomisd %xmm2, %xmm2
xorpd %xmm1, %xmm1
jp 0x738d0
mulsd %xmm0, %xmm3
ucomisd %xmm3, %xmm3
jp 0x738d0
movsd %xmm3, -0x48(%rbp)
movsd %xmm2, -0x38(%rbp)
leaq 0x29922f(%rip), %rax    # 0x30cfd0
movsd (%rax), %xmm1
movsd %xmm1, -0x20(%rbp)
leaq 0x299227(%rip), %rax    # 0x30cfd8
movsd (%rax), %xmm0
movsd %xmm0, -0x28(%rbp)
leaq 0x29921f(%rip), %rax    # 0x30cfe0
movsd (%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
movaps %xmm1, %xmm0
callq 0x9710
movsd %xmm0, -0x50(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9080
movsd %xmm0, -0x20(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x40(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
movsd 0xaa341(%rip), %xmm1    # 0x11e148
movsd -0x20(%rbp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
addsd 0x95254(%rip), %xmm1    # 0x109070
movapd %xmm0, %xmm7
xorpd %xmm6, %xmm6
ucomisd %xmm6, %xmm1
jb 0x73e33
xorps %xmm0, %xmm0
sqrtsd %xmm1, %xmm0
jmp 0x73e4a
movapd %xmm1, %xmm0
movsd %xmm7, -0x28(%rbp)
callq 0x9270
movsd -0x28(%rbp), %xmm7
xorpd %xmm6, %xmm6
movsd -0x38(%rbp), %xmm2
movsd 0xaa2f9(%rip), %xmm1    # 0x11e150
divsd %xmm0, %xmm1
movsd -0x60(%rbp), %xmm4
movapd %xmm4, %xmm0
addsd %xmm1, %xmm0
movsd -0x50(%rbp), %xmm5
mulsd %xmm0, %xmm5
movsd -0x40(%rbp), %xmm3
mulsd %xmm5, %xmm3
leaq 0x299167(%rip), %rax    # 0x30cfe8
movsd %xmm3, (%rax)
mulsd %xmm5, %xmm7
leaq 0x299160(%rip), %rax    # 0x30cff0
movsd %xmm7, (%rax)
mulsd 0xaa2bc(%rip), %xmm1    # 0x11e158
addsd %xmm4, %xmm1
movsd -0x20(%rbp), %xmm0
mulsd %xmm1, %xmm0
leaq 0x299148(%rip), %rax    # 0x30cff8
movsd %xmm0, (%rax)
subsd -0x30(%rbp), %xmm3
subsd %xmm2, %xmm7
subsd -0x48(%rbp), %xmm0
mulsd %xmm7, %xmm7
mulsd %xmm3, %xmm3
addsd %xmm7, %xmm3
mulsd %xmm0, %xmm0
addsd %xmm3, %xmm0
xorps %xmm1, %xmm1
sqrtsd %xmm0, %xmm1
movapd %xmm1, %xmm2
divsd 0xaa34f(%rip), %xmm2    # 0x11e238
cvttsd2si %xmm2, %eax
movsd 0x28(%r14), %xmm0
divsd 0xaa345(%rip), %xmm0    # 0x11e240
movsd 0x97a2d(%rip), %xmm3    # 0x10b930
cvttpd2dq %xmm2, %xmm2
cvtdq2pd %xmm2, %xmm2
mulsd %xmm3, %xmm0
mulsd %xmm3, %xmm2
addsd %xmm0, %xmm2
leal 0x1(%rax), %ecx
xorps %xmm4, %xmm4
cvtsi2sd %ecx, %xmm4
mulsd %xmm3, %xmm4
addsd %xmm0, %xmm4
movapd %xmm1, %xmm5
subsd %xmm2, %xmm5
movapd 0x95257(%rip), %xmm2    # 0x109190
andpd %xmm2, %xmm5
subsd %xmm4, %xmm1
andpd %xmm2, %xmm1
ucomisd %xmm1, %xmm5
cmoval %ecx, %eax
movl %eax, 0x50(%r14)
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
mulsd %xmm3, %xmm1
addsd %xmm0, %xmm1
mulsd 0x97909(%rip), %xmm1    # 0x10b870
ucomisd %xmm6, %xmm1
jne 0x73f73
jnp 0x738d0
movl %ebx, %edi
movsd %xmm1, -0x30(%rbp)
callq 0xdc1dd
movsd -0x30(%rbp), %xmm1
leaq 0x2993bd(%rip), %rcx    # 0x30d348
orl %eax, (%rcx)
jmp 0x738d0