// File: /workspace/github_cpp_makefile_star2_O2/myudelson[P]hmm-scalable/utils.cpp
// Function: projectsimplex(double*, signed char)
void projectsimplex(NUMBER* ar, NPAR size) {
    NPAR i, num_at_hi, num_at_lo; // number of elements at lower,upper boundary
    NPAR *at_hi = Calloc(NPAR, (size_t)size);
    NPAR *at_lo = Calloc(NPAR, (size_t)size);
    NUMBER err, lambda;
    NUMBER* ar_copy = Calloc(NUMBER, (size_t)size);
    memcpy(ar_copy, ar, sizeof(NUMBER)*(size_t)size);
    int iter = 0;
    while( !issimplex(ar, size) ) {
        lambda = 0;
        num_at_hi = 0;
        num_at_lo = 0;
        err = -1;
        // threshold
        for(i=0; i<size; i++) {
            at_lo[i] = (ar[i]<=/*lb[i]*/0)?1:0;
            ar[i] = (at_lo[i]==1)?/*lb[i]*/0:ar[i];
            num_at_lo = (NPAR)( num_at_lo + at_lo[i] );
            
            at_hi[i] = (ar[i]>=/*ub[i]*/1)?1:0;
            ar[i] = (at_hi[i]==1)?/*ub[i]*/1:ar[i];
            num_at_hi = (NPAR)( num_at_hi + at_hi[i] );
            
            err += ar[i];
        }
        //		if (size > (num_at_hi + num_at_lo) )
        //			lambda = err / (size - (num_at_hi + num_at_lo));
        if ( err > 0 && size > num_at_lo )
            lambda = err / (size - num_at_lo);
        else if ( err < 0 && size > num_at_hi )
            lambda = err / (size - num_at_hi);
        
        int will_suffer_from_lambda = 0; // those values that, if lessened by lambda, will be below 0 or over 1
        NUMBER err2 = 0.0, lambda2 = 0.0;
        for(i=0; i<size; i++) {
            if( (at_lo[i]==0 && at_hi[i]==0) ) {
                if( ar[i] < lambda ) {
                    will_suffer_from_lambda++;
                    err2 += ar[i];
                }
            }
        }
        
        if( will_suffer_from_lambda == 0 ) {
            for(i=0; i<size; i++) {
                ar[i] -= (at_lo[i]==0 && err>0)?lambda:0;
                ar[i] -= (at_hi[i]==0 && err<0)?lambda:0;
            }
        } else {
            lambda2 = (err-err2) / ( size - (num_at_hi + num_at_lo) - will_suffer_from_lambda );
            for(i=0; i<size; i++) {
                if( at_lo[i]==0 && at_hi[i]==0 ) {
                    ar[i] = (ar[i]<lambda)?0:(ar[i]-lambda2);
                }
            }
        }
        iter++;
        if(iter==100) {
            string ar_was = "";
            for(int i=0; i<size; i++) ar_was = ar_was + std::to_string(ar_copy[i]).substr(0,5) + std::to_string((i<(size-1))?',':' ');
            string ar_is = "";
            for(int i=0; i<size; i++) ar_is = ar_is + std::to_string(ar[i]).substr(0,5) + std::to_string((i<(size-1))?',':' ');
            fprintf(stderr,"WARNING! Stuck in projectsimplex()! ar was %s and now is %s.\n",ar_was.c_str(),ar_is.c_str());
            break;//exit(1);
        }
    } // until satisfied
    // force last to be 1 - sum of all but last -- this code, actually breaks things
    //    err = 0;
    //    for(i=0; i<(size); i++) {
    //        err += ar[i];
    //    }
    //    err = 1;
    //    for(i=0; i<(size-1); i++) {
    //        err -= ar[i];
    //    }
    //    ar[size-1] = err;
    //    err = 1;
    //    for(i=1; i<(size-0); i++) {
    //        err -= ar[i];
    //    }
    //    ar[0] = err;
    
    NUMBER sum = 0.0;
    for(i=0; i<size; i++) {
        sum += ar[i];
        if(ar[i]<0 || ar[i] >1) {
            string ar_was = "";
            for(int i=0; i<size; i++) ar_was = ar_was + std::to_string(ar_copy[i]).substr(0,5) + std::to_string((i<(size-1))?',':' ');
            string ar_is = "";
            for(int i=0; i<size; i++) ar_is = ar_is + std::to_string(ar[i]).substr(0,5) + std::to_string((i<(size-1))?',':' ');
            fprintf(stderr, "ERROR! projected value is not within [0, 1] range! ar was %s and now is %s.\n",ar_was.c_str(),ar_is.c_str());
        }
    }
//    if( fabs(sum-1)>SAFETY)
//        fprintf(stderr, "ERROR! projected simplex does not sum to 1!\n");
	
    free(at_hi);
    free(at_lo);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x128, %rsp            # imm = 0x128
movl %esi, %ebp
movq %rdi, %r15
movslq %esi, %rbx
pushq $0x1
popq %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x4120
movq %rax, %r12
movq %rbx, %rdi
movq %r14, %rsi
callq 0x4120
movq %rax, %r13
pushq $0x8
popq %rsi
movq %rbx, %rdi
callq 0x4120
leaq (,%rbx,8), %rdx
movq %rax, 0x20(%rsp)
movq %rax, %rdi
movq %r15, %rsi
callq 0x41c0
xorl %r14d, %r14d
testb %bpl, %bpl
cmovlel %r14d, %ebx
movzbl %bl, %eax
movq %r15, %rbx
movq %rax, %r15
movq %rbp, (%rsp)
movsbl %bpl, %ebp
movq %rbx, 0x18(%rsp)
movq %r12, 0x70(%rsp)
movq %r13, 0x68(%rsp)
movq %rax, 0x78(%rsp)
movq %rbx, %rdi
movl %ebp, %esi
callq 0x53c2
testb %al, %al
jne 0x5923
xorl %eax, %eax
xorl %esi, %esi
xorl %edx, %edx
movsd 0x2723d(%rip), %xmm0    # 0x2c8e8
cmpq %rax, %r15
je 0x56f7
movsd (%rbx,%rax,8), %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm2, %xmm1
setae %cl
setae (%r13,%rax)
jae 0x56cc
movapd %xmm2, %xmm1
addb %cl, %sil
movsd 0x27219(%rip), %xmm2    # 0x2c8f0
ucomisd %xmm2, %xmm1
setae (%r12,%rax)
setae %cl
minsd %xmm1, %xmm2
movsd %xmm2, (%rbx,%rax,8)
addb %cl, %dl
addsd %xmm2, %xmm0
incq %rax
jmp 0x56ab
ucomisd 0x27201(%rip), %xmm0    # 0x2c900
movsbl %dl, %ecx
movsbl %sil, %eax
movq (%rsp), %rdi
jbe 0x5717
cmpb %dil, %sil
jge 0x5717
movl %edi, %edx
subl %eax, %edx
jmp 0x5737
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
seta %sil
cmpb %dil, %dl
setl %dl
andb %sil, %dl
cmpb $0x1, %dl
jne 0x5743
movq (%rsp), %rdx
subl %ecx, %edx
cvtsi2sd %edx, %xmm2
movapd %xmm0, %xmm1
divsd %xmm2, %xmm1
xorpd %xmm2, %xmm2
xorl %esi, %esi
xorl %edx, %edx
cmpq %rsi, %r15
je 0x5775
cmpb $0x0, (%r13,%rsi)
jne 0x5770
cmpb $0x0, (%r12,%rsi)
jne 0x5770
movsd (%rbx,%rsi,8), %xmm3
ucomisd %xmm3, %xmm1
jbe 0x5770
incl %edx
addsd %xmm3, %xmm2
incq %rsi
jmp 0x574b
testl %edx, %edx
je 0x57c9
subsd %xmm2, %xmm0
addl %ecx, %eax
addl %edx, %eax
movq (%rsp), %rcx
subl %eax, %ecx
cvtsi2sd %ecx, %xmm2
divsd %xmm2, %xmm0
xorl %eax, %eax
cmpq %rax, %r15
je 0x581e
cmpb $0x0, (%r13,%rax)
jne 0x57c4
cmpb $0x0, (%r12,%rax)
jne 0x57c4
movsd (%rbx,%rax,8), %xmm2
movapd %xmm2, %xmm3
subsd %xmm0, %xmm3
cmpltsd %xmm1, %xmm2
andnpd %xmm3, %xmm2
movlpd %xmm2, (%rbx,%rax,8)
incq %rax
jmp 0x5791
xorl %eax, %eax
cmpq %rax, %r15
je 0x581e
cmpb $0x0, (%r13,%rax)
xorpd %xmm2, %xmm2
movapd %xmm1, %xmm3
je 0x57e4
xorpd %xmm3, %xmm3
cmpb $0x0, (%r12,%rax)
je 0x57f1
xorpd %xmm4, %xmm4
jmp 0x57fe
movapd %xmm0, %xmm4
cmpltsd %xmm2, %xmm4
andpd %xmm1, %xmm4
cmpltsd %xmm0, %xmm2
andpd %xmm3, %xmm2
movsd (%rbx,%rax,8), %xmm3
subsd %xmm2, %xmm3
subsd %xmm4, %xmm3
movsd %xmm3, (%rbx,%rax,8)
incq %rax
jmp 0x57cb
incl %r14d
cmpl $0x64, %r14d
jne 0x568b
leaq 0x28415(%rip), %rsi     # 0x2dc47
leaq 0x28(%rsp), %rdi
leaq 0x48(%rsp), %rdx
callq 0x7dea
movq (%rsp), %rcx
leal -0x1(%rcx), %eax
xorl %ebp, %ebp
testl %ecx, %ecx
movl $0x0, %edx
cmovgl %ecx, %edx
movq %rdx, 0x8(%rsp)
cltq
movq %rax, 0x10(%rsp)
leaq 0xa8(%rsp), %rbx
leaq 0xc8(%rsp), %r14
leaq 0xe8(%rsp), %r12
leaq 0x88(%rsp), %r15
leaq 0x48(%rsp), %r13
cmpq %rbp, 0x8(%rsp)
je 0x5b7c
movq 0x20(%rsp), %rax
movsd (%rax,%rbp,8), %xmm0
movq %rbx, %rdi
callq 0x7eb0
movq %r14, %rdi
movq %rbx, %rsi
xorl %edx, %edx
pushq $0x5
popq %rcx
callq 0x4240
movq %r12, %rdi
leaq 0x28(%rsp), %rsi
movq %r14, %rdx
callq 0x7e8c
cmpq 0x10(%rsp), %rbp
pushq $0x20
popq %rsi
pushq $0x2c
popq %rax
cmovll %eax, %esi
movq %r15, %rdi
callq 0x7ed3
movq %r13, %rdi
movq %r12, %rsi
movq %r15, %rdx
callq 0x7e25
leaq 0x28(%rsp), %rdi
movq %r13, %rsi
callq 0x42b0
movq %r13, %rdi
callq 0x40f0
movq %r15, %rdi
callq 0x40f0
movq %r12, %rdi
callq 0x40f0
movq %r14, %rdi
callq 0x40f0
movq %rbx, %rdi
callq 0x40f0
incq %rbp
jmp 0x5885
movq (%rsp), %rcx
leal -0x1(%rcx), %eax
cltq
movq %rax, 0x10(%rsp)
movl %ecx, %eax
movq %rax, 0x8(%rsp)
leaq 0x48(%rsp), %r13
leaq 0xe8(%rsp), %r14
leaq 0x88(%rsp), %r12
leaq 0xa8(%rsp), %r15
leaq 0xc8(%rsp), %rbp
xorl %eax, %eax
cmpq 0x78(%rsp), %rax
je 0x5b56
movq %rax, 0x80(%rsp)
movsd (%rbx,%rax,8), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
ja 0x5998
ucomisd 0x26f67(%rip), %xmm0    # 0x2c8f0
ja 0x5998
movq 0x80(%rsp), %rax
incq %rax
jmp 0x595f
leaq 0x28(%rsp), %rdi
leaq 0x282a3(%rip), %rsi     # 0x2dc47
movq %r13, %rdx
callq 0x7dea
xorl %ebx, %ebx
cmpq %rbx, 0x8(%rsp)
je 0x5a4c
movq 0x20(%rsp), %rax
movsd (%rax,%rbx,8), %xmm0
movq %r15, %rdi
callq 0x7eb0
movq %rbp, %rdi
movq %r15, %rsi
xorl %edx, %edx
pushq $0x5
popq %rcx
callq 0x4240
movq %r14, %rdi
leaq 0x28(%rsp), %rsi
movq %rbp, %rdx
callq 0x7e8c
cmpq 0x10(%rsp), %rbx
pushq $0x20
popq %rsi
pushq $0x2c
popq %rax
cmovll %eax, %esi
movq %r12, %rdi
callq 0x7ed3
movq %r13, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x7e25
leaq 0x28(%rsp), %rdi
movq %r13, %rsi
callq 0x42b0
movq %r13, %rdi
callq 0x40f0
movq %r12, %rdi
callq 0x40f0
movq %r14, %rdi
callq 0x40f0
movq %rbp, %rdi
callq 0x40f0
movq %r15, %rdi
callq 0x40f0
incq %rbx
jmp 0x59ae
movq %r13, %rdi
leaq 0x281f1(%rip), %rsi     # 0x2dc47
movq %r14, %rdx
callq 0x7dea
xorl %ebx, %ebx
cmpq %rbx, 0x8(%rsp)
je 0x5b18
movq 0x18(%rsp), %rax
movsd (%rax,%rbx,8), %xmm0
movq %r12, %rdi
callq 0x7eb0
movq %r15, %rdi
movq %r12, %rsi
xorl %edx, %edx
pushq $0x5
popq %rcx
callq 0x4240
movq %rbp, %rdi
movq %r13, %rsi
movq %r15, %rdx
callq 0x7e8c
movq %rbx, %rax
movq %r13, %rbx
movq %rax, (%rsp)
cmpq 0x10(%rsp), %rax
pushq $0x20
popq %rsi
pushq $0x2c
popq %rax
cmovll %eax, %esi
leaq 0x108(%rsp), %r13
movq %r13, %rdi
callq 0x7ed3
movq %r14, %rdi
movq %rbp, %rsi
movq %r13, %rdx
callq 0x7e25
movq %rbx, %rdi
movq %r14, %rsi
callq 0x42b0
movq %r14, %rdi
callq 0x40f0
leaq 0x108(%rsp), %rdi
movq %rbx, %r13
callq 0x40f0
movq %rbp, %rdi
callq 0x40f0
movq %r15, %rdi
callq 0x40f0
movq %r12, %rdi
callq 0x40f0
movq (%rsp), %rbx
incq %rbx
jmp 0x5a60
movq 0x334b9(%rip), %rax     # 0x38fd8
movq (%rax), %rdi
movq 0x28(%rsp), %rdx
movq 0x48(%rsp), %rcx
leaq 0x26eeb(%rip), %rsi     # 0x2ca1e
xorl %eax, %eax
callq 0x4350
movq %r13, %rdi
callq 0x40f0
leaq 0x28(%rsp), %rdi
callq 0x40f0
movq 0x18(%rsp), %rbx
jmp 0x598b
movq 0x70(%rsp), %rdi
callq 0x4290
movq 0x68(%rsp), %rdi
callq 0x4290
addq $0x128, %rsp            # imm = 0x128
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x280c4(%rip), %rsi     # 0x2dc47
leaq 0x48(%rsp), %rdi
leaq 0xe8(%rsp), %rdx
callq 0x7dea
xorl %ebp, %ebp
leaq 0x88(%rsp), %rbx
leaq 0xa8(%rsp), %r14
leaq 0xc8(%rsp), %r12
leaq 0x108(%rsp), %r15
leaq 0xe8(%rsp), %r13
cmpq %rbp, 0x8(%rsp)
je 0x5c5d
movq 0x18(%rsp), %rax
movsd (%rax,%rbp,8), %xmm0
movq %rbx, %rdi
callq 0x7eb0
movq %r14, %rdi
movq %rbx, %rsi
xorl %edx, %edx
pushq $0x5
popq %rcx
callq 0x4240
movq %r12, %rdi
leaq 0x48(%rsp), %rsi
movq %r14, %rdx
callq 0x7e8c
cmpq 0x10(%rsp), %rbp
pushq $0x20
popq %rsi
pushq $0x2c
popq %rax
cmovll %eax, %esi
movq %r15, %rdi
callq 0x7ed3
movq %r13, %rdi
movq %r12, %rsi
movq %r15, %rdx
callq 0x7e25
leaq 0x48(%rsp), %rdi
movq %r13, %rsi
callq 0x42b0
movq %r13, %rdi
callq 0x40f0
movq %r15, %rdi
callq 0x40f0
movq %r12, %rdi
callq 0x40f0
movq %r14, %rdi
callq 0x40f0
movq %rbx, %rdi
callq 0x40f0
incq %rbp
jmp 0x5bbf
movq 0x33374(%rip), %rax     # 0x38fd8
movq (%rax), %rdi
leaq 0x28(%rsp), %rbx
movq (%rbx), %rdx
leaq 0x48(%rsp), %r14
movq (%r14), %rcx
leaq 0x26d62(%rip), %rsi     # 0x2c9e0
xorl %eax, %eax
callq 0x4350
movq %r14, %rdi
callq 0x40f0
movq %rbx, %rdi
callq 0x40f0
movq 0x18(%rsp), %rbx
movq (%rsp), %rcx
jmp 0x5931
jmp 0x5d59
jmp 0x5caf
jmp 0x5d59
movq %rax, %rbx
jmp 0x5d78
jmp 0x5cd1
jmp 0x5ce3
jmp 0x5ce8
jmp 0x5d07
jmp 0x5d19
jmp 0x5d23
jmp 0x5d35
jmp 0x5d3a
jmp 0x5d5e
jmp 0x5d59
movq %rax, %rbx
leaq 0x108(%rsp), %rdi
callq 0x40f0
jmp 0x5ceb
movq %rax, %rbx
jmp 0x5cf8
movq %rax, %rbx
leaq 0xc8(%rsp), %rdi
callq 0x40f0
leaq 0xa8(%rsp), %rdi
callq 0x40f0
jmp 0x5d0a
movq %rax, %rbx
leaq 0x88(%rsp), %rdi
callq 0x40f0
jmp 0x5d1c
movq %rax, %rbx
leaq 0x48(%rsp), %rdi
jmp 0x5d69
movq %rax, %rbx
leaq 0x88(%rsp), %rdi
callq 0x40f0
jmp 0x5d3d
movq %rax, %rbx
jmp 0x5d4a
movq %rax, %rbx
leaq 0xe8(%rsp), %rdi
callq 0x40f0
leaq 0xc8(%rsp), %rdi
callq 0x40f0
jmp 0x5d61
movq %rax, %rbx
jmp 0x5d6e
movq %rax, %rbx
leaq 0xa8(%rsp), %rdi
callq 0x40f0
leaq 0x28(%rsp), %rdi
callq 0x40f0
movq %rbx, %rdi
callq 0x4450