// File: /workspace/github_cpp_makefile_star2_O2/jkingan[P]ladyheather/heathgps.cpp
// Function: x72_discipline_state()
void x72_discipline_state()
{
static int change_count;
double slope;
int val;
int i1, i2;
int flag;
double x;
double dds_change;

   // discipline the output frequency 

   if(pps_enabled == 0) {  // should never happen
      x72_start_discipline();
      return;
   }
   if(x72_tc_val <= 0) {
      x72_tc_val = X72_TIME_CONST;
       x72_reset_filter();
   }
   if(x72_jamthresh_val > X72_JAMSYNC_THRESH) x72_jamthresh_val = X72_JAMSYNC_THRESH;
   if(x72_jamthresh_val <= 0) x72_jamthresh_val = X72_JAMSYNC_THRESH;

   val = x72_pps;
   val = fix_x72_tics(val);
if(show_debug_info) sprintf(debug_text, "disciplining!  interval:%d   ival:%d  last:%d  sum:%d mcv:%g   dds:%g   x72_pps:%d -> %d   ppo:%g", x72_pps_interval, x72_ival,last_ival,x72_dis_sum,x72_pps_stats(0),  last_x72_dds, x72_pps, val, pps_offset);
   val = IABS(val);

   if((val > (int) (x72_jamthresh_val*1.0E-9*x72_osc))) {  // PPS is off by a large amount, do an immediate jamsync
if(show_debug_info) sprintf(debug_text5, "disipline holdover pps limit!!! %d", x72_pps);
      x72_start_discipline();
// !!!!!!!! if DDS is too far off and drift exceeds this rate, we'll never start disciplining
//x72_pps_stats(0);
//slope = x72_pps_trend;
//slope /= (double) x72_pps_interval;
//goto jamit_damnit;
      return;
   }

   if(x72_pps_interval == 0) last_dis_ival = x72_ival;
   i1 = fix_x72_tics(x72_ival);           // fixup possible wrap near 0
   i2 = fix_x72_tics(last_dis_ival);
   last_dis_ival = x72_ival;

   x72_update_pps_list(i1);      // add fixed up ival to pps list

   x72_dis_sum += (i1 - i2);     // accumulate difference between the last two PPS values
   if(IABS(x72_dis_sum) >= 2) ++x72_sum_sum;   // error sum exceeds threshold, see for how long
   else                       x72_sum_sum = 0; // error sum is back in allowed range

   flag = 0;
#define FORCE_TIC 3  // 3 -> 50 ns   was 6
   if((IABS(x72_dis_sum) >= FORCE_TIC) && (x72_pps_interval >= (LONG_INTERVAL*2))) {   // was *1
      flag = 1;   // accumulated PPS error is large over a long interval
   }
   else if(1 && (IABS(x72_dis_sum) >= (FORCE_TIC*3))) {  // was *2 - bigger error triggers an immediate DDS change
      flag = 2;
   }

   if(IABS(x72_sum_sum) >= 5) {  // PPS is off by 2 or more tics for over 5 consecutive seconds
      if((x72_pps_interval >= x72_tc_val) || (flag == 2)) { // we have enough data to evaluate
         x = x72_pps_stats(1);
         if(x || flag) { // PPS most common value is not zero or large PPS error
            // update DDS based on the interval average PPS error
            if(1 && (x72_pps_interval < LONG_INTERVAL)) { // qwerty drift rate is a bit large, scale average value betwen 1.0 and 2.0 times the average
               slope = x72_pps_avg * (1.0 + (((double) (x72_pps_interval - x72_tc_val)) / (2.0*(double) x72_tc_val)) );
            }
            else {  // very slow drift rate
               slope = x72_pps_trend;
            }
            slope /= (double) x72_pps_interval;
         }
         else slope = 0.0;

//jamit_damnit:
         if(slope) {  // adjust the DDS frequency to cancel the average PPS drift during the interval
            slope /= (double) x72_osc;
            last_interval_dds = interval_start_dds;
            interval_start_dds = last_x72_dds + slope;
interval_start_dds = x72_avg_dds(interval_start_dds);
            ++change_count;
            dds_change = interval_start_dds-last_x72_dds;
if(show_debug_info) {
   BEEP(516);
   sprintf(plot_title, "New dds %d!  %g olddds:%g  change:%g  interval:%d  ival:%d  lastiv:%d  sum:%d  mcv:%g  avg:%g  flag:%d", change_count, interval_start_dds,last_x72_dds,dds_change, x72_pps_interval, x72_ival, last_dis_ival, x72_dis_sum, x72_pps_stats(0), x72_pps_avg, flag);
   mark_q_entry[9] = plot_q_in;
}
if(debug_file) fprintf(debug_file, "New dds %d!  %g olddds:%g  change:%g  interval:%d  ival:%d  lastiv:%d  sum:%d mcv:%g  avg:%g  flag:%d\n", change_count,interval_start_dds,last_x72_dds,dds_change, x72_pps_interval, x72_ival, last_dis_ival, x72_dis_sum, x72_pps_stats(0), x72_pps_avg, flag);

            set_x72_dds(interval_start_dds);
            have_x72_pps = 0;
            last_dis_ival = x72_ival;
            new_x72_interval(3);

            update_x72_tic(last_x72_pps, 3);  // re-align PPS out to PPS in
            x72_jamsync_ticks = 0;   // we just changed the TIC register to move the PPS output
            x72_jamsync_flag = 0;    // now wait for the change to take effect
            x72_user_state = X72_STATE_JAM2;
         }
      }
   }
   else {  // PID is not currently used
//      update_x72_pid();
   }
update_x72_pid();

   return;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
leaq 0x247208(%rip), %rax    # 0x2f7d20
cmpb $0x0, (%rax)
je 0xb0c9f
leaq 0x1542dc(%rip), %rcx    # 0x204e04
cmpl $0x0, (%rcx)
jg 0xb0b63
leaq 0x1543f4(%rip), %rax    # 0x204f28
andl $0x0, (%rax)
andq $0x0, 0x2afbf1(%rip)    # 0x360730
andl $0x0, 0x2b31ea(%rip)    # 0x363d30
andl $0x0, 0x2b31e7(%rip)    # 0x363d34
andq $0x0, 0x2b31d3(%rip)    # 0x363d28
movl $0x190, (%rcx)          # imm = 0x190
andq $0x0, 0x2b31d5(%rip)    # 0x363d38
leaq 0x1542b6(%rip), %rcx    # 0x204e20
movsd (%rcx), %xmm1
ucomisd 0x44e2a(%rip), %xmm1    # 0xf59a0
ja 0xb0b82
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jb 0xb0b97
movabsq $0x4072c00000000000, %rax # imm = 0x4072C00000000000
movq %rax, (%rcx)
movsd 0x44e09(%rip), %xmm1    # 0xf59a0
leaq 0x15425e(%rip), %r8     # 0x204dfc
movl (%r8), %r13d
leaq 0x1540c0(%rip), %rbx    # 0x204c68
movsd (%rbx), %xmm0
cvttsd2si %xmm0, %ecx
pushq $0x2
popq %rsi
movl %ecx, %eax
cltd
idivl %esi
xorl %edx, %edx
cmpl %r13d, %eax
cmovll %ecx, %edx
subl %edx, %r13d
leaq 0x151cfa(%rip), %rax    # 0x2028c4
cmpb $0x0, (%rax)
je 0xb0c5e
leaq 0x15434a(%rip), %rax    # 0x204f24
movl (%rax), %ebx
leaq 0x154359(%rip), %rax    # 0x204f3c
movl (%rax), %r14d
movl 0x2ab2b3(%rip), %r15d   # 0x35bea0
movl 0x2afa40(%rip), %r12d   # 0x360634
callq 0xb0447
leaq 0x1542e0(%rip), %rax    # 0x204ee0
movsd (%rax), %xmm1
leaq 0x1541f1(%rip), %rax    # 0x204dfc
movl (%rax), %r10d
leaq 0x245ba3(%rip), %rax    # 0x2f67b8
movsd (%rax), %xmm2
leaq 0x277d80(%rip), %rdi    # 0x3289a0
leaq 0x6405c(%rip), %rsi     # 0x114c83
movl %ebx, %edx
leaq 0x154038(%rip), %rbx    # 0x204c68
movl %r14d, %ecx
movl %r15d, %r8d
movl %r12d, %r9d
movb $0x3, %al
pushq %r13
pushq %r10
callq 0x9060
leaq 0x1541b1(%rip), %r8     # 0x204dfc
addq $0x10, %rsp
leaq 0x1541ca(%rip), %rax    # 0x204e20
movsd (%rax), %xmm1
movsd (%rbx), %xmm0
movl %r13d, %eax
negl %eax
cmovsl %r13d, %eax
mulsd 0x424d9(%rip), %xmm1    # 0xf3148
mulsd %xmm0, %xmm1
cvttsd2si %xmm1, %ecx
cmpl %ecx, %eax
jle 0xb0cb2
leaq 0x151c42(%rip), %rax    # 0x2028c4
cmpb $0x0, (%rax)
je 0xb0c9f
movl (%r8), %edx
leaq 0x27824f(%rip), %rdi    # 0x328ee0
leaq 0x6404d(%rip), %rsi     # 0x114ce5
xorl %eax, %eax
callq 0x9060
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x83005
movq %rbx, %r13
leaq 0x154268(%rip), %r12    # 0x204f24
xorl %r15d, %r15d
leaq 0x154276(%rip), %rax    # 0x204f3c
movl (%rax), %ecx
movl %ecx, %esi
cmpl %r15d, (%r12)
je 0xb0cd6
movl 0x2ab1ce(%rip), %esi    # 0x35bea4
cvttsd2si %xmm0, %ebx
movl %ebx, %eax
cltd
pushq $0x2
popq %rdi
idivl %edi
cmpl %ecx, %eax
movl $0x0, %edx
cmovll %ebx, %edx
movl %ecx, %r14d
subl %edx, %r14d
cmpl %esi, %eax
cmovgel %r15d, %ebx
subl %esi, %ebx
movl %ecx, 0x2ab1a4(%rip)    # 0x35bea4
movl %r14d, %edi
callq 0xb06eb
addl %r14d, %ebx
addl 0x2af923(%rip), %ebx    # 0x360634
movl %ebx, %edx
negl %edx
cmovsl %ebx, %edx
movl 0x2af91a(%rip), %eax    # 0x360638
incl %eax
cmpl $0x2, %edx
cmovbl %r15d, %eax
movl %ebx, 0x2af907(%rip)    # 0x360634
movl %eax, 0x2af905(%rip)    # 0x360638
seta %sil
movl (%r12), %ecx
cmpl $0x960, %ecx            # imm = 0x960
setge %dil
testb %dil, %sil
je 0xb0d52
pushq $0x1
popq %r14
movb $0x1, %bl
jmp 0xb0d70
xorl %r15d, %r15d
movl $0x0, %ebx
movl $0x0, %r14d
cmpl $0x9, %edx
jb 0xb0d70
movb $0x1, %r15b
movb $0x1, %bl
pushq $0x2
popq %rdx
movl %edx, %r14d
movl %eax, %edx
negl %edx
cmovsl %eax, %edx
cmpl $0x5, %edx
jb 0xb1008
leaq 0x15407d(%rip), %rax    # 0x204e04
cmpl (%rax), %ecx
setl %al
notb %r15b
testb %al, %r15b
jne 0xb1008
callq 0xb0447
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
setnp %al
sete %cl
andb %al, %cl
notb %bl
testb %cl, %bl
jne 0xb1008
movl (%r12), %eax
cmpl $0x4af, %eax            # imm = 0x4AF
jg 0xb0df1
leaq 0x15403b(%rip), %rcx    # 0x204e04
movl (%rcx), %ecx
movl %eax, %edx
subl %ecx, %edx
cvtsi2sd %edx, %xmm0
cvtsi2sd %ecx, %xmm1
addsd %xmm1, %xmm1
divsd %xmm1, %xmm0
addsd 0x42289(%rip), %xmm0    # 0xf3070
mulsd 0x2af851(%rip), %xmm0   # 0x360640
jmp 0xb0dfc
leaq 0x154138(%rip), %rcx    # 0x204f30
movsd (%rcx), %xmm0
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0xb0e14
jnp 0xb1008
divsd (%r13), %xmm0
movsd 0x2af826(%rip), %xmm1   # 0x360648
movsd %xmm1, 0x2af826(%rip)   # 0x360650
leaq 0x1540af(%rip), %r15    # 0x204ee0
addsd (%r15), %xmm0
movsd %xmm0, 0x2af80a(%rip)   # 0x360648
callq 0xb0591
movsd %xmm0, 0x2af7fd(%rip)   # 0x360648
incl 0x2b2efb(%rip)          # 0x363d4c
movapd %xmm0, %xmm1
subsd (%r15), %xmm1
leaq 0x151a63(%rip), %rax    # 0x2028c4
cmpb $0x0, (%rax)
movsd %xmm1, -0x50(%rbp)
je 0xb0f25
movl $0x204, %edi            # imm = 0x204
callq 0x9d23
movl 0x2b2ecd(%rip), %ebx    # 0x363d4c
movsd 0x2af7c1(%rip), %xmm0   # 0x360648
movsd %xmm0, -0x40(%rbp)
movsd (%r15), %xmm0
movsd %xmm0, -0x38(%rbp)
movl (%r12), %eax
movl %eax, -0x30(%rbp)
leaq 0x154098(%rip), %r13    # 0x204f3c
movq %r13, %r15
movl (%r13), %r13d
movl 0x2aaff3(%rip), %eax    # 0x35bea4
movl %eax, -0x2c(%rbp)
movl 0x2af77a(%rip), %eax    # 0x360634
movq %rax, -0x48(%rbp)
callq 0xb0447
movapd %xmm0, %xmm3
movsd 0x2af771(%rip), %xmm4   # 0x360640
leaq 0x27797a(%rip), %rdi    # 0x328850
leaq 0x63e2b(%rip), %rsi     # 0x114d08
movl %ebx, %edx
movsd -0x40(%rbp), %xmm0
movsd -0x38(%rbp), %xmm1
movsd -0x50(%rbp), %xmm2
movl -0x30(%rbp), %ecx
movl %r13d, %r8d
movl -0x2c(%rbp), %r9d
movb $0x5, %al
pushq %r14
pushq -0x48(%rbp)
callq 0x9060
popq %rcx
popq %rdx
leaq 0x15441b(%rip), %rax    # 0x205328
movq (%rax), %rax
leaq 0x154399(%rip), %rcx    # 0x2052b0
movq %rax, 0x48(%rcx)
movsd 0x2af725(%rip), %xmm0   # 0x360648
jmp 0xb0f2c
leaq 0x154010(%rip), %r15    # 0x204f3c
leaq 0x17745d(%rip), %rax    # 0x228390
movq (%rax), %rbx
testq %rbx, %rbx
je 0xb0fc5
movl 0x2b2e06(%rip), %r13d   # 0x363d4c
leaq 0x153f93(%rip), %rax    # 0x204ee0
movsd (%rax), %xmm1
movsd %xmm1, -0x40(%rbp)
movl (%r12), %eax
movl %eax, -0x30(%rbp)
movl (%r15), %r12d
movl 0x2aaf3e(%rip), %eax    # 0x35bea4
movl %eax, -0x2c(%rbp)
movl 0x2af6c5(%rip), %eax    # 0x360634
movq %rax, -0x48(%rbp)
movsd %xmm0, -0x38(%rbp)
callq 0xb0447
movapd %xmm0, %xmm3
movsd 0x2af6b7(%rip), %xmm4   # 0x360640
leaq 0x63ddf(%rip), %rsi     # 0x114d6f
movq %rbx, %rdi
movl %r13d, %edx
movsd -0x38(%rbp), %xmm0
movsd -0x40(%rbp), %xmm1
movsd -0x50(%rbp), %xmm2
movl -0x30(%rbp), %ecx
movl %r12d, %r8d
movl -0x2c(%rbp), %r9d
movb $0x5, %al
pushq %r14
pushq -0x48(%rbp)
callq 0x9560
popq %rcx
popq %rdx
movsd 0x2af683(%rip), %xmm0   # 0x360648
callq 0x6779b
leaq 0x153cff(%rip), %rax    # 0x204cd0
andl $0x0, (%rax)
movl (%r15), %eax
movl %eax, 0x2aaec7(%rip)    # 0x35bea4
pushq $0x3
popq %rdi
callq 0xb0658
movl 0x2a5bfd(%rip), %edi    # 0x356be8
callq 0x64928
andl $0x0, 0x2af635(%rip)    # 0x36062c
andl $0x0, 0x2af62a(%rip)    # 0x360628
movl $0x5, 0x2a5bdc(%rip)    # 0x356be4
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop