// File: /workspace/github_cpp_makefile_star2_O3/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 0x239824(%rip), %r15    # 0x33f640
cmpb $0x0, (%r15)
leaq 0x239815(%rip), %r14    # 0x33f63c
jne 0x105e2f
cmpl $0x0, (%r14)
je 0x105e34
callq 0x105ba9
movsd 0x34f8d4(%rip), %xmm1   # 0x455710
leaq 0x239f85(%rip), %rax    # 0x33fdc8
movsd (%rax), %xmm0
leaq 0x23981a(%rip), %rax    # 0x33f668
addsd (%rax), %xmm0
movsd %xmm1, 0x34f8be(%rip)   # 0x455718
subsd 0x34f7a6(%rip), %xmm0   # 0x455608
movsd %xmm0, 0x34f8a6(%rip)   # 0x455710
movsd %xmm0, 0x34f8b6(%rip)   # 0x455728
ucomisd 0x5b56(%rip), %xmm0     # 0x10b9d0
jbe 0x105e97
movabsq $0x4000000000000000, %rax # imm = 0x4000000000000000
movq %rax, 0x34f89b(%rip)    # 0x455728
movsd 0x5b3b(%rip), %xmm2     # 0x10b9d0
jmp 0x105ec8
movsd 0x5b81(%rip), %xmm4     # 0x10ba20
ucomisd %xmm0, %xmm4
movapd %xmm0, %xmm2
jbe 0x105ec8
movabsq $-0x4000000000000000, %rax # imm = 0xC000000000000000
movq %rax, 0x34f86e(%rip)    # 0x455728
movsd 0x34f83e(%rip), %xmm3   # 0x455700
movapd %xmm4, %xmm2
jmp 0x105ee4
movsd 0x34f830(%rip), %xmm3   # 0x455700
ucomisd 0x268c8(%rip), %xmm3    # 0x12c7a0
jb 0x105ee4
xorpd %xmm4, %xmm4
ucomisd %xmm4, %xmm2
ja 0x105efc
movsd 0x268bc(%rip), %xmm4    # 0x12c7a8
ucomisd %xmm3, %xmm4
jb 0x105f18
xorpd %xmm3, %xmm3
ucomisd %xmm2, %xmm3
jbe 0x105f18
movsd 0x34f864(%rip), %xmm3   # 0x455768
xorpd 0x32a4(%rip), %xmm3     # 0x1091b0
mulsd %xmm3, %xmm2
movsd %xmm2, 0x34f810(%rip)   # 0x455728
mulsd 0x34f830(%rip), %xmm2   # 0x455750
addsd 0x34f808(%rip), %xmm2   # 0x455730
movsd %xmm2, 0x34f800(%rip)   # 0x455730
movsd 0x34f820(%rip), %xmm5   # 0x455758
movapd %xmm2, %xmm4
addsd %xmm5, %xmm4
movsd 0x3128(%rip), %xmm3     # 0x109070
ucomisd %xmm3, %xmm4
ja 0x105f5c
movsd 0x31a2(%rip), %xmm3     # 0x1090f8
ucomisd %xmm4, %xmm3
jbe 0x105f6c
subsd %xmm5, %xmm3
movsd %xmm3, 0x34f7c8(%rip)   # 0x455730
movapd %xmm3, %xmm2
mulsd 0x34f7cc(%rip), %xmm1   # 0x455740
mulsd 0x34f7bc(%rip), %xmm0   # 0x455738
addsd %xmm1, %xmm0
movsd 0x34f7c0(%rip), %xmm1   # 0x455748
mulsd 0x34f778(%rip), %xmm1   # 0x455708
addsd %xmm0, %xmm1
movsd %xmm1, 0x34f76c(%rip)   # 0x455708
movsd 0x34f7bc(%rip), %xmm0   # 0x455760
movapd 0x3204(%rip), %xmm3     # 0x1091b0
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, 0x34f727(%rip)   # 0x455700
movsd 0x267bf(%rip), %xmm0    # 0x12c7a0
ucomisd %xmm0, %xmm5
ja 0x105ff5
movsd 0x267b9(%rip), %xmm0    # 0x12c7a8
ucomisd %xmm5, %xmm0
jbe 0x106001
movsd %xmm0, 0x34f703(%rip)   # 0x455700
movapd %xmm0, %xmm5
mulsd 0x267a7(%rip), %xmm5    # 0x12c7b0
movsd 0x267a7(%rip), %xmm0    # 0x12c7b8
movapd %xmm5, %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, 0x34f627(%rip)   # 0x455648
movapd %xmm5, %xmm1
divsd %xmm0, %xmm1
movsd %xmm5, -0x20(%rbp)
subsd %xmm5, %xmm0
movsd %xmm0, 0x34f606(%rip)   # 0x455640
mulsd 0x309e(%rip), %xmm1     # 0x1090e0
cvttsd2si %xmm1, %edx
leaq 0x2feb4(%rip), %rsi     # 0x135f01
leaq -0x120(%rbp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x9060
movq %rbx, %rdi
callq 0x5f585
cmpb $0x0, (%r15)
jne 0x106072
cmpl $0x0, (%r14)
je 0x1060cc
movsd 0x34f5c6(%rip), %xmm1   # 0x455640
movsd 0x34f5c6(%rip), %xmm2   # 0x455648
movsd 0x34f676(%rip), %xmm3   # 0x455700
movsd 0x34f67e(%rip), %xmm4   # 0x455710
xorpd 0x3116(%rip), %xmm4     # 0x1091b0
movsd 0x34f68e(%rip), %xmm5   # 0x455730
leaq 0x2388f7(%rip), %rdi    # 0x33e9a0
leaq 0x2fe5c(%rip), %rsi     # 0x135f0c
movsd -0x20(%rbp), %xmm0
movb $0x6, %al
callq 0x9060
movsd 0x34f63c(%rip), %xmm0   # 0x455700
movsd %xmm0, 0x34f654(%rip)   # 0x455720
addq $0x108, %rsp            # imm = 0x108
popq %rbx
popq %r14
popq %r15
popq %rbp
retq