// File: /workspace/github_cpp_makefile_star2_O1/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 0x111e01(%rip), %rbx    # 0x2188d0
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
jne 0x106bd2
leaq 0x205cd5(%rip), %r14    # 0x30c7b8
movsd (%r14), %xmm0
callq 0xc0f29
leaq 0x238bfc(%rip), %r15    # 0x33f6f0
movsd %xmm0, (%r15)
leaq 0x114428(%rip), %r12    # 0x21af28
movl (%r12), %edx
leaq 0x238285(%rip), %rdi    # 0x33ed90
leaq 0x2f466(%rip), %rsi     # 0x135f78
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
leaq 0x1142e0(%rip), %rcx    # 0x21ae04
movl (%rcx), %ecx
subl %ecx, %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
cvtsi2sd %ecx, %xmm1
addsd %xmm1, %xmm1
divsd %xmm1, %xmm0
addsd 0x252d(%rip), %xmm0     # 0x109070
mulsd (%r15), %xmm0
movsd %xmm0, (%r15)
leaq 0x2380ec(%rip), %rdi    # 0x33ec40
leaq 0x2f433(%rip), %rsi     # 0x135f8e
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
testl %eax, %eax
je 0x106b7f
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
movsd (%r15), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, (%r15)
leaq 0x1140e2(%rip), %rcx    # 0x21ac68
movsd (%r15), %xmm0
divsd (%rcx), %xmm0
movsd %xmm0, (%r15)
cmpl $0x3b, %eax
jg 0x106ba9
leaq 0x114340(%rip), %rax    # 0x21aee0
movsd (%rax), %xmm0
movsd %xmm0, (%r15)
movsd (%r15), %xmm0
leaq 0x237f3b(%rip), %rdi    # 0x33eaf0
leaq 0x2f3de(%rip), %rsi     # 0x135f9a
movb $0x1, %al
callq 0x9060
movsd (%r14), %xmm0
mulsd 0x17620(%rip), %xmm0    # 0x11e1f0
jmp 0x106bf3
leaq 0x205be7(%rip), %rax    # 0x30c7c0
movsd (%rax), %xmm0
leaq 0x238b0c(%rip), %rax    # 0x33f6f0
movsd %xmm0, (%rax)
leaq 0x205bc9(%rip), %rax    # 0x30c7b8
movsd (%rax), %xmm0
leaq 0x238aee(%rip), %r12    # 0x33f6e8
movsd %xmm0, (%r12)
leaq 0x238ad9(%rip), %r14    # 0x33f6e0
movsd (%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x106c18
jnp 0x106c2e
leaq 0x238a99(%rip), %rax    # 0x33f6b8
movsd (%rax), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x106c2e
callq 0x10659d
leaq 0x238a63(%rip), %r15    # 0x33f698
cmpb $0x0, (%r15)
je 0x106c40
callq 0x10671d
movsd 0x34ecc8(%rip), %xmm0   # 0x455910
movsd %xmm0, 0x34ecc8(%rip)   # 0x455918
movsd (%r12), %xmm1
leaq 0x238a63(%rip), %rax    # 0x33f6c0
mulsd (%rax), %xmm1
leaq 0x238a38(%rip), %rax    # 0x33f6a0
movsd (%rax), %xmm5
divsd 0x23c4(%rip), %xmm5     # 0x109038
mulsd %xmm1, %xmm5
leaq 0x238a71(%rip), %r13    # 0x33f6f0
movsd (%r13), %xmm1
addsd %xmm5, %xmm1
movsd %xmm1, 0x34ec7f(%rip)   # 0x455910
movsd %xmm1, 0x34ec8f(%rip)   # 0x455928
movsd 0x2407(%rip), %xmm2     # 0x1090a8
ucomisd %xmm2, %xmm1
ja 0x106cb5
movsd 0x2441(%rip), %xmm2     # 0x1090f0
ucomisd %xmm1, %xmm2
jbe 0x106cbd
movsd %xmm2, 0x34ec6b(%rip)   # 0x455928
movsd 0x34ec3b(%rip), %xmm3   # 0x455900
ucomisd 0x2393(%rip), %xmm3     # 0x109060
movsd 0x34ec53(%rip), %xmm2   # 0x455928
jb 0x106ce1
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0x106cf9
movsd 0x23ff(%rip), %xmm4     # 0x1090e8
ucomisd %xmm3, %xmm4
jb 0x106d15
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0x106d15
movsd 0x34ec6f(%rip), %xmm3   # 0x455970
xorpd 0x2497(%rip), %xmm3     # 0x1091a0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x34ec13(%rip)   # 0x455928
movsd 0x34ec0b(%rip), %xmm3   # 0x455928
mulsd 0x34ec2b(%rip), %xmm3   # 0x455950
addsd 0x34ec03(%rip), %xmm3   # 0x455930
movsd %xmm3, 0x34ebfb(%rip)   # 0x455930
movsd 0x34ec23(%rip), %xmm2   # 0x455960
addsd %xmm2, %xmm3
movsd 0x2327(%rip), %xmm4     # 0x109070
ucomisd %xmm4, %xmm3
ja 0x106d5d
movsd 0x23a1(%rip), %xmm4     # 0x1090f8
ucomisd %xmm3, %xmm4
jbe 0x106d69
subsd %xmm2, %xmm4
movsd %xmm4, 0x34ebc7(%rip)   # 0x455930
mulsd 0x34ebcf(%rip), %xmm0   # 0x455940
mulsd 0x34ebbf(%rip), %xmm1   # 0x455938
addsd %xmm0, %xmm1
movsd 0x34ebc3(%rip), %xmm0   # 0x455948
mulsd 0x34eb7b(%rip), %xmm0   # 0x455908
addsd %xmm1, %xmm0
movsd %xmm0, 0x34eb6f(%rip)   # 0x455908
movsd 0x34ebc7(%rip), %xmm1   # 0x455968
movapd 0x23f7(%rip), %xmm3     # 0x1091a0
xorpd %xmm1, %xmm3
xorpd %xmm4, %xmm4
cmpltsd %xmm0, %xmm4
andpd %xmm4, %xmm3
andnpd %xmm1, %xmm4
orpd %xmm3, %xmm4
addsd %xmm0, %xmm4
addsd 0x34eb62(%rip), %xmm2   # 0x455930
addsd %xmm4, %xmm2
movsd %xmm2, 0x34eb26(%rip)   # 0x455900
movsd 0x227e(%rip), %xmm0     # 0x109060
ucomisd %xmm0, %xmm2
ja 0x106df6
movsd 0x22f8(%rip), %xmm0     # 0x1090e8
ucomisd %xmm2, %xmm0
jbe 0x106dfe
movsd %xmm0, 0x34eb02(%rip)   # 0x455900
leaq 0x238887(%rip), %rax    # 0x33f68c
movl (%rax), %eax
testl %eax, %eax
je 0x106ea0
leaq 0x23887a(%rip), %rcx    # 0x33f690
movl (%rcx), %edx
cmpl %eax, %edx
jl 0x106e42
leaq 0x238871(%rip), %rsi    # 0x33f694
movslq (%rsi), %rsi
leaq 0x34ef33(%rip), %rdi    # 0x455d60
movsd 0x3501f3(%rip), %xmm0   # 0x457028
subsd (%rdi,%rsi,8), %xmm0
movsd %xmm0, 0x3501e6(%rip)   # 0x457028
leaq 0x23884b(%rip), %rsi    # 0x33f694
movslq (%rsi), %rdi
leal 0x1(%rdi), %r8d
xorl %r9d, %r9d
cmpl %eax, %r8d
cmovll %r8d, %r9d
leal 0x1(%rdx), %r8d
cmpl %eax, %edx
movsd 0x34ea98(%rip), %xmm0   # 0x455900
movsd 0x3501b8(%rip), %xmm2   # 0x457028
addsd %xmm0, %xmm2
movsd %xmm2, 0x3501ac(%rip)   # 0x457028
leaq 0x34eedd(%rip), %rdx    # 0x455d60
movsd %xmm0, (%rdx,%rdi,8)
movl %r9d, (%rsi)
cmovgel %eax, %r8d
movl %r8d, (%rcx)
xorps %xmm0, %xmm0
cvtsi2sd %r8d, %xmm0
divsd %xmm0, %xmm2
jmp 0x106ea8
movsd 0x34ea58(%rip), %xmm2   # 0x455900
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
leaq 0x206252(%rip), %rax    # 0x30d108
movsd (%rax), %xmm3
movsd 0x2246(%rip), %xmm1     # 0x109108
mulsd %xmm2, %xmm1
divsd %xmm3, %xmm1
xorpd %xmm0, %xmm0
cmpneqsd %xmm3, %xmm0
andpd %xmm1, %xmm0
leaq 0x206282(%rip), %rax    # 0x30d160
addsd (%rax), %xmm0
movsd %xmm2, -0x30(%rbp)
jne 0x106f28
movsd %xmm5, -0x38(%rbp)
movaps %xmm0, -0x50(%rbp)
callq 0x70f27
movsd (%r13), %xmm2
movsd (%r12), %xmm3
leaq 0x237fd6(%rip), %rdi    # 0x33eee0
leaq 0x2f095(%rip), %rsi     # 0x135fa6
movaps -0x50(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
movsd -0x38(%rbp), %xmm4
movb $0x5, %al
callq 0x9060
jmp 0x106f3a
cvtsd2ss %xmm0, %xmm0
cvtss2sd %xmm0, %xmm0
movl $0x4, %edi
callq 0x6ced6
cmpb $0x0, (%r15)
je 0x106fa8
movsd 0x34e9b8(%rip), %xmm0   # 0x455900
leaq 0x238741(%rip), %rax    # 0x33f690
movl (%rax), %edx
movsd 0x34e9b7(%rip), %xmm3   # 0x455910
xorpd 0x223f(%rip), %xmm3     # 0x1091a0
movsd 0x34e99f(%rip), %xmm2   # 0x455908
movsd 0x34e9a7(%rip), %xmm4   # 0x455918
movsd 0x34e9b7(%rip), %xmm5   # 0x455930
movsd (%r14), %xmm6
leaq 0x237a1b(%rip), %rdi    # 0x33e9a0
leaq 0x2f054(%rip), %rsi     # 0x135fe0
movsd -0x30(%rbp), %xmm1
movb $0x7, %al
callq 0x9060
movsd 0x34e960(%rip), %xmm0   # 0x455900
movsd %xmm0, 0x34e978(%rip)   # 0x455920
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq