user: Please show me the C implementation that matches the assembly code listed below:

```asm
<s_init_ux_pi_t_box_mpc>:
  endbr64
  push   %r15
  push   %r14
  mov    %r9,%r14
  push   %r13
  push   %r12
  push   %rbp
  push   %rbx
  lea    (%rcx,%rcx,1),%ebx
  sub    $0x28,%rsp
  cmpl   $0x1,0x78(%rsp)
  mov    %edi,0xc(%rsp)
  mov    %esi,0x18(%rsp)
  mov    0x70(%rsp),%rbp
  mov    %edx,0x8(%rsp)
  mov    %r8d,0x14(%rsp)
  je     L441
  xor    %eax,%eax
  xor    %r12d,%r12d
  test   %ebx,%ebx
  jle    L120
  mov    0x68(%rsp),%rax
  mov    (%r9),%rdi
  xor    %ecx,%ecx
  pxor   %xmm4,%xmm4
  mov    0x0(%rbp),%rdx
  movss  D116(%rip),%xmm0
  movss  D112(%rip),%xmm2
  mov    (%rax),%rax
  movss  D78(%rip),%xmm5
  jmp    L422
  nopl   (%rax)
L287:
  comiss %xmm1,%xmm0
  jbe    L639
  movss  (%rax),%xmm1
  subss  0x4(%rax),%xmm1
  mulss  %xmm5,%xmm1
  movss  %xmm1,(%rsi)
  subss  (%rax),%xmm1
  movss  %xmm1,(%rdx)
  movss  0x4(%rax),%xmm1
  xorps  %xmm2,%xmm1
  subss  (%rsi),%xmm1
  movss  %xmm1,0x4(%rdx)
L679:
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  cmp    %ebx,%ecx
  jge    L577
L422:
  mov    %ecx,%esi
  movaps %xmm4,%xmm3
  sar    %esi
  movslq %esi,%rsi
  lea    (%rdi,%rsi,4),%rsi
  movl   $0x0,(%rsi)
  subss  (%rax),%xmm3
  movss  %xmm3,(%rdx)
  movss  0x4(%rax),%xmm1
  comiss %xmm3,%xmm0
  xorps  %xmm2,%xmm1
  subss  (%rsi),%xmm1
  movss  %xmm1,0x4(%rdx)
  ja     L287
  comiss %xmm1,%xmm0
  jbe    L679
  movss  %xmm0,0x4(%rdx)
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  movss  -0x4(%rax),%xmm1
  xorps  %xmm2,%xmm1
  subss  %xmm0,%xmm1
  movss  %xmm1,(%rsi)
  cmp    %ebx,%ecx
  jl     L422
L577:
  mov    %ebx,%eax
  mov    %ebx,%r12d
  sar    %eax
L120:
  cmp    %eax,0x8(%rsp)
  jle    L205
  mov    0x8(%rsp),%edi
  mov    (%r14),%rcx
  xor    %esi,%esi
  lea    -0x1(%rdi),%edx
  sub    %eax,%edx
  cltq
  lea    0x4(,%rdx,4),%rdx
  lea    (%rcx,%rax,4),%rdi
  call   <memset@plt>
L205:
  mov    0x14(%rsp),%eax
  lea    (%rax,%rax,1),%r15d
  cmp    %r15d,%r12d
  jge    L652
  lea    -0x1(%r15),%edx
  mov    0x0(%rbp),%rcx
  movslq %r12d,%rsi
  movss  D98(%rip),%xmm0
  sub    %r12d,%edx
  add    %rsi,%rdx
  lea    (%rcx,%rsi,4),%rax
  lea    0x4(%rcx,%rdx,4),%rdx
  nopw   0x0(%rax,%rax,1)
L338:
  movss  %xmm0,(%rax)
  add    $0x4,%rax
  cmp    %rdx,%rax
  jne    L338
L652:
  mov    0x8(%rsp),%r13d
  add    0x18(%rsp),%r13d
  cmpl   $0x1,0xc(%rsp)
  jle    L424
  mov    %r15d,%eax
  movslq 0xc(%rsp),%rdi
  mov    %rbp,0x70(%rsp)
  mov    $0x1,%r12d
  sar    %eax
  mov    %r14,%rbp
  mov    %r13d,%r14d
  movss  D116(%rip),%xmm1
  mov    %eax,0x10(%rsp)
  mov    0x70(%rsp),%r13
  movss  D112(%rip),%xmm3
  mov    %ebx,0x1c(%rsp)
  mov    %r12,%rbx
  mov    %rdi,%r12
  nopl   0x0(%rax,%rax,1)
L439:
  xor    %eax,%eax
  test   %r15d,%r15d
  jle    L244
  mov    0x68(%rsp),%rax
  mov    0x0(%rbp,%rbx,8),%r10
  xor    %esi,%esi
  pxor   %xmm4,%xmm4
  mov    0x0(%r13,%rbx,8),%rdx
  mov    (%rax,%rbx,8),%rax
  jmp    L140
  cs nopw 0x0(%rax,%rax,1)
L421:
  comiss %xmm0,%xmm1
  jbe    L74
  movss  (%rax),%xmm0
  subss  0x4(%rax),%xmm0
  mulss  D78(%rip),%xmm0
  movss  %xmm0,(%rcx)
  subss  (%rax),%xmm0
  movss  %xmm0,(%rdx)
  movss  0x4(%rax),%xmm0
  xorps  %xmm3,%xmm0
  subss  (%rcx),%xmm0
  movss  %xmm0,0x4(%rdx)
L288:
  add    $0x2,%esi
  add    $0x8,%rax
  add    $0x8,%rdx
  cmp    %r15d,%esi
  jge    L209
L140:
  mov    %esi,%ecx
  movaps %xmm4,%xmm2
  sar    %ecx
  movslq %ecx,%rcx
  lea    (%r10,%rcx,4),%rcx
  movl   $0x0,(%rcx)
  subss  (%rax),%xmm2
  movss  %xmm2,(%rdx)
  movss  0x4(%rax),%xmm0
  comiss %xmm2,%xmm1
  xorps  %xmm3,%xmm0
  subss  (%rcx),%xmm0
  movss  %xmm0,0x4(%rdx)
  ja     L421
  comiss %xmm0,%xmm1
  jbe    L288
  movss  %xmm1,0x4(%rdx)
  add    $0x2,%esi
  add    $0x8,%rax
  add    $0x8,%rdx
  movss  -0x4(%rax),%xmm0
  xorps  %xmm3,%xmm0
  subss  %xmm1,%xmm0
  movss  %xmm0,(%rcx)
  cmp    %r15d,%esi
  jl     L140
L209:
  movslq 0x10(%rsp),%rax
L244:
  cmp    %r14d,%eax
  jge    L303
  mov    0x0(%rbp,%rbx,8),%rsi
  lea    -0x1(%r14),%edx
  sub    %eax,%edx
  lea    (%rsi,%rax,4),%rdi
  lea    0x4(,%rdx,4),%rdx
  xor    %esi,%esi
  call   <memset@plt>
  movss  D116(%rip),%xmm1
  movss  D112(%rip),%xmm3
L303:
  add    $0x1,%rbx
  cmp    %r12,%rbx
  jne    L439
  mov    %r13,%rax
  mov    0x1c(%rsp),%ebx
  mov    %r14d,%r13d
  mov    %rbp,%r14
  mov    %rax,%rbp
L424:
  test   %ebx,%ebx
  jle    L636
  movslq 0xc(%rsp),%rax
  lea    -0x1(%rbx),%ecx
  movss  D98(%rip),%xmm0
  mov    0x0(%rbp,%rax,8),%rdx
  lea    0x4(%rdx),%rax
  lea    (%rax,%rcx,4),%rcx
  jmp    L37
  nopl   0x0(%rax,%rax,1)
L683:
  add    $0x4,%rax
L37:
  movss  %xmm0,(%rdx)
  mov    %rax,%rdx
  cmp    %rcx,%rax
  jne    L683
L636:
  mov    0x8(%rsp),%eax
  lea    (%rax,%rax,1),%edx
  cmp    0x14(%rsp),%eax
  jge    L269
  movslq 0xc(%rsp),%rax
  mov    0x68(%rsp),%rbx
  movslq %edx,%rcx
  mov    %edx,%r8d
  shl    $0x2,%rcx
  movss  D116(%rip),%xmm0
  pxor   %xmm4,%xmm4
  movss  D78(%rip),%xmm5
  mov    (%rbx,%rax,8),%rdi
  mov    (%r14,%rax,8),%rsi
  movss  D112(%rip),%xmm2
  add    %rcx,%rdi
  add    0x0(%rbp,%rax,8),%rcx
  jmp    L115
  nopl   0x0(%rax,%rax,1)
L14:
  comiss %xmm1,%xmm0
  jbe    L390
  movss  (%rdi),%xmm1
  subss  0x4(%rdi),%xmm1
  mulss  %xmm5,%xmm1
  movss  %xmm1,(%rax)
  subss  (%rdi),%xmm1
  movss  %xmm1,(%rcx)
  movss  0x4(%rdi),%xmm1
  xorps  %xmm2,%xmm1
  subss  (%rax),%xmm1
  movss  %xmm1,0x4(%rcx)
L656:
  add    $0x2,%r8d
  add    $0x8,%rdi
  add    $0x8,%rcx
  cmp    %r15d,%r8d
  jge    L116
L115:
  mov    %r8d,%eax
  movaps %xmm4,%xmm3
  shr    $0x1f,%eax
  add    %r8d,%eax
  sar    %eax
  cltq
  lea    (%rsi,%rax,4),%rax
  movl   $0x0,(%rax)
  subss  (%rdi),%xmm3
  movss  %xmm3,(%rcx)
  movss  0x4(%rdi),%xmm1
  comiss %xmm3,%xmm0
  xorps  %xmm2,%xmm1
  subss  (%rax),%xmm1
  movss  %xmm1,0x4(%rcx)
  ja     L14
  comiss %xmm1,%xmm0
  jbe    L656
  movss  %xmm0,0x4(%rcx)
  movss  0x4(%rdi),%xmm1
  xorps  %xmm2,%xmm1
  subss  %xmm0,%xmm1
  movss  %xmm1,(%rax)
  jmp    L656
  nopl   0x0(%rax)
L639:
  movss  %xmm0,(%rdx)
  movss  (%rax),%xmm1
  addss  %xmm0,%xmm1
  movss  %xmm1,(%rsi)
  jmp    L679
  nopl   (%rax)
L74:
  movss  %xmm1,(%rdx)
  movss  (%rax),%xmm0
  addss  %xmm1,%xmm0
  movss  %xmm0,(%rcx)
  jmp    L288
  nopl   (%rax)
L390:
  movss  %xmm0,(%rcx)
  movss  (%rdi),%xmm1
  addss  %xmm0,%xmm1
  movss  %xmm1,(%rax)
  jmp    L656
  nopl   (%rax)
L116:
  sub    $0x1,%r15d
  mov    0x14(%rsp),%ebx
  xor    %eax,%eax
  sub    %edx,%r15d
  and    $0xfffffffe,%r15d
  cmp    %ebx,0x8(%rsp)
  cmovge %eax,%r15d
  lea    0x2(%rdx,%r15,1),%ebx
  sar    %ebx
  mov    %ebx,0x8(%rsp)
L269:
  cmp    %r13d,0x8(%rsp)
  jge    L289
  movslq 0xc(%rsp),%rax
  movslq 0x8(%rsp),%rbx
  lea    -0x1(%r13),%r10d
  xor    %esi,%esi
  mov    (%r14,%rax,8),%rax
  sub    %ebx,%r10d
  lea    0x4(,%r10,4),%rdx
  lea    (%rax,%rbx,4),%rdi
  call   <memset@plt>
L289:
  movslq 0xc(%rsp),%rax
  test   %eax,%eax
  js     L217
  mov    0x60(%rsp),%rbx
  mov    0x18(%rsp),%r13d
  lea    0x8(%rbx,%rax,8),%rbp
  lea    -0x1(%r13),%eax
  lea    0x4(,%rax,4),%r12
  nopw   0x0(%rax,%rax,1)
L634:
  test   %r13d,%r13d
  jle    L447
  mov    (%rbx),%rdi
  mov    %r12,%rdx
  xor    %esi,%esi
  call   <memset@plt>
L447:
  add    $0x8,%rbx
  cmp    %rbp,%rbx
  jne    L634
L217:
  add    $0x28,%rsp
  pop    %rbx
  pop    %rbp
  pop    %r12
  pop    %r13
  pop    %r14
  pop    %r15
  ret
L441:
  test   %ebx,%ebx
  jle    L629
  mov    0x68(%rsp),%rax
  mov    (%r9),%rdi
  xor    %ecx,%ecx
  movss  D112(%rip),%xmm2
  mov    0x0(%rbp),%rdx
  movss  D116(%rip),%xmm0
  mov    (%rax),%rax
  movss  D78(%rip),%xmm4
  jmp    L675
  nopl   (%rax)
L593:
  comiss %xmm1,%xmm0
  jbe    L173
  movss  (%rax),%xmm1
  subss  0x4(%rax),%xmm1
  mulss  %xmm4,%xmm1
  movss  %xmm1,(%rsi)
  subss  (%rax),%xmm1
  movss  %xmm1,(%rdx)
  movss  0x4(%rax),%xmm1
  xorps  %xmm2,%xmm1
  subss  (%rsi),%xmm1
  movss  %xmm1,0x4(%rdx)
L284:
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  cmp    %ebx,%ecx
  jge    L609
L675:
  mov    %ecx,%esi
  sar    %esi
  movslq %esi,%rsi
  lea    (%rdi,%rsi,4),%rsi
  movss  (%rsi),%xmm3
  subss  (%rax),%xmm3
  movss  %xmm3,(%rdx)
  movss  0x4(%rax),%xmm1
  comiss %xmm3,%xmm0
  xorps  %xmm2,%xmm1
  subss  (%rsi),%xmm1
  movss  %xmm1,0x4(%rdx)
  ja     L593
  comiss %xmm1,%xmm0
  jbe    L284
  movss  %xmm0,0x4(%rdx)
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  movss  -0x4(%rax),%xmm1
  xorps  %xmm2,%xmm1
  subss  %xmm0,%xmm1
  movss  %xmm1,(%rsi)
  cmp    %ebx,%ecx
  jl     L675
L609:
  mov    %ebx,%ecx
L696:
  mov    0x14(%rsp),%eax
  lea    (%rax,%rax,1),%edi
  cmp    %ecx,%edi
  jle    L464
  lea    -0x1(%rdi),%edx
  mov    0x0(%rbp),%rsi
  movslq %ecx,%r10
  movss  D98(%rip),%xmm0
  sub    %ecx,%edx
  add    %r10,%rdx
  lea    (%rsi,%r10,4),%rax
  lea    0x4(%rsi,%rdx,4),%rdx
  nopl   (%rax)
L499:
  movss  %xmm0,(%rax)
  add    $0x4,%rax
  cmp    %rdx,%rax
  jne    L499
L464:
  mov    0xc(%rsp),%eax
  cmp    $0x1,%eax
  jle    L622
  movss  D116(%rip),%xmm2
  movss  D78(%rip),%xmm4
  movslq %eax,%r13
  mov    $0x1,%r11d
  mov    0x68(%rsp),%r9
  movss  D112(%rip),%xmm3
  nopl   0x0(%rax,%rax,1)
L405:
  test   %edi,%edi
  jle    L503
  mov    (%r14,%r11,8),%r8
  mov    (%r9,%r11,8),%rax
  xor    %ecx,%ecx
  mov    0x0(%rbp,%r11,8),%rdx
  jmp    L112
  nopl   0x0(%rax)
L459:
  comiss %xmm0,%xmm2
  jbe    L252
  movss  (%rax),%xmm0
  subss  0x4(%rax),%xmm0
  mulss  %xmm4,%xmm0
  movss  %xmm0,(%rsi)
  subss  (%rax),%xmm0
  movss  %xmm0,(%rdx)
  movss  0x4(%rax),%xmm0
  xorps  %xmm3,%xmm0
  subss  (%rsi),%xmm0
  movss  %xmm0,0x4(%rdx)
L174:
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  cmp    %edi,%ecx
  jge    L503
L112:
  mov    %ecx,%esi
  sar    %esi
  movslq %esi,%rsi
  lea    (%r8,%rsi,4),%rsi
  movss  (%rsi),%xmm1
  subss  (%rax),%xmm1
  movss  %xmm1,(%rdx)
  movss  0x4(%rax),%xmm0
  comiss %xmm1,%xmm2
  xorps  %xmm3,%xmm0
  subss  (%rsi),%xmm0
  movss  %xmm0,0x4(%rdx)
  ja     L459
  comiss %xmm0,%xmm2
  jbe    L174
  movss  %xmm2,0x4(%rdx)
  add    $0x2,%ecx
  add    $0x8,%rax
  add    $0x8,%rdx
  movss  -0x4(%rax),%xmm0
  xorps  %xmm3,%xmm0
  subss  %xmm2,%xmm0
  movss  %xmm0,(%rsi)
  cmp    %edi,%ecx
  jl     L112
L503:
  add    $0x1,%r11
  cmp    %r11,%r13
  jne    L405
L622:
  test   %ebx,%ebx
  jle    L53
  movslq 0xc(%rsp),%rax
  lea    -0x1(%rbx),%ecx
  movss  D98(%rip),%xmm0
  mov    0x0(%rbp,%rax,8),%rdx
  lea    0x4(%rdx),%rax
  lea    (%rax,%rcx,4),%rcx
  jmp    L280
  nopl   0x0(%rax,%rax,1)
L290:
  add    $0x4,%rax
L280:
  movss  %xmm0,(%rdx)
  mov    %rax,%rdx
  cmp    %rax,%rcx
  jne    L290
L53:
  mov    0x8(%rsp),%eax
  lea    (%rax,%rax,1),%esi
  cmp    0x14(%rsp),%eax
  jge    L217
  movslq 0xc(%rsp),%rax
  mov    0x68(%rsp),%rbx
  movslq %esi,%rdx
  movss  D112(%rip),%xmm2
  shl    $0x2,%rdx
  movss  D116(%rip),%xmm0
  movss  D78(%rip),%xmm4
  mov    (%rbx,%rax,8),%rcx
  mov    (%r14,%rax,8),%r8
  add    %rdx,%rcx
  add    0x0(%rbp,%rax,8),%rdx
  jmp    L133
  nopl   0x0(%rax)
L579:
  comiss %xmm1,%xmm0
  jbe    L211
  movss  (%rcx),%xmm1
  subss  0x4(%rcx),%xmm1
  mulss  %xmm4,%xmm1
  movss  %xmm1,(%rax)
  subss  (%rcx),%xmm1
  movss  %xmm1,(%rdx)
  movss  0x4(%rcx),%xmm1
  xorps  %xmm2,%xmm1
  subss  (%rax),%xmm1
  movss  %xmm1,0x4(%rdx)
L309:
  add    $0x2,%esi
  add    $0x8,%rcx
  add    $0x8,%rdx
  cmp    %edi,%esi
  jge    L217
L133:
  mov    %esi,%eax
  shr    $0x1f,%eax
  add    %esi,%eax
  sar    %eax
  cltq
  lea    (%r8,%rax,4),%rax
  movss  (%rax),%xmm3
  subss  (%rcx),%xmm3
  movss  %xmm3,(%rdx)
  movss  0x4(%rcx),%xmm1
  comiss %xmm3,%xmm0
  xorps  %xmm2,%xmm1
  subss  (%rax),%xmm1
  movss  %xmm1,0x4(%rdx)
  ja     L579
  comiss %xmm1,%xmm0
  jbe    L309
  movss  %xmm0,0x4(%rdx)
  movss  0x4(%rcx),%xmm1
  xorps  %xmm2,%xmm1
  subss  %xmm0,%xmm1
  movss  %xmm1,(%rax)
  jmp    L309
  nopl   0x0(%rax,%rax,1)
L173:
  movss  %xmm0,(%rdx)
  movss  (%rax),%xmm1
  addss  %xmm0,%xmm1
  movss  %xmm1,(%rsi)
  jmp    L284
  nopl   (%rax)
L252:
  movss  %xmm2,(%rdx)
  movss  (%rax),%xmm0
  addss  %xmm2,%xmm0
  movss  %xmm0,(%rsi)
  jmp    L174
  nopl   (%rax)
L211:
  movss  %xmm0,(%rdx)
  movss  (%rcx),%xmm1
  addss  %xmm0,%xmm1
  movss  %xmm1,(%rax)
  jmp    L309
L629:
  xor    %ecx,%ecx
  jmp    L696
```

tool_calls: name: extract_symbolic_data
arguments: data_tag: D116
data_type: byte[4]
name: extract_symbolic_data
arguments: data_tag: D112
data_type: byte[4]
name: extract_symbolic_data
arguments: data_tag: D78
data_type: f32
name: extract_symbolic_data
arguments: data_tag: D98
data_type: f32

tool: {"D116": [0x6f,0x12,0x83,0x3a]}

tool: {"D112": [0x00,0x00,0x00,0x80]}

tool: D78: 0.5

tool: D98: 1

assistant: ```c
void s_init_ux_pi_t_box_mpc(int N, int nx, int nu, int nbu, int nb, float **ux, float **pi, float **db, float **t, int warm_start) {
  int jj, ll, ii;
  float thr0 = 1e-3;
  if (warm_start == 1) {
    for (ll = 0; ll < 2 * nbu; ll += 2) {
      t[0][ll + 0] = ux[0][ll / 2] - db[0][ll + 0];
      t[0][ll + 1] = -db[0][ll + 1] - ux[0][ll / 2];
      if (t[0][ll + 0] < thr0) {
        if (t[0][ll + 1] < thr0) {
          ux[0][ll / 2] = (-db[0][ll + 1] + db[0][ll + 0]) * 0.5;
          t[0][ll + 0] = ux[0][ll / 2] - db[0][ll + 0];
          t[0][ll + 1] = -db[0][ll + 1] - ux[0][ll / 2];
        } else {
          t[0][ll + 0] = thr0;
          ux[0][ll / 2] = db[0][ll + 0] + thr0;
        }
      } else if (t[0][ll + 1] < thr0) {
        t[0][ll + 1] = thr0;
        ux[0][ll / 2] = -db[0][ll + 1] - thr0;
      }
    }
    for (; ll < 2 * nb; ll++)
      t[0][ll] = 1.0;
    for (jj = 1; jj < N; jj++) {
      for (ll = 0; ll < 2 * nb; ll += 2) {
        t[jj][ll + 0] = ux[jj][ll / 2] - db[jj][ll + 0];
        t[jj][ll + 1] = -db[jj][ll + 1] - ux[jj][ll / 2];
        if (t[jj][ll + 0] < thr0) {
          if (t[jj][ll + 1] < thr0) {
            ux[jj][ll / 2] = (-db[jj][ll + 1] + db[jj][ll + 0]) * 0.5;
            t[jj][ll + 0] = ux[jj][ll / 2] - db[jj][ll + 0];
            t[jj][ll + 1] = -db[jj][ll + 1] - ux[jj][ll / 2];
          } else {
            t[jj][ll + 0] = thr0;
            ux[jj][ll / 2] = db[jj][ll + 0] + thr0;
          }
        } else if (t[jj][ll + 1] < thr0) {
          t[jj][ll + 1] = thr0;
          ux[jj][ll / 2] = -db[jj][ll + 1] - thr0;
        }
      }
    }
    for (ll = 0; ll < 2 * nbu; ll++)
      t[N][ll] = 1;
    for (ll = 2 * nu; ll < 2 * nb; ll += 2) {
      t[N][ll + 0] = ux[N][ll / 2] - db[N][ll + 0];
      t[N][ll + 1] = -db[N][ll + 1] - ux[N][ll / 2];
      if (t[N][ll + 0] < thr0) {
        if (t[N][ll + 1] < thr0) {
          ux[N][ll / 2] = (-db[N][ll + 1] + db[N][ll + 0]) * 0.5;
          t[N][ll + 0] = ux[N][ll / 2] - db[N][ll + 0];
          t[N][ll + 1] = -db[N][ll + 1] - ux[N][ll / 2];
        } else {
          t[N][ll + 0] = thr0;
          ux[N][ll / 2] = db[N][ll + 0] + thr0;
        }
      } else if (t[N][ll + 1] < thr0) {
        t[N][ll + 1] = thr0;
        ux[N][ll / 2] = -db[N][ll + 1] - thr0;
      }
    }
  } else {
    for (ll = 0; ll < 2 * nbu; ll += 2) {
      ux[0][ll / 2] = 0.0;
      t[0][ll + 0] = ux[0][ll / 2] - db[0][ll + 0];
      t[0][ll + 1] = -db[0][ll + 1] - ux[0][ll / 2];
      if (t[0][ll + 0] < thr0) {
        if (t[0][ll + 1] < thr0) {
          ux[0][ll / 2] = (-db[0][ll + 1] + db[0][ll + 0]) * 0.5;
          t[0][ll + 0] = ux[0][ll / 2] - db[0][ll + 0];
          t[0][ll + 1] = -db[0][ll + 1] - ux[0][ll / 2];
        } else {
          t[0][ll + 0] = thr0;
          ux[0][ll / 2] = db[0][ll + 0] + thr0;
        }
      } else if (t[0][ll + 1] < thr0) {
        t[0][ll + 1] = thr0;
        ux[0][ll / 2] = -db[0][ll + 1] - thr0;
      }
    }
    for (ii = ll / 2; ii < nu; ii++)
      ux[0][ii] = 0.0;
    for (; ll < 2 * nb; ll++)
      t[0][ll] = 1.0;
    for (jj = 1; jj < N; jj++) {
      for (ll = 0; ll < 2 * nb; ll += 2) {
        ux[jj][ll / 2] = 0.0;
        t[jj][ll + 0] = ux[jj][ll / 2] - db[jj][ll + 0];
        t[jj][ll + 1] = -db[jj][ll + 1] - ux[jj][ll / 2];
        if (t[jj][ll + 0] < thr0) {
          if (t[jj][ll + 1] < thr0) {
            ux[jj][ll / 2] = (-db[jj][ll + 1] + db[jj][ll + 0]) * 0.5;
            t[jj][ll + 0] = ux[jj][ll / 2] - db[jj][ll + 0];
            t[jj][ll + 1] = -db[jj][ll + 1] - ux[jj][ll / 2];
          } else {
            t[jj][ll + 0] = thr0;
            ux[jj][ll / 2] = db[jj][ll + 0] + thr0;
          }
        } else if (t[jj][ll + 1] < thr0) {
          t[jj][ll + 1] = thr0;
          ux[jj][ll / 2] = -db[jj][ll + 1] - thr0;
        }
      }
      for (ii = ll / 2; ii < nx + nu; ii++)
        ux[jj][ii] = 0.0;
    }
    for (ll = 0; ll < 2 * nbu; ll++)
      t[N][ll] = 1.0;
    for (ll = 2 * nu; ll < 2 * nb; ll += 2) {
      ux[N][ll / 2] = 0.0;
      t[N][ll + 0] = ux[N][ll / 2] - db[N][ll + 0];
      t[N][ll + 1] = -db[N][ll + 1] - ux[N][ll / 2];
      if (t[N][ll + 0] < thr0) {
        if (t[N][ll + 1] < thr0) {
          ux[N][ll / 2] = (-db[N][ll + 1] + db[N][ll + 0]) * 0.5;
          t[N][ll + 0] = ux[N][ll / 2] - db[N][ll + 0];
          t[N][ll + 1] = -db[N][ll + 1] - ux[N][ll / 2];
        } else {
          t[N][ll + 0] = thr0;
          ux[N][ll / 2] = db[N][ll + 0] + thr0;
        }
      } else if (t[N][ll + 1] < thr0) {
        t[N][ll + 1] = thr0;
        ux[N][ll / 2] = -db[N][ll + 1] - thr0;
      }
    }
    for (ii = ll / 2; ii < nx + nu; ii++)
      ux[N][ii] = 0.0;
    for (jj = 0; jj <= N; jj++)
      for (ll = 0; ll < nx; ll++)
        pi[jj][ll] = 0.0;
  }
}
```