// File: /workspace/github_cpp_makefile_star2_O2/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 $0x98, %rsp
movl %edi, %ebx
leal -0x21(%rbx), %eax
cmpl $-0x20, %eax
jb 0x69c32
movl %ebx, %eax
imulq $0x348, %rax, %rax      # imm = 0x348
leaq 0x1bf3df(%rip), %rcx    # 0x228fb0
leaq (%rcx,%rax), %r14
cmpb $0x0, 0x202(%rcx,%rax)
je 0x69c23
cmpl $0x0, 0x244(%r14)
je 0x69c23
movsd 0x2d0(%r14), %xmm0
mulsd %xmm0, %xmm0
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
movsd %xmm0, -0x48(%rbp)
mulsd %xmm0, %xmm1
movsd 0x9e671(%rip), %xmm0    # 0x108280
divsd %xmm1, %xmm0
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm0
jb 0x69c40
sqrtsd %xmm0, %xmm0
jmp 0x69c49
orl $-0x1, 0x50(%r14)
xorpd %xmm1, %xmm1
movsd %xmm1, 0x38(%r14)
addq $0x98, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq
callq 0x9260
xorpd %xmm3, %xmm3
movsd 0x10(%r14), %xmm4
subsd 0x2d8(%r14), %xmm4
movsd 0x9e628(%rip), %xmm1    # 0x108288
addsd %xmm4, %xmm1
movsd 0x9e624(%rip), %xmm2    # 0x108290
cmpltsd %xmm4, %xmm2
andpd %xmm2, %xmm1
andnpd %xmm4, %xmm2
orpd %xmm1, %xmm2
movsd 0x8bc6b(%rip), %xmm4    # 0xf58f0
addsd %xmm2, %xmm4
movapd %xmm2, %xmm1
cmpltsd 0x9e602(%rip), %xmm1    # 0x108298
andpd %xmm1, %xmm4
andnpd %xmm2, %xmm1
orpd %xmm4, %xmm1
addsd 0x2a8(%r14), %xmm0
movapd %xmm1, -0x60(%rbp)
mulsd %xmm1, %xmm0
addsd 0x2b0(%r14), %xmm0
pushq $0x1f
popq %r15
movsd %xmm0, -0x38(%rbp)
movapd %xmm3, %xmm2
decl %r15d
je 0x69d68
movsd 0x2c0(%r14), %xmm1
movsd %xmm1, -0x30(%rbp)
movsd %xmm0, -0x20(%rbp)
callq 0x9080
mulsd -0x30(%rbp), %xmm0
movsd -0x20(%rbp), %xmm1
subsd %xmm0, %xmm1
subsd -0x38(%rbp), %xmm1
movsd %xmm1, -0x30(%rbp)
movsd 0x2c0(%r14), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x20(%rbp), %xmm0
callq 0x9710
movsd -0x20(%rbp), %xmm3
mulsd -0x28(%rbp), %xmm0
movsd 0x89343(%rip), %xmm1    # 0xf3070
subsd %xmm0, %xmm1
movsd -0x30(%rbp), %xmm2
divsd %xmm1, %xmm2
movapd %xmm3, %xmm0
subsd %xmm2, %xmm0
movapd %xmm0, %xmm1
subsd %xmm3, %xmm1
andpd 0x8943e(%rip), %xmm1    # 0xf3190
movsd 0x9e546(%rip), %xmm2    # 0x1082a0
ucomisd %xmm1, %xmm2
movapd %xmm3, %xmm2
jbe 0x69cc6
movsd 0x2c0(%r14), %xmm1
mulsd %xmm1, %xmm1
movsd 0x892f3(%rip), %xmm0    # 0xf3070
subsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
movsd %xmm2, -0x30(%rbp)
jb 0x69d96
sqrtsd %xmm0, %xmm0
jmp 0x69da0
callq 0x9260
movsd -0x30(%rbp), %xmm2
movsd %xmm0, -0x20(%rbp)
movapd %xmm2, %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 0x89293(%rip), %xmm1    # 0xf3070
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 0x89249(%rip), %xmm2    # 0xf3070
subsd %xmm0, %xmm2
movsd -0x28(%rbp), %xmm1
divsd %xmm2, %xmm1
movsd -0x20(%rbp), %xmm0
callq 0x9050
addsd 0x308(%r14), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x2c8(%r14), %xmm1
movsd %xmm1, -0xa8(%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 -0xa8(%rbp), %xmm0
addsd %xmm1, %xmm0
addsd -0x40(%rbp), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd 0x2c0(%r14), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
mulsd -0x40(%rbp), %xmm0
movsd 0x890dd(%rip), %xmm1    # 0xf3070
subsd %xmm0, %xmm1
movsd -0x48(%rbp), %xmm0
mulsd %xmm1, %xmm0
addsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x48(%rbp)
movsd 0x318(%r14), %xmm0
mulsd -0x60(%rbp), %xmm0
addsd 0x2f8(%r14), %xmm0
addsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x30(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9710
mulsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
mulsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd 0x9e2ad(%rip), %xmm2    # 0x1082a8
movsd 0x310(%r14), %xmm1
addsd %xmm2, %xmm1
mulsd -0x60(%rbp), %xmm1
addsd 0x2e8(%r14), %xmm1
cvtss2sd 0x1f4(%r14), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
callq 0x9710
movsd %xmm0, -0x20(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x48(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
movsd -0x48(%rbp), %xmm1
mulsd -0x38(%rbp), %xmm1
mulsd %xmm0, %xmm1
movsd -0x20(%rbp), %xmm0
mulsd -0x40(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9080
movsd %xmm0, -0x48(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x60(%rbp)
movsd -0x28(%rbp), %xmm0
callq 0x9710
movsd %xmm0, -0x28(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x9080
movsd -0x20(%rbp), %xmm1
ucomisd %xmm1, %xmm1
jp 0x69c28
movsd -0x38(%rbp), %xmm3
movsd -0x60(%rbp), %xmm1
mulsd %xmm3, %xmm1
mulsd -0x28(%rbp), %xmm1
movsd -0x40(%rbp), %xmm2
mulsd -0x48(%rbp), %xmm2
addsd %xmm1, %xmm2
ucomisd %xmm2, %xmm2
xorpd %xmm1, %xmm1
jp 0x69c2c
mulsd %xmm0, %xmm3
ucomisd %xmm3, %xmm3
jp 0x69c2c
leaq 0x28ced6(%rip), %rax    # 0x2f6fd0
movsd (%rax), %xmm0
leaq 0x28ced3(%rip), %rax    # 0x2f6fd8
movsd (%rax), %xmm1
leaq 0x28ced0(%rip), %rax    # 0x2f6fe0
movsd %xmm2, -0x40(%rbp)
movsd (%rax), %xmm2
movsd %xmm3, -0x38(%rbp)
callq 0x55535
leaq 0x28cebe(%rip), %rax    # 0x2f6fe8
movsd (%rax), %xmm0
subsd -0x20(%rbp), %xmm0
leaq 0x28ceb6(%rip), %rax    # 0x2f6ff0
movsd (%rax), %xmm1
subsd -0x40(%rbp), %xmm1
leaq 0x28ceae(%rip), %rax    # 0x2f6ff8
movsd (%rax), %xmm2
subsd -0x38(%rbp), %xmm2
mulsd %xmm1, %xmm1
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
mulsd %xmm2, %xmm2
addsd %xmm0, %xmm2
sqrtsd %xmm2, %xmm1
movapd %xmm1, %xmm2
divsd 0x9e139(%rip), %xmm2    # 0x1082b0
cvttsd2si %xmm2, %eax
movsd 0x28(%r14), %xmm0
divsd 0x9e12f(%rip), %xmm0    # 0x1082b8
movsd 0x8b817(%rip), %xmm3    # 0xf59a8
cvttpd2dq %xmm2, %xmm2
cvtdq2pd %xmm2, %xmm2
mulsd %xmm3, %xmm0
mulsd %xmm3, %xmm2
addsd %xmm0, %xmm2
leal 0x1(%rax), %ecx
cvtsi2sd %ecx, %xmm4
mulsd %xmm3, %xmm4
addsd %xmm0, %xmm4
movapd %xmm1, %xmm5
subsd %xmm2, %xmm5
movapd 0x88fcc(%rip), %xmm2    # 0xf3190
andpd %xmm2, %xmm5
subsd %xmm4, %xmm1
andpd %xmm2, %xmm1
ucomisd %xmm1, %xmm5
cmoval %ecx, %eax
cvtsi2sd %eax, %xmm1
mulsd %xmm3, %xmm1
addsd %xmm0, %xmm1
mulsd 0x8b6fd(%rip), %xmm1    # 0xf58e8
movl %eax, 0x50(%r14)
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm1
jne 0x6a1ff
jnp 0x69c2c
movl %ebx, %edi
movsd %xmm1, -0x20(%rbp)
callq 0xca4da
movsd -0x20(%rbp), %xmm1
leaq 0x28d131(%rip), %rcx    # 0x2f7348
orl %eax, (%rcx)
jmp 0x69c2c