// File: /workspace/github_cpp_makefile_star2_O3/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 0x1116fc(%rip), %rbx    # 0x2188d0
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
jne 0x1072c8
leaq 0x2055d0(%rip), %r14    # 0x30c7b8
movsd (%r14), %xmm0
callq 0xc1507
leaq 0x2384f7(%rip), %r15    # 0x33f6f0
movsd %xmm0, (%r15)
leaq 0x113d23(%rip), %r12    # 0x21af28
movl (%r12), %edx
leaq 0x237b80(%rip), %rdi    # 0x33ed90
leaq 0x2ee5a(%rip), %rsi     # 0x136071
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
leaq 0x113bdb(%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 0x1e28(%rip), %xmm0     # 0x109070
mulsd (%r15), %xmm0
movsd %xmm0, (%r15)
leaq 0x2379e7(%rip), %rdi    # 0x33ec40
leaq 0x2ee27(%rip), %rsi     # 0x136087
movb $0x1, %al
callq 0x9060
movl (%r12), %eax
testl %eax, %eax
je 0x107294
xorps %xmm1, %xmm1
cvtsi2sd %eax, %xmm1
movsd (%r15), %xmm0
divsd %xmm1, %xmm0
leaq 0x1139e2(%rip), %rcx    # 0x21ac68
divsd (%rcx), %xmm0
movsd %xmm0, (%r15)
cmpl $0x3b, %eax
jg 0x1072a4
leaq 0x113c45(%rip), %rax    # 0x21aee0
movsd (%rax), %xmm0
movsd %xmm0, (%r15)
leaq 0x237845(%rip), %rdi    # 0x33eaf0
leaq 0x2ede1(%rip), %rsi     # 0x136093
movb $0x1, %al
callq 0x9060
movsd (%r14), %xmm0
mulsd 0x1e82(%rip), %xmm0     # 0x109148
jmp 0x1072e9
leaq 0x2054f1(%rip), %rax    # 0x30c7c0
movsd (%rax), %xmm0
leaq 0x238416(%rip), %rax    # 0x33f6f0
movsd %xmm0, (%rax)
leaq 0x2054d3(%rip), %rax    # 0x30c7b8
movsd (%rax), %xmm0
leaq 0x2383f8(%rip), %r12    # 0x33f6e8
movsd %xmm0, (%r12)
leaq 0x2383e3(%rip), %r14    # 0x33f6e0
movsd (%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x10730e
jnp 0x107324
leaq 0x2383a3(%rip), %rax    # 0x33f6b8
movsd (%rax), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x107324
callq 0x106cb3
leaq 0x23836d(%rip), %r15    # 0x33f698
cmpb $0x0, (%r15)
je 0x107336
callq 0x106e22
movsd 0x34e4f2(%rip), %xmm0   # 0x455830
movsd %xmm0, 0x34e4f2(%rip)   # 0x455838
movsd (%r12), %xmm1
leaq 0x23836d(%rip), %rax    # 0x33f6c0
mulsd (%rax), %xmm1
leaq 0x238342(%rip), %rax    # 0x33f6a0
movsd (%rax), %xmm5
divsd 0x1cce(%rip), %xmm5     # 0x109038
mulsd %xmm1, %xmm5
leaq 0x23837b(%rip), %r13    # 0x33f6f0
movsd (%r13), %xmm1
addsd %xmm5, %xmm1
movsd %xmm1, 0x34e4a9(%rip)   # 0x455830
movsd %xmm1, 0x34e4b9(%rip)   # 0x455848
ucomisd 0x1d11(%rip), %xmm1     # 0x1090a8
jbe 0x1073b4
movabsq $0x4024000000000000, %rax # imm = 0x4024000000000000
movq %rax, 0x34e49e(%rip)    # 0x455848
movsd 0x1cf6(%rip), %xmm2     # 0x1090a8
jmp 0x1073e5
movsd 0x1d34(%rip), %xmm4     # 0x1090f0
ucomisd %xmm1, %xmm4
movapd %xmm1, %xmm2
jbe 0x1073e5
movabsq $-0x3fdc000000000000, %rax # imm = 0xC024000000000000
movq %rax, 0x34e471(%rip)    # 0x455848
movsd 0x34e441(%rip), %xmm3   # 0x455820
movapd %xmm4, %xmm2
jmp 0x107401
movsd 0x34e433(%rip), %xmm3   # 0x455820
ucomisd 0x1c6b(%rip), %xmm3     # 0x109060
jb 0x107401
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0x107419
movsd 0x1cdf(%rip), %xmm4     # 0x1090e8
ucomisd %xmm3, %xmm4
jb 0x107435
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0x107435
movsd 0x34e46f(%rip), %xmm3   # 0x455890
xorpd 0x1d87(%rip), %xmm3     # 0x1091b0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x34e413(%rip)   # 0x455848
mulsd 0x34e433(%rip), %xmm2   # 0x455870
addsd 0x34e40b(%rip), %xmm2   # 0x455850
movsd %xmm2, 0x34e403(%rip)   # 0x455850
movsd 0x34e42b(%rip), %xmm6   # 0x455880
movapd %xmm2, %xmm4
addsd %xmm6, %xmm4
movsd 0x1c0b(%rip), %xmm3     # 0x109070
ucomisd %xmm3, %xmm4
ja 0x107479
movsd 0x1c85(%rip), %xmm3     # 0x1090f8
ucomisd %xmm4, %xmm3
jbe 0x107489
subsd %xmm6, %xmm3
movsd %xmm3, 0x34e3cb(%rip)   # 0x455850
movapd %xmm3, %xmm2
mulsd 0x34e3cf(%rip), %xmm0   # 0x455860
mulsd 0x34e3bf(%rip), %xmm1   # 0x455858
addsd %xmm0, %xmm1
movsd 0x34e3c3(%rip), %xmm0   # 0x455868
mulsd 0x34e37b(%rip), %xmm0   # 0x455828
addsd %xmm1, %xmm0
movsd %xmm0, 0x34e36f(%rip)   # 0x455828
movsd 0x34e3c7(%rip), %xmm1   # 0x455888
movapd 0x1ce7(%rip), %xmm3     # 0x1091b0
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, 0x34e32a(%rip)   # 0x455820
movsd 0x1b62(%rip), %xmm0     # 0x109060
ucomisd %xmm0, %xmm6
ja 0x107512
movsd 0x1bdc(%rip), %xmm0     # 0x1090e8
ucomisd %xmm6, %xmm0
jbe 0x10751e
movsd %xmm0, 0x34e306(%rip)   # 0x455820
movapd %xmm0, %xmm6
leaq 0x238167(%rip), %rax    # 0x33f68c
movl (%rax), %eax
testl %eax, %eax
je 0x10759d
leaq 0x23815e(%rip), %rcx    # 0x33f690
movl (%rcx), %edi
movsd 0x34fa0c(%rip), %xmm0   # 0x456f48
leaq 0x238151(%rip), %rdx    # 0x33f694
movslq (%rdx), %rsi
cmpl %eax, %edi
jl 0x107557
leaq 0x34e72f(%rip), %r8     # 0x455c80
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, 0x34f9d1(%rip)   # 0x456f48
leaq 0x34e702(%rip), %rdi    # 0x455c80
movsd %xmm6, (%rdi,%rsi,8)
movl %r9d, (%rdx)
cmovgel %eax, %r8d
movl %r8d, (%rcx)
xorps %xmm1, %xmm1
cvtsi2sd %r8d, %xmm1
divsd %xmm1, %xmm0
movapd %xmm0, %xmm6
cmpq $0x2000000, (%rbx)      # imm = 0x2000000
leaq 0x205b5d(%rip), %rax    # 0x30d108
movsd (%rax), %xmm2
movsd 0x1b51(%rip), %xmm1     # 0x109108
mulsd %xmm6, %xmm1
divsd %xmm2, %xmm1
xorpd %xmm0, %xmm0
cmpneqsd %xmm2, %xmm0
andpd %xmm1, %xmm0
leaq 0x205b8d(%rip), %rax    # 0x30d160
addsd (%rax), %xmm0
movsd %xmm6, -0x30(%rbp)
jne 0x10761d
movsd %xmm5, -0x38(%rbp)
movaps %xmm0, -0x50(%rbp)
callq 0x70fb0
movsd (%r13), %xmm2
movsd (%r12), %xmm3
leaq 0x2378e1(%rip), %rdi    # 0x33eee0
leaq 0x2ea99(%rip), %rsi     # 0x13609f
movaps -0x50(%rbp), %xmm0
movsd -0x30(%rbp), %xmm1
movsd -0x38(%rbp), %xmm4
movb $0x5, %al
callq 0x9060
jmp 0x10762f
cvtsd2ss %xmm0, %xmm0
cvtss2sd %xmm0, %xmm0
movl $0x4, %edi
callq 0x6ce5e
cmpb $0x0, (%r15)
je 0x10769d
movsd 0x34e1e3(%rip), %xmm0   # 0x455820
leaq 0x23804c(%rip), %rax    # 0x33f690
movl (%rax), %edx
movsd 0x34e1e2(%rip), %xmm3   # 0x455830
xorpd 0x1b5a(%rip), %xmm3     # 0x1091b0
movsd 0x34e1ca(%rip), %xmm2   # 0x455828
movsd 0x34e1d2(%rip), %xmm4   # 0x455838
movsd 0x34e1e2(%rip), %xmm5   # 0x455850
movsd (%r14), %xmm6
leaq 0x237326(%rip), %rdi    # 0x33e9a0
leaq 0x2ea58(%rip), %rsi     # 0x1360d9
movsd -0x30(%rbp), %xmm1
movb $0x7, %al
callq 0x9060
movsd 0x34e18b(%rip), %xmm0   # 0x455820
movsd %xmm0, 0x34e1a3(%rip)   # 0x455840
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq