// File: /workspace/github_cpp_makefile_star2_O1/jkingan[P]ladyheather/heathmsc.cpp
// Function: temp_pid_filter()
void temp_pid_filter()
{
DATA_SIZE pwm_width;
char cmd[SLEN];

   #ifdef DEBUG_TEMP_CONTROL
      if(pid_debug || bang_bang) {
         show_pid_values();
      }
   #endif

   last_PID_error = PID_error;

   // PID_error is the curent temperature error (desired_temp - current_temp)
   // Negative values indicate the temperature is too high and needs to be 
   // lowered.  Positive values indicate the temperature is too low and needs
   // to be raised.
   //
   // FILTER_OFFSET is a temperature offset constant: negative values raise 
   // the temperature curve,  positive values lower it
   PID_error = (desired_temp + FILTER_OFFSET) - this_temperature;

   // integral_step is the clipped temperature error
   integral_step = PID_error; 
   if(integral_step > MAX_INTEGRAL) integral_step = MAX_INTEGRAL;  
   else if(integral_step < (-MAX_INTEGRAL)) integral_step = (-MAX_INTEGRAL);

   if((PID_out >= MAX_PID) && (integral_step > 0.0)) integral_step *= (-integrator_reset);
   else if((PID_out <= (-MAX_PID)) && (integral_step < 0.0)) integral_step *= (-integrator_reset);

   integrator += (integral_step * k4);
   if((integrator+k6) > (DATA_SIZE) 1.0) integrator = ((DATA_SIZE) 1.0 - k6);    // integrator value is maxed out
   else if((integrator+k6) < ((DATA_SIZE) -1.0)) integrator = ((DATA_SIZE) -1.0 - k6);

   PID_out = (k1*PID_error) + (k2*last_PID_error) + (k3*last_PID_out);
   last_PID_out = PID_out;
   if(PID_out > (DATA_SIZE) 0.0) PID_out -= k7;   // autotune debug - manual step size
   else                          PID_out += k7;
   PID_out += (integrator + k6);

   // clamp the filter response
   if(PID_out > (DATA_SIZE) MAX_PID) PID_out = (DATA_SIZE) MAX_PID;
   else if(PID_out < (DATA_SIZE) (-MAX_PID)) PID_out = (DATA_SIZE) (-MAX_PID);

   // convert filter output to PWM duty cycle times
   pwm_width = (-PID_out) * (DATA_SIZE) PWM_CYCLE_TIME;
   cool_time = (DATA_SIZE) PWM_CYCLE_TIME + pwm_width;
   heat_time = (DATA_SIZE) PWM_CYCLE_TIME - pwm_width;

//sprintf(debug_text, "width:%f  heat:%f(%d)  cool:%f(%d)", pwm_width, heat_time,(int)heat_time,cool_time, (int) cool_time);
   sprintf(cmd, "$CTRL %04d", (int) ((pwm_width/PWM_CYCLE_TIME)*(-1000.0)));
   send_fan_cmd(cmd);

   #ifdef DEBUG_TEMP_CONTROL
      if(pid_debug || bang_bang) {
         sprintf(debug_text,  "pwm=%.2f  heat:%.1f ms  cool:%.1f ms   PID(%.5f)  err(%.5f)  int=%.5f",
            pwm_width, heat_time,cool_time,  
            PID_out,  PID_error*((DATA_SIZE) -1.0),  integrator);
         last_PID_display =  PID_out;
      }
   #endif
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0x108, %rsp            # imm = 0x108
leaq 0x239f18(%rip), %r15    # 0x33f640
cmpb $0x0, (%r15)
leaq 0x239f09(%rip), %r14    # 0x33f63c
jne 0x10573b
cmpl $0x0, (%r14)
je 0x105740
callq 0x1054b7
movsd 0x3500a8(%rip), %xmm1   # 0x4557f0
leaq 0x23a679(%rip), %rax    # 0x33fdc8
movsd (%rax), %xmm0
leaq 0x239f0e(%rip), %rax    # 0x33f668
addsd (%rax), %xmm0
movsd %xmm1, 0x350092(%rip)   # 0x4557f8
subsd 0x34ff7a(%rip), %xmm0   # 0x4556e8
movsd %xmm0, 0x35007a(%rip)   # 0x4557f0
movsd %xmm0, 0x35008a(%rip)   # 0x455808
movsd 0x624a(%rip), %xmm2     # 0x10b9d0
ucomisd %xmm2, %xmm0
ja 0x10579a
movsd 0x628c(%rip), %xmm2     # 0x10ba20
ucomisd %xmm0, %xmm2
jbe 0x1057a2
movsd %xmm2, 0x350066(%rip)   # 0x455808
movsd 0x350036(%rip), %xmm3   # 0x4557e0
ucomisd 0x26f06(%rip), %xmm3    # 0x12c6b8
movsd 0x35004e(%rip), %xmm2   # 0x455808
jb 0x1057c6
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0x1057de
movsd 0x26ef2(%rip), %xmm4    # 0x12c6c0
ucomisd %xmm3, %xmm4
jb 0x1057fa
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0x1057fa
movsd 0x350062(%rip), %xmm3   # 0x455848
xorpd 0x39b2(%rip), %xmm3     # 0x1091a0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x35000e(%rip)   # 0x455808
movsd 0x350006(%rip), %xmm3   # 0x455808
mulsd 0x350026(%rip), %xmm3   # 0x455830
addsd 0x34fffe(%rip), %xmm3   # 0x455810
movsd %xmm3, 0x34fff6(%rip)   # 0x455810
movsd 0x350016(%rip), %xmm2   # 0x455838
addsd %xmm2, %xmm3
movsd 0x3842(%rip), %xmm4     # 0x109070
ucomisd %xmm4, %xmm3
ja 0x105842
movsd 0x38bc(%rip), %xmm4     # 0x1090f8
ucomisd %xmm3, %xmm4
jbe 0x10584e
subsd %xmm2, %xmm4
movsd %xmm4, 0x34ffc2(%rip)   # 0x455810
mulsd 0x34ffca(%rip), %xmm1   # 0x455820
mulsd 0x34ffba(%rip), %xmm0   # 0x455818
addsd %xmm1, %xmm0
movsd 0x34ffbe(%rip), %xmm1   # 0x455828
mulsd 0x34ff76(%rip), %xmm1   # 0x4557e8
addsd %xmm0, %xmm1
movsd %xmm1, 0x34ff6a(%rip)   # 0x4557e8
movsd 0x34ffba(%rip), %xmm0   # 0x455840
movapd 0x3912(%rip), %xmm3     # 0x1091a0
xorpd %xmm0, %xmm3
xorpd %xmm4, %xmm4
cmpltsd %xmm1, %xmm4
andpd %xmm4, %xmm3
andnpd %xmm0, %xmm4
orpd %xmm3, %xmm4
addsd %xmm1, %xmm4
addsd 0x34ff5d(%rip), %xmm2   # 0x455810
addsd %xmm4, %xmm2
movsd %xmm2, 0x34ff21(%rip)   # 0x4557e0
movsd 0x26df1(%rip), %xmm0    # 0x12c6b8
ucomisd %xmm0, %xmm2
ja 0x1058db
movsd 0x26deb(%rip), %xmm0    # 0x12c6c0
ucomisd %xmm2, %xmm0
jbe 0x1058e3
movsd %xmm0, 0x34fefd(%rip)   # 0x4557e0
movsd 0x34fef5(%rip), %xmm2   # 0x4557e0
mulsd 0x26dd5(%rip), %xmm2    # 0x12c6c8
movsd 0x26dd5(%rip), %xmm0    # 0x12c6d0
movapd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, 0x34fe1d(%rip)   # 0x455728
movapd %xmm2, %xmm1
divsd %xmm0, %xmm1
movsd %xmm2, -0x20(%rbp)
subsd %xmm2, %xmm0
movsd %xmm0, 0x34fdfc(%rip)   # 0x455720
mulsd 0x37b4(%rip), %xmm1     # 0x1090e0
cvttsd2si %xmm1, %edx
leaq 0x304d1(%rip), %rsi     # 0x135e08
leaq -0x120(%rbp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x9060
movq %rbx, %rdi
callq 0x5f5fd
cmpb $0x0, (%r15)
jne 0x10595c
cmpl $0x0, (%r14)
je 0x1059b6
movsd 0x34fdbc(%rip), %xmm1   # 0x455720
movsd 0x34fdbc(%rip), %xmm2   # 0x455728
movsd 0x34fe6c(%rip), %xmm3   # 0x4557e0
movsd 0x34fe74(%rip), %xmm4   # 0x4557f0
xorpd 0x381c(%rip), %xmm4     # 0x1091a0
movsd 0x34fe84(%rip), %xmm5   # 0x455810
leaq 0x23900d(%rip), %rdi    # 0x33e9a0
leaq 0x30479(%rip), %rsi     # 0x135e13
movsd -0x20(%rbp), %xmm0
movb $0x6, %al
callq 0x9060
movsd 0x34fe32(%rip), %xmm0   # 0x4557e0
movsd %xmm0, 0x34fe4a(%rip)   # 0x455800
addq $0x108, %rsp            # imm = 0x108
popq %rbx
popq %r14
popq %r15
popq %rbp
retq