// File: /workspace/github_cpp_makefile_star2_O2/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 0x23a156(%rip), %r15    # 0x329640
cmpb $0x0, (%r15)
leaq 0x23a147(%rip), %r14    # 0x32963c
jne 0xef4fd
cmpl $0x0, (%r14)
je 0xef502
callq 0xef290
movsd 0x350206(%rip), %xmm1   # 0x43f710
leaq 0x23a8b7(%rip), %rax    # 0x329dc8
movsd (%rax), %xmm0
leaq 0x23a14c(%rip), %rax    # 0x329668
addsd (%rax), %xmm0
movsd %xmm1, 0x3501f0(%rip)   # 0x43f718
subsd 0x3500d8(%rip), %xmm0   # 0x43f608
movsd %xmm0, 0x3501d8(%rip)   # 0x43f710
movsd %xmm0, 0x3501e8(%rip)   # 0x43f728
movsd 0x6500(%rip), %xmm3     # 0xf5a48
ucomisd %xmm3, %xmm0
ja 0xef560
movsd 0x6542(%rip), %xmm3     # 0xf5a98
ucomisd %xmm0, %xmm3
movapd %xmm0, %xmm2
jbe 0xef56c
movsd %xmm3, 0x3501c0(%rip)   # 0x43f728
movapd %xmm3, %xmm2
movsd 0x35018c(%rip), %xmm3   # 0x43f700
ucomisd 0x27144(%rip), %xmm3    # 0x1166c0
jb 0xef588
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0xef5a0
movsd 0x27138(%rip), %xmm4    # 0x1166c8
ucomisd %xmm3, %xmm4
jb 0xef5bc
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0xef5bc
movsd 0x3501c0(%rip), %xmm3   # 0x43f768
xorpd 0x3c00(%rip), %xmm3     # 0xf31b0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x35016c(%rip)   # 0x43f728
mulsd 0x35018c(%rip), %xmm2   # 0x43f750
addsd 0x350164(%rip), %xmm2   # 0x43f730
movsd %xmm2, 0x35015c(%rip)   # 0x43f730
movsd 0x35017c(%rip), %xmm5   # 0x43f758
movapd %xmm2, %xmm4
addsd %xmm5, %xmm4
movsd 0x3a84(%rip), %xmm3     # 0xf3070
ucomisd %xmm3, %xmm4
ja 0xef600
movsd 0x3afe(%rip), %xmm3     # 0xf30f8
ucomisd %xmm4, %xmm3
jbe 0xef610
subsd %xmm5, %xmm3
movsd %xmm3, 0x350124(%rip)   # 0x43f730
movapd %xmm3, %xmm2
mulsd 0x350128(%rip), %xmm1   # 0x43f740
mulsd 0x350118(%rip), %xmm0   # 0x43f738
addsd %xmm1, %xmm0
movsd 0x35011c(%rip), %xmm1   # 0x43f748
mulsd 0x3500d4(%rip), %xmm1   # 0x43f708
addsd %xmm0, %xmm1
movsd %xmm1, 0x3500c8(%rip)   # 0x43f708
movsd 0x350118(%rip), %xmm0   # 0x43f760
movapd 0x3b60(%rip), %xmm3     # 0xf31b0
xorpd %xmm0, %xmm3
xorpd %xmm4, %xmm4
cmpltsd %xmm1, %xmm4
andpd %xmm4, %xmm3
andnpd %xmm0, %xmm4
orpd %xmm3, %xmm4
addsd %xmm1, %xmm4
addsd %xmm2, %xmm5
addsd %xmm4, %xmm5
movsd %xmm5, 0x350083(%rip)   # 0x43f700
movsd 0x2703b(%rip), %xmm0    # 0x1166c0
ucomisd %xmm0, %xmm5
ja 0xef699
movsd 0x27035(%rip), %xmm0    # 0x1166c8
ucomisd %xmm5, %xmm0
jbe 0xef6a5
movsd %xmm0, 0x35005f(%rip)   # 0x43f700
movapd %xmm0, %xmm5
mulsd 0x27023(%rip), %xmm5    # 0x1166d0
movsd 0x27023(%rip), %xmm0    # 0x1166d8
movapd %xmm5, %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, 0x34ff83(%rip)   # 0x43f648
movapd %xmm5, %xmm1
divsd %xmm0, %xmm1
movsd %xmm5, -0x20(%rbp)
subsd %xmm5, %xmm0
movsd %xmm0, 0x34ff62(%rip)   # 0x43f640
mulsd 0x39fa(%rip), %xmm1     # 0xf30e0
cvttsd2si %xmm1, %edx
leaq 0x3071c(%rip), %rsi     # 0x11fe0d
leaq -0x120(%rbp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x9060
movq %rbx, %rdi
callq 0x57ddb
cmpb $0x0, (%r15)
jne 0xef716
cmpl $0x0, (%r14)
je 0xef770
movsd 0x34ff22(%rip), %xmm1   # 0x43f640
movsd 0x34ff22(%rip), %xmm2   # 0x43f648
movsd 0x34ffd2(%rip), %xmm3   # 0x43f700
movsd 0x34ffda(%rip), %xmm4   # 0x43f710
xorpd 0x3a72(%rip), %xmm4     # 0xf31b0
movsd 0x34ffea(%rip), %xmm5   # 0x43f730
leaq 0x239253(%rip), %rdi    # 0x3289a0
leaq 0x306c4(%rip), %rsi     # 0x11fe18
movsd -0x20(%rbp), %xmm0
movb $0x6, %al
callq 0x9060
movsd 0x34ff98(%rip), %xmm0   # 0x43f700
movsd %xmm0, 0x34ffb0(%rip)   # 0x43f720
addq $0x108, %rsp            # imm = 0x108
popq %rbx
popq %r14
popq %r15
popq %rbp
retq