// File: msu-starx[P]starx/exosuit.cpp
// Function: ExoSuit::initialize()
void ExoSuit::initialize(void){
    //Filter
    u.clear();
    y.clear();
    lasty.clear();
    u.resize(4);
    y.resize(4);
    lasty.resize(4);
    // Sensor States
    adcValues.clear();
    pressureStates.clear();
    forceStates.clear();
    emgStates.clear();
    adcValues.resize(board.ADCPinCount);
    pressureStates.resize(pressurePins.size());
    forceStates.resize(forcePins.size());
    emgStates.resize(emgPins.size());
    // Pressure Controller States
    pressurePID.clear();
    outputStates.clear();
    setpointStates.clear();
    dutyCycleStates.clear();
    dutyCycles.clear();
    pressurePID.resize(pressurePins.size());
    outputStates.resize(pressurePins.size());
    setpointStates.resize(pressurePins.size());
    dutyCycleStates.resize(pressurePins.size());
    dutyCycles.resize(pressurePins.size());
    for(int i = 0; i < pressurePID.size(); i++) {
        pressurePID[i].setMode(pressurePID[i].AUTOMATIC);
        pressurePID[i].setOutputLimits(outputMin, outputMax);
        pressurePID[i].setTunings(Kp, Ki, Kd);
    }
    // Serial Port
    board.setupSerial(portName, baudRate);
    sleep(1); // Pause for 1 second
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xb0, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x58(%rbp)
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x18, %rdi
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x30, %rdi
callq 0x56f0
movq -0x58(%rbp), %rdi
movl $0x4, %esi
callq 0x5a30
movq -0x58(%rbp), %rdi
addq $0x18, %rdi
movl $0x4, %esi
callq 0x5a30
movq -0x58(%rbp), %rdi
addq $0x30, %rdi
movl $0x4, %esi
callq 0x5a30
movq -0x58(%rbp), %rdi
addq $0x318, %rdi            # imm = 0x318
callq 0x5660
movq -0x58(%rbp), %rdi
addq $0x378, %rdi            # imm = 0x378
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x390, %rdi            # imm = 0x390
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x3a8, %rdi            # imm = 0x3A8
callq 0x56f0
movq -0x58(%rbp), %rax
movq %rax, %rdi
addq $0x318, %rdi            # imm = 0x318
movslq 0x1e0(%rax), %rsi
callq 0x5ac0
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x378, %rax            # imm = 0x378
movq %rax, -0x90(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x90(%rbp), %rdi
movq %rax, %rsi
callq 0x5a30
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x390, %rax            # imm = 0x390
movq %rax, -0x88(%rbp)
addq $0x348, %rdi            # imm = 0x348
callq 0x5710
movq -0x88(%rbp), %rdi
movq %rax, %rsi
callq 0x5a30
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x3a8, %rax            # imm = 0x3A8
movq %rax, -0x80(%rbp)
addq $0x360, %rdi            # imm = 0x360
callq 0x5710
movq -0x80(%rbp), %rdi
movq %rax, %rsi
callq 0x5a30
movq -0x58(%rbp), %rdi
addq $0x470, %rdi            # imm = 0x470
callq 0x5b50
movq -0x58(%rbp), %rdi
addq $0x410, %rdi            # imm = 0x410
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x428, %rdi            # imm = 0x428
callq 0x56f0
movq -0x58(%rbp), %rdi
addq $0x440, %rdi            # imm = 0x440
callq 0x5880
movq -0x58(%rbp), %rdi
addq $0x458, %rdi            # imm = 0x458
callq 0x58a0
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x470, %rax            # imm = 0x470
movq %rax, -0x78(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x78(%rbp), %rdi
movq %rax, %rsi
callq 0x5b70
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x410, %rax            # imm = 0x410
movq %rax, -0x70(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x70(%rbp), %rdi
movq %rax, %rsi
callq 0x5a30
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x428, %rax            # imm = 0x428
movq %rax, -0x68(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x68(%rbp), %rdi
movq %rax, %rsi
callq 0x5a30
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x440, %rax            # imm = 0x440
movq %rax, -0x60(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x60(%rbp), %rdi
movq %rax, %rsi
callq 0x5c00
movq -0x58(%rbp), %rdi
movq %rdi, %rax
addq $0x458, %rax            # imm = 0x458
movq %rax, -0x50(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq -0x50(%rbp), %rdi
movq %rax, %rsi
callq 0x5c90
movl $0x0, -0xc(%rbp)
movq -0x58(%rbp), %rdi
movslq -0xc(%rbp), %rax
movq %rax, -0x98(%rbp)
addq $0x470, %rdi            # imm = 0x470
callq 0x5830
movq %rax, %rcx
movq -0x98(%rbp), %rax
cmpq %rcx, %rax
jae 0x55af
movq -0x58(%rbp), %rdi
addq $0x470, %rdi            # imm = 0x470
movslq -0xc(%rbp), %rsi
callq 0x5860
movq -0x58(%rbp), %rdi
movq %rax, -0xa0(%rbp)
addq $0x470, %rdi            # imm = 0x470
movslq -0xc(%rbp), %rsi
callq 0x5860
movq -0xa0(%rbp), %rdi
movl 0x4(%rax), %esi
callq 0xb6c0
movq -0x58(%rbp), %rdi
addq $0x470, %rdi            # imm = 0x470
movslq -0xc(%rbp), %rsi
callq 0x5860
movq %rax, %rdi
movq -0x58(%rbp), %rax
movsd 0x3f8(%rax), %xmm0
movsd 0x400(%rax), %xmm1
callq 0xb5d0
movq -0x58(%rbp), %rdi
addq $0x470, %rdi            # imm = 0x470
movslq -0xc(%rbp), %rsi
callq 0x5860
movq %rax, %rdi
movq -0x58(%rbp), %rax
movsd 0x3e0(%rax), %xmm0
movsd 0x3e8(%rax), %xmm1
movsd 0x3f0(%rax), %xmm2
callq 0xb460
movl -0xc(%rbp), %eax
addl $0x1, %eax
movl %eax, -0xc(%rbp)
jmp 0x54cd
movq -0x58(%rbp), %rax
addq $0x50, %rax
movq %rax, -0xb0(%rbp)
leaq -0x31(%rbp), %rdi
movq %rdi, -0xa8(%rbp)
callq 0x23d0
movq -0xa8(%rbp), %rdx
leaq 0x6adc(%rip), %rsi      # 0xc0b8
leaq -0x30(%rbp), %rdi
callq 0x3360
jmp 0x55e7
movq -0xb0(%rbp), %rdi
leaq -0x30(%rbp), %rsi
movl $0xf4240, %edx          # imm = 0xF4240
callq 0xa2e0
jmp 0x55fe
leaq -0x30(%rbp), %rdi
callq 0x2140
leaq -0x31(%rbp), %rdi
callq 0x2250
movl $0x1, %edi
callq 0x2190
addq $0xb0, %rsp
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x5646
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x30(%rbp), %rdi
callq 0x2140
leaq -0x31(%rbp), %rdi
callq 0x2250
movq -0x40(%rbp), %rdi
callq 0x23c0
nopl (%rax,%rax)