// File: /workspace/github_cpp_makefile_star2_O2/myudelson[P]hmm-scalable/HMMProblem.cpp
// Function: HMMProblem::doLagrangeStep(FitBit*)
NUMBER HMMProblem::doLagrangeStep(FitBit *fb) {
	NPAR nS = this->p->nS, nO = this->p->nO;
	NPAR i,j,m;
    
    NUMBER  ll = 0;
	NUMBER * b_PI = init1D<NUMBER>((NDAT)this->p->nS);
    NUMBER   b_PI_den = 0;
	NUMBER ** b_A     = init2D<NUMBER>((NDAT)this->p->nS, (NDAT)this->p->nS);
	NUMBER *  b_A_den = init1D<NUMBER>((NDAT)this->p->nS);
	NUMBER ** b_B     = init2D<NUMBER>((NDAT)this->p->nS, (NDAT)this->p->nO);
	NUMBER *  b_B_den = init1D<NUMBER>((NDAT)this->p->nS);
	// compute sums PI
    
    NUMBER buf = 0;
    // collapse
    for(i=0; i<nS; i++) {
        if(fb->pi != NULL)
            buf = -fb->pi[i]*fb->gradPI[i]; /*negatie since were going against gradient*/
        b_PI_den += buf;
        b_PI[i] += buf;
        if(fb->A != NULL)
            for(j=0; j<nS; j++){
                buf = -fb->A[i][j]*fb->gradA[i][j]; /*negatie since were going against gradient*/
                b_A_den[i] += buf;
                b_A[i][j] += buf;
            }
        if(fb->B != NULL)
            for(m=0; m<nO; m++) {
                buf = -fb->B[i][m]*fb->gradB[i][m]; /*negatie since were going against gradient*/
                b_B_den[i] += buf;
                b_B[i][m] += buf;
            }
    }
    // divide
    for(i=0; i<nS; i++) {
        if(fb->pi != NULL)
            fb->pi[i] = (b_PI_den>0) ? (b_PI[i] / b_PI_den) : fb->pi[i];
        if(fb->A != NULL)
            for(j=0; j<nS; j++)
                fb->A[i][j] = (b_A_den[i]>0) ? (b_A[i][j] / b_A_den[i]) : fb->A[i][j];
        if(fb->B != NULL)
            for(m=0; m<nO; m++)
                fb->B[i][m] = (b_B_den[i]>0) ? (b_B[i][m] / b_B_den[i]) : fb->B[i][m];
    }
    // scale
    if( !this->hasNon01Constraints() || fb->observe_non01_boundaries==0 ) {
        if(fb->pi != NULL) projectsimplex(fb->pi, nS);
        for(i=0; i<nS; i++) {
            if(fb->A != NULL) projectsimplex(fb->A[i], nS);
            if(fb->B != NULL) projectsimplex(fb->B[i], nO);
        }
    } else {
        if(fb->pi != NULL) projectsimplexbounded(fb->pi, this->getLbPI(), this->getUbPI(), nS);
        for(i=0; i<nS; i++) {
            if(fb->A != NULL) projectsimplexbounded(fb->A[i], this->getLbA()[i], this->getUbA()[i], nS);
            if(fb->B != NULL) projectsimplexbounded(fb->B[i], this->getLbB()[i], this->getUbB()[i], nO);
        }
    }
    // compute LL
    computeAlphaAndPOParam(fb->xndat, fb->x_data);
    ll = HMMProblem::getSumLogPOPara(fb->xndat, fb->x_data);

	free(b_PI);
	free2D<NUMBER>(b_A, nS);
	free(b_A_den);
	free2D<NUMBER>(b_B, nS);
	free(b_B_den);
    
    return ll;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
movq %rsi, %r14
movq %rdi, %r12
movq 0x88(%rdi), %rax
movsbl 0x8b8(%rax), %r15d
movzbl 0x8b9(%rax), %eax
movl %eax, 0x8(%rsp)
movl %r15d, %edi
callq 0x7dde
movq %rax, 0x20(%rsp)
movq 0x88(%r12), %rax
movsbl 0x8b8(%rax), %edi
movl %edi, %esi
callq 0x9328
movq %rax, %r13
movq 0x88(%r12), %rax
movsbl 0x8b8(%rax), %edi
callq 0x7dde
movq %rax, %rbp
movq 0x88(%r12), %rax
movsbl 0x8b8(%rax), %edi
movsbl 0x8b9(%rax), %esi
callq 0x9328
movq %rax, %rbx
movq %r12, 0x18(%rsp)
movq 0x88(%r12), %rax
movsbl 0x8b8(%rax), %edi
callq 0x7dde
movq 0x20(%rsp), %r8
movq %rax, %r12
xorl %ecx, %ecx
testb %r15b, %r15b
movl $0x0, %eax
movl %r15d, 0x10(%rsp)
cmovgl %r15d, %eax
movl 0x8(%rsp), %edx
testb %dl, %dl
cmovlel %ecx, %edx
movslq %eax, %r15
movzbl %dl, %eax
xorpd %xmm0, %xmm0
movapd 0x17946(%rip), %xmm1    # 0x2c940
xorpd %xmm2, %xmm2
cmpq %r15, %rcx
je 0x150ed
movq 0x10(%r14), %rdx
testq %rdx, %rdx
je 0x15022
movsd (%rdx,%rcx,8), %xmm2
xorpd %xmm1, %xmm2
movq 0x58(%r14), %rdx
mulsd (%rdx,%rcx,8), %xmm2
movsd (%r8,%rcx,8), %xmm3
addsd %xmm2, %xmm3
movsd %xmm3, (%r8,%rcx,8)
movq 0x18(%r14), %rdx
movapd %xmm2, %xmm3
testq %rdx, %rdx
je 0x1508c
xorl %esi, %esi
movapd %xmm2, %xmm3
cmpq %rsi, %r15
je 0x1508c
movq (%rdx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm3
xorpd %xmm1, %xmm3
movq 0x60(%r14), %rdi
movq (%rdi,%rcx,8), %rdi
mulsd (%rdi,%rsi,8), %xmm3
movsd (%rbp,%rcx,8), %xmm4
addsd %xmm3, %xmm4
movsd %xmm4, (%rbp,%rcx,8)
movq (%r13,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm4
addsd %xmm3, %xmm4
movsd %xmm4, (%rdi,%rsi,8)
incq %rsi
jmp 0x15045
movq 0x20(%r14), %rdx
testq %rdx, %rdx
je 0x150dd
xorl %esi, %esi
cmpq %rsi, %rax
je 0x150dd
movq (%rdx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm3
xorpd %xmm1, %xmm3
movq 0x68(%r14), %rdi
movq (%rdi,%rcx,8), %rdi
mulsd (%rdi,%rsi,8), %xmm3
movsd (%r12,%rcx,8), %xmm4
addsd %xmm3, %xmm4
movsd %xmm4, (%r12,%rcx,8)
movq (%rbx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm4
addsd %xmm3, %xmm4
movsd %xmm4, (%rdi,%rsi,8)
incq %rsi
jmp 0x15097
addsd %xmm2, %xmm0
incq %rcx
movapd %xmm3, %xmm2
jmp 0x14ffe
xorl %ecx, %ecx
xorpd %xmm1, %xmm1
cmpq %r15, %rcx
je 0x151ae
movq 0x10(%r14), %rdx
testq %rdx, %rdx
je 0x15121
ucomisd %xmm1, %xmm0
jbe 0x15117
movsd (%r8,%rcx,8), %xmm2
divsd %xmm0, %xmm2
jmp 0x1511c
movsd (%rdx,%rcx,8), %xmm2
movsd %xmm2, (%rdx,%rcx,8)
movq 0x18(%r14), %rdx
testq %rdx, %rdx
je 0x15164
xorl %esi, %esi
cmpq %rsi, %r15
je 0x15164
movsd (%rbp,%rcx,8), %xmm3
ucomisd %xmm1, %xmm3
jbe 0x15151
movq (%r13,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm2
divsd %xmm3, %xmm2
movq (%rdx,%rcx,8), %rdi
jmp 0x1515a
movq (%rdx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm2
movsd %xmm2, (%rdi,%rsi,8)
incq %rsi
jmp 0x1512c
movq 0x20(%r14), %rdx
testq %rdx, %rdx
je 0x151a6
xorl %esi, %esi
cmpq %rsi, %rax
je 0x151a6
movsd (%r12,%rcx,8), %xmm3
ucomisd %xmm1, %xmm3
jbe 0x15193
movq (%rbx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm2
divsd %xmm3, %xmm2
movq (%rdx,%rcx,8), %rdi
jmp 0x1519c
movq (%rdx,%rcx,8), %rdi
movsd (%rdi,%rsi,8), %xmm2
movsd %xmm2, (%rdi,%rsi,8)
incq %rsi
jmp 0x1516f
incq %rcx
jmp 0x150f3
movq 0x18(%rsp), %rax
cmpb $0x0, 0x80(%rax)
je 0x15297
cmpb $0x0, 0xf9(%r14)
movq 0x10(%r14), %rdi
je 0x1529b
testq %rdi, %rdi
je 0x151ed
movq 0x18(%rsp), %rax
movq 0x50(%rax), %rsi
movq 0x68(%rax), %rdx
movl 0x10(%rsp), %ecx
callq 0x5d80
movsbl 0x8(%rsp), %eax
movl %eax, 0x14(%rsp)
xorl %r8d, %r8d
cmpq %r8, %r15
je 0x152f9
movq 0x18(%r14), %rax
testq %rax, %rax
movq %r8, 0x8(%rsp)
je 0x1524b
movq (%rax,%r8,8), %rdi
movq 0x18(%rsp), %rcx
cmpb $0x0, 0x80(%rcx)
movq 0x58(%rcx), %rax
movl $0x0, %edx
cmoveq %rdx, %rax
movq (%rax,%r8,8), %rsi
movq 0x70(%rcx), %rax
cmoveq %rdx, %rax
movq (%rax,%r8,8), %rdx
movl 0x10(%rsp), %ecx
callq 0x5d80
movq 0x8(%rsp), %r8
movq 0x20(%r14), %rax
testq %rax, %rax
je 0x1528f
movq (%rax,%r8,8), %rdi
movq 0x18(%rsp), %rcx
cmpb $0x0, 0x80(%rcx)
movq 0x60(%rcx), %rax
movl $0x0, %edx
cmoveq %rdx, %rax
movq (%rax,%r8,8), %rsi
movq 0x78(%rcx), %rax
cmoveq %rdx, %rax
movq (%rax,%r8,8), %rdx
movl 0x14(%rsp), %ecx
callq 0x5d80
movq 0x8(%rsp), %r8
incq %r8
jmp 0x151f9
movq 0x10(%r14), %rdi
testq %rdi, %rdi
je 0x152a9
movl 0x10(%rsp), %esi
callq 0x5600
xorl %ecx, %ecx
movsbl 0x8(%rsp), %eax
movl %eax, 0x14(%rsp)
cmpq %rcx, %r15
je 0x152f9
movq 0x18(%r14), %rax
testq %rax, %rax
movq %rcx, 0x8(%rsp)
je 0x152d9
movq (%rax,%rcx,8), %rdi
movl 0x10(%rsp), %esi
callq 0x5600
movq 0x8(%rsp), %rcx
movq 0x20(%r14), %rax
testq %rax, %rax
je 0x152f4
movq (%rax,%rcx,8), %rdi
movl 0x14(%rsp), %esi
callq 0x5600
movq 0x8(%rsp), %rcx
incq %rcx
jmp 0x152b4
movl 0xe8(%r14), %esi
movq 0xf0(%r14), %rdx
movq 0x18(%rsp), %rdi
callq 0xe7b0
movl 0xe8(%r14), %esi
movq 0xf0(%r14), %rdx
callq 0xe400
movsd %xmm0, 0x8(%rsp)
movq 0x20(%rsp), %rdi
callq 0x4290
movq %r13, %rdi
movl 0x10(%rsp), %r14d
movl %r14d, %esi
callq 0x7f2c
movq %rbp, %rdi
callq 0x4290
movq %rbx, %rdi
movl %r14d, %esi
callq 0x7f2c
movq %r12, %rdi
callq 0x4290
movsd 0x8(%rsp), %xmm0
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq