// File: /workspace/github_cpp_makefile_star2_O2/jkingan[P]ladyheather/heathmsc.cpp
// Function: osc_pid_filter()
void osc_pid_filter()
{
double dac_ctrl;
double pps_tweak;
double osc_PID_val;
int i;

   if(0 && osc_prefilter) {
      if(rcvr_type == X72_RCVR) {
         osc_osc_q[opq_in] = osc_offset;
         osc_pps_q[opq_in] = pps_offset * 1.0E-9;
      }
      else {
         osc_osc_q[opq_in] = osc_offset;
         osc_pps_q[opq_in] = pps_offset;
      }
      if(++opq_in >= osc_prefilter) opq_in = 0;
      if(++opq_count > osc_prefilter) opq_count = osc_prefilter;

      pps_bin_count = 0;
      avg_pps = avg_osc = 0.0;
      for(i=0; i<opq_count; i++) {  // !!! we should calc this more efficiently by tracking what is in the queue
         avg_osc += osc_osc_q[i];
         avg_pps += osc_pps_q[i];
         ++pps_bin_count;
      }
      avg_osc /= (double) pps_bin_count;
      avg_pps /= (double) pps_bin_count;
   }
   else if(rcvr_type == X72_RCVR) {
      avg_osc = x72_xxx_dds(pps_offset);
sprintf(debug_text4, "avg osc1:%g  count:%d", avg_osc, x72_dds_count);
      avg_osc = avg_osc * (1.0 + (((double) (x72_dds_count - x72_tc_val)) / (2.0*(double) x72_tc_val)) );
sprintf(debug_text3, "avg osc2:%g", avg_osc);
      if(x72_dds_count) avg_osc /= x72_dds_count;
      avg_osc /= x72_osc;
      if(x72_dds_count < 60) avg_osc = last_x72_dds;
sprintf(debug_text2, "avg osc3:%g", avg_osc);
      avg_pps = pps_offset * 1.0E-9;
   }
   else {
      avg_osc = osc_offset;
      avg_pps = pps_offset;
   }

   if(osc_rampup && (osc_rampup < OSC_I_TC)) {
      calc_osc_k_factors();
   }

   #ifdef DEBUG_OSC_CONTROL
      if(osc_pid_debug) {
         show_osc_pid_values();
      }
   #endif

   last_osc_PID_error = osc_PID_error;

   // osc_PID_error is the curent oscillator error 
   // Negative values indicate the osc is too high and needs to be 
   // lowered.  Positive values indicate the osc too high and needs
   // to be raised.
   //
   // OSC_FILTER_OFFSET is a offset constant: negative values raise 
   // the osc curve,  positive values lower it
// pps_tweak = ((float) pps_offset) * OSC_FILTER_OFFSET * (OSC_P_GAIN/1000.0F);
// osc_PID_error = ((float) osc_offset) + pps_tweak;  // OSC_FILTER_OFFSET;
   pps_tweak = avg_pps * OSC_FILTER_OFFSET * (OSC_P_GAIN/1000.0);
   osc_PID_error = avg_osc + pps_tweak;  // OSC_FILTER_OFFSET;

   // osc_integral_step is the clipped osc error
   osc_integral_step = osc_PID_error; 
   if(osc_integral_step > OSC_MAX_INTEGRAL) osc_integral_step = OSC_MAX_INTEGRAL;  
   else if(osc_integral_step < (-OSC_MAX_INTEGRAL)) osc_integral_step = (-OSC_MAX_INTEGRAL);

   if((osc_PID_out >= OSC_MAX_PID) && (osc_integral_step > 0.0)) osc_integral_step *= (-osc_integrator_reset);
   else if((osc_PID_out <= (-OSC_MAX_PID)) && (osc_integral_step < 0.0)) osc_integral_step *= (-osc_integrator_reset);

   osc_integrator += (osc_integral_step * osc_k4);
   if((osc_integrator+osc_k6) > 1.0F) osc_integrator = (1.0F-osc_k6);    // integrator value is maxed out
   else if((osc_integrator+osc_k6) < (-1.0F)) osc_integrator = (-1.0F-osc_k6);

   osc_PID_out = (osc_k1*osc_PID_error) + (osc_k2*last_osc_PID_error) + (osc_k3*last_osc_PID_out);
   last_osc_PID_out = osc_PID_out;
   if(osc_PID_out > 0.0F) osc_PID_out -= osc_k7;   // autotune debug - manual step size
   else                   osc_PID_out += osc_k7;
   osc_PID_out += (osc_integrator + osc_k6);

   // clamp the filter response
   if(osc_PID_out > OSC_MAX_PID) osc_PID_out = OSC_MAX_PID;
   else if(osc_PID_out < (-OSC_MAX_PID)) osc_PID_out = (-OSC_MAX_PID);

   if(osc_postfilter) {
      if(post_q_count >= osc_postfilter) post_q_sum -= osc_post_q[post_q_in];
      post_q_sum += osc_PID_out;
      osc_post_q[post_q_in] = osc_PID_out;
      if(++post_q_in >= osc_postfilter)   post_q_in = 0;
      if(++post_q_count > osc_postfilter) post_q_count = osc_postfilter;
      osc_PID_val = post_q_sum / (double) post_q_count;
   }
   else {
      osc_PID_val = osc_PID_out;
   }

   // convert filter output to DAC volatge
   if(rcvr_type == X72_RCVR) {
      if(osc_gain) dac_ctrl = (osc_PID_val*0.10) / (double) osc_gain;  // 4.0E-8 = MAX_DDS_STEP
      else dac_ctrl = 0.0;
      dac_ctrl += (double) osc_pid_initial_voltage;
      set_x72_dds(dac_ctrl);
sprintf(debug_text5, "set dds: %g  opv:%g  avg_osc:%g  avg_pps:%g  pps_tweak:%g", dac_ctrl,osc_PID_val, avg_osc,avg_pps,pps_tweak);
   }
   else {
      if(osc_gain) dac_ctrl = (osc_PID_val*MAX_PID_HZ) / (double) osc_gain;
      else dac_ctrl = 0.0;
      dac_ctrl += (double) osc_pid_initial_voltage;
      set_dac_voltage((float) dac_ctrl, 4);
   }

   #ifdef DEBUG_OSC_CONTROL
      if(osc_pid_debug) {
         sprintf(debug_text,  " PID(%g)  post(%g,%d)  last_out(%g)  err(%g)  last_err(%g)  int=%g  ramp=%g",
            osc_PID_out, osc_PID_val,post_q_count,  last_osc_PID_out, osc_PID_error*(-1.0F),  last_osc_PID_error, osc_integrator, osc_rampup);
         last_osc_PID_display =  osc_PID_out;
      }
   #endif
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x112109(%rip), %rbx    # 0x2028d0
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
jne 0xf08b5
leaq 0x205fdd(%rip), %r14    # 0x2f67b8
movsd (%r14), %xmm0
callq 0xb0591
leaq 0x238f04(%rip), %r15    # 0x3296f0
movsd %xmm0, (%r15)
leaq 0x114730(%rip), %r12    # 0x204f28
movl (%r12), %edx
leaq 0x23858d(%rip), %rdi    # 0x328d90
leaq 0x2f773(%rip), %rsi     # 0x11ff7d
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
leaq 0x1145e8(%rip), %rcx    # 0x204e04
movl (%rcx), %ecx
subl %ecx, %eax
cvtsi2sd %eax, %xmm0
cvtsi2sd %ecx, %xmm1
addsd %xmm1, %xmm1
divsd %xmm1, %xmm0
addsd 0x2838(%rip), %xmm0     # 0xf3070
mulsd (%r15), %xmm0
movsd %xmm0, (%r15)
leaq 0x2383f7(%rip), %rdi    # 0x328c40
leaq 0x2f743(%rip), %rsi     # 0x11ff93
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
movsd (%r15), %xmm0
testl %eax, %eax
je 0xf086c
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
leaq 0x1143f5(%rip), %rcx    # 0x204c68
divsd (%rcx), %xmm0
movsd %xmm0, (%r15)
cmpl $0x3b, %eax
jg 0xf0891
leaq 0x114658(%rip), %rax    # 0x204ee0
movsd (%rax), %xmm0
movsd %xmm0, (%r15)
leaq 0x238258(%rip), %rdi    # 0x328af0
leaq 0x2f700(%rip), %rsi     # 0x11ff9f
movb $0x1, %al
callq 0x9060
movsd (%r14), %xmm0
mulsd 0x2895(%rip), %xmm0     # 0xf3148
jmp 0xf08d6
leaq 0x205f04(%rip), %rax    # 0x2f67c0
movsd (%rax), %xmm0
leaq 0x238e29(%rip), %rax    # 0x3296f0
movsd %xmm0, (%rax)
leaq 0x205ee6(%rip), %rax    # 0x2f67b8
movsd (%rax), %xmm0
leaq 0x238e0b(%rip), %r12    # 0x3296e8
movsd %xmm0, (%r12)
leaq 0x238df6(%rip), %r14    # 0x3296e0
movsd (%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xf08fb
jnp 0xf0911
leaq 0x238db6(%rip), %rax    # 0x3296b8
movsd (%rax), %xmm1
ucomisd %xmm0, %xmm1
jbe 0xf0911
callq 0xf02f3
leaq 0x238d80(%rip), %r15    # 0x329698
cmpb $0x0, (%r15)
je 0xf0923
callq 0xf0462
movsd 0x34ef05(%rip), %xmm0   # 0x43f830
movsd %xmm0, 0x34ef05(%rip)   # 0x43f838
movsd (%r12), %xmm1
leaq 0x238d80(%rip), %rax    # 0x3296c0
mulsd (%rax), %xmm1
leaq 0x238d55(%rip), %rax    # 0x3296a0
movsd (%rax), %xmm5
divsd 0x26e1(%rip), %xmm5     # 0xf3038
mulsd %xmm1, %xmm5
leaq 0x238d8e(%rip), %r13    # 0x3296f0
movsd (%r13), %xmm1
addsd %xmm5, %xmm1
movsd %xmm1, 0x34eebc(%rip)   # 0x43f830
movsd %xmm1, 0x34eecc(%rip)   # 0x43f848
movsd 0x2724(%rip), %xmm3     # 0xf30a8
ucomisd %xmm3, %xmm1
ja 0xf099c
movsd 0x275e(%rip), %xmm3     # 0xf30f0
ucomisd %xmm1, %xmm3
movapd %xmm1, %xmm2
jbe 0xf09a8
movsd %xmm3, 0x34eea4(%rip)   # 0x43f848
movapd %xmm3, %xmm2
movsd 0x34ee70(%rip), %xmm3   # 0x43f820
ucomisd 0x26a8(%rip), %xmm3     # 0xf3060
jb 0xf09c4
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0xf09dc
movsd 0x271c(%rip), %xmm4     # 0xf30e8
ucomisd %xmm3, %xmm4
jb 0xf09f8
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0xf09f8
movsd 0x34eeac(%rip), %xmm3   # 0x43f890
xorpd 0x27c4(%rip), %xmm3     # 0xf31b0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x34ee50(%rip)   # 0x43f848
mulsd 0x34ee70(%rip), %xmm2   # 0x43f870
addsd 0x34ee48(%rip), %xmm2   # 0x43f850
movsd %xmm2, 0x34ee40(%rip)   # 0x43f850
movsd 0x34ee68(%rip), %xmm6   # 0x43f880
movapd %xmm2, %xmm4
addsd %xmm6, %xmm4
movsd 0x2648(%rip), %xmm3     # 0xf3070
ucomisd %xmm3, %xmm4
ja 0xf0a3c
movsd 0x26c2(%rip), %xmm3     # 0xf30f8
ucomisd %xmm4, %xmm3
jbe 0xf0a4c
subsd %xmm6, %xmm3
movsd %xmm3, 0x34ee08(%rip)   # 0x43f850
movapd %xmm3, %xmm2
mulsd 0x34ee0c(%rip), %xmm0   # 0x43f860
mulsd 0x34edfc(%rip), %xmm1   # 0x43f858
addsd %xmm0, %xmm1
movsd 0x34ee00(%rip), %xmm0   # 0x43f868
mulsd 0x34edb8(%rip), %xmm0   # 0x43f828
addsd %xmm1, %xmm0
movsd %xmm0, 0x34edac(%rip)   # 0x43f828
movsd 0x34ee04(%rip), %xmm1   # 0x43f888
movapd 0x2724(%rip), %xmm3     # 0xf31b0
xorpd %xmm1, %xmm3
xorpd %xmm4, %xmm4
cmpltsd %xmm0, %xmm4
andpd %xmm4, %xmm3
andnpd %xmm1, %xmm4
orpd %xmm3, %xmm4
addsd %xmm0, %xmm4
addsd %xmm2, %xmm6
addsd %xmm4, %xmm6
movsd %xmm6, 0x34ed67(%rip)   # 0x43f820
movsd 0x259f(%rip), %xmm0     # 0xf3060
ucomisd %xmm0, %xmm6
ja 0xf0ad5
movsd 0x2619(%rip), %xmm0     # 0xf30e8
ucomisd %xmm6, %xmm0
jbe 0xf0ae1
movsd %xmm0, 0x34ed43(%rip)   # 0x43f820
movapd %xmm0, %xmm6
leaq 0x238ba4(%rip), %rax    # 0x32968c
movl (%rax), %eax
testl %eax, %eax
je 0xf0b5d
leaq 0x238b9b(%rip), %rcx    # 0x329690
movl (%rcx), %edi
movsd 0x350449(%rip), %xmm0   # 0x440f48
leaq 0x238b8e(%rip), %rdx    # 0x329694
movslq (%rdx), %rsi
cmpl %eax, %edi
jl 0xf0b1a
leaq 0x34f16c(%rip), %r8     # 0x43fc80
subsd (%r8,%rsi,8), %xmm0
leal 0x1(%rsi), %r8d
xorl %r9d, %r9d
cmpl %eax, %r8d
cmovll %r8d, %r9d
leal 0x1(%rdi), %r8d
cmpl %eax, %edi
addsd %xmm6, %xmm0
movsd %xmm0, 0x35040e(%rip)   # 0x440f48
leaq 0x34f13f(%rip), %rdi    # 0x43fc80
movsd %xmm6, (%rdi,%rsi,8)
movl %r9d, (%rdx)
cmovgel %eax, %r8d
movl %r8d, (%rcx)
cvtsi2sd %r8d, %xmm1
divsd %xmm1, %xmm0
movapd %xmm0, %xmm6
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
leaq 0x20659d(%rip), %rax    # 0x2f7108
movsd (%rax), %xmm2
movsd 0x2591(%rip), %xmm1     # 0xf3108
mulsd %xmm6, %xmm1
divsd %xmm2, %xmm1
xorpd %xmm0, %xmm0
cmpneqsd %xmm2, %xmm0
andpd %xmm1, %xmm0
leaq 0x2065cd(%rip), %rax    # 0x2f7160
addsd (%rax), %xmm0
movsd %xmm6, -0x30(%rbp)
jne 0xf0bdd
movsd %xmm5, -0x38(%rbp)
movaps %xmm0, -0x50(%rbp)
callq 0x6779b
movsd (%r13), %xmm2
movsd (%r12), %xmm3
leaq 0x238321(%rip), %rdi    # 0x328ee0
leaq 0x2f3e5(%rip), %rsi     # 0x11ffab
movaps -0x50(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
movsd -0x38(%rbp), %xmm4
movb $0x5, %al
callq 0x9060
jmp 0xf0bed
cvtsd2ss %xmm0, %xmm0
cvtss2sd %xmm0, %xmm0
pushq $0x4
popq %rdi
callq 0x63c97
cmpb $0x0, (%r15)
je 0xf0c5b
movsd 0x34ec25(%rip), %xmm0   # 0x43f820
leaq 0x238a8e(%rip), %rax    # 0x329690
movl (%rax), %edx
movsd 0x34ec24(%rip), %xmm3   # 0x43f830
xorpd 0x259c(%rip), %xmm3     # 0xf31b0
movsd 0x34ec0c(%rip), %xmm2   # 0x43f828
movsd 0x34ec14(%rip), %xmm4   # 0x43f838
movsd 0x34ec24(%rip), %xmm5   # 0x43f850
movsd (%r14), %xmm6
leaq 0x237d68(%rip), %rdi    # 0x3289a0
leaq 0x2f3a6(%rip), %rsi     # 0x11ffe5
movsd -0x30(%rbp), %xmm1
movb $0x7, %al
callq 0x9060
movsd 0x34ebcd(%rip), %xmm0   # 0x43f820
movsd %xmm0, 0x34ebe5(%rip)   # 0x43f840
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq