// File: msu-starx[P]starx/exosuit.cpp
// Function: ExoSuit::updateSensors()
void ExoSuit::updateSensors(void){
    u.clear();
    y.clear();
    lasty.clear();
    pressureStates.clear();
    forceStates.clear();
    emgStates.clear();
    for(int i = 0; i < SensorMax; i++) {
        // Pressure Sensors
        /*if(i < pressurePins.size())
           pressureStates.push_back(cvt.convertToPressure(cvt.convertToVoltage(adcValues[pressurePins[i]], board.ADCVoltageMultiplier)));*/
        if(i < pressurePins.size()) {
            u.push_back(cvt.convertToPressure(cvt.convertToVoltage(adcValues[pressurePins[i]], board.ADCVoltageMultiplier)));
            y.push_back(0.945 * lasty[i] + 0.0549 * u[i]);
            lasty.push_back(y[i]);
            pressureStates.push_back(y[i]);
        }
        // Force Sensors
        if(i < forcePins.size())
            forceStates.push_back(cvt.convertToForce(cvt.convertToVoltage(adcValues[forcePins[i]], board.ADCVoltageMultiplier)));
        // EMG Sensors
        if(i < emgPins.size())
            emgStates.push_back(cvt.convertToVoltage(adcValues[emgPins[i]], board.ADCVoltageMultiplier));
    }
    for(int i = 0; i < pressureStates.size(); i++) {
        std::cout << pressureStates[i] << " ";
    }
    for(int i = 0; i < emgStates.size(); i++) {
        std::cout << emgStates[i] << " ";
    }
    std::cout << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xe0, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x40(%rbp)
callq 0x56f0
movq -0x40(%rbp), %rdi
addq $0x18, %rdi
callq 0x56f0
movq -0x40(%rbp), %rdi
addq $0x30, %rdi
callq 0x56f0
movq -0x40(%rbp), %rdi
addq $0x378, %rdi            # imm = 0x378
callq 0x56f0
movq -0x40(%rbp), %rdi
addq $0x390, %rdi            # imm = 0x390
callq 0x56f0
movq -0x40(%rbp), %rdi
addq $0x3a8, %rdi            # imm = 0x3A8
callq 0x56f0
movl $0x0, -0xc(%rbp)
movq -0x40(%rbp), %rcx
movl -0xc(%rbp), %eax
cmpl 0x310(%rcx), %eax
jae 0x4e83
movq -0x40(%rbp), %rdi
movslq -0xc(%rbp), %rax
movq %rax, -0x48(%rbp)
addq $0x330, %rdi            # imm = 0x330
callq 0x5710
movq %rax, %rcx
movq -0x48(%rbp), %rax
cmpq %rcx, %rax
jae 0x4cf7
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x48, %rax
movq %rax, -0x70(%rbp)
movq %rdi, %rax
addq $0x48, %rax
movq %rax, -0x78(%rbp)
movq %rdi, %rax
addq $0x318, %rax            # imm = 0x318
movq %rax, -0x80(%rbp)
addq $0x330, %rdi            # imm = 0x330
movslq -0xc(%rbp), %rsi
callq 0x5760
movq -0x80(%rbp), %rdi
movslq (%rax), %rsi
callq 0x5760
movq -0x78(%rbp), %rdi
movq %rax, %rcx
movq -0x40(%rbp), %rax
movl (%rcx), %esi
movsd 0x1e8(%rax), %xmm0
callq 0xa3b0
movq -0x70(%rbp), %rdi
callq 0xa3e0
movq -0x40(%rbp), %rdi
movsd %xmm0, -0x18(%rbp)
leaq -0x18(%rbp), %rsi
callq 0x5730
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x18, %rax
movq %rax, -0x60(%rbp)
addq $0x30, %rdi
movslq -0xc(%rbp), %rsi
callq 0x5780
movq -0x40(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x68(%rbp)
movslq -0xc(%rbp), %rsi
callq 0x5780
movsd -0x68(%rbp), %xmm0
movq -0x60(%rbp), %rdi
movsd 0x742d(%rip), %xmm1     # 0xc0b0
mulsd (%rax), %xmm1
movsd 0x7419(%rip), %xmm2     # 0xc0a8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
leaq -0x20(%rbp), %rsi
callq 0x5730
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x30, %rax
movq %rax, -0x58(%rbp)
addq $0x18, %rdi
movslq -0xc(%rbp), %rsi
callq 0x5780
movq -0x58(%rbp), %rdi
movq %rax, %rsi
callq 0x57a0
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x378, %rax            # imm = 0x378
movq %rax, -0x50(%rbp)
addq $0x18, %rdi
movslq -0xc(%rbp), %rsi
callq 0x5780
movq -0x50(%rbp), %rdi
movq %rax, %rsi
callq 0x57a0
movq -0x40(%rbp), %rdi
movslq -0xc(%rbp), %rax
movq %rax, -0x88(%rbp)
addq $0x348, %rdi            # imm = 0x348
callq 0x5710
movq %rax, %rcx
movq -0x88(%rbp), %rax
cmpq %rcx, %rax
jae 0x4dc2
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x390, %rax            # imm = 0x390
movq %rax, -0x90(%rbp)
movq %rdi, %rax
addq $0x48, %rax
movq %rax, -0x98(%rbp)
movq %rdi, %rax
addq $0x48, %rax
movq %rax, -0xa0(%rbp)
movq %rdi, %rax
addq $0x318, %rax            # imm = 0x318
movq %rax, -0xa8(%rbp)
addq $0x348, %rdi            # imm = 0x348
movslq -0xc(%rbp), %rsi
callq 0x5760
movq -0xa8(%rbp), %rdi
movslq (%rax), %rsi
callq 0x5760
movq -0xa0(%rbp), %rdi
movq %rax, %rcx
movq -0x40(%rbp), %rax
movl (%rcx), %esi
movsd 0x1e8(%rax), %xmm0
callq 0xa3b0
movq -0x98(%rbp), %rdi
callq 0xa430
movq -0x90(%rbp), %rdi
movsd %xmm0, -0x28(%rbp)
leaq -0x28(%rbp), %rsi
callq 0x5730
movq -0x40(%rbp), %rdi
movslq -0xc(%rbp), %rax
movq %rax, -0xb0(%rbp)
addq $0x360, %rdi            # imm = 0x360
callq 0x5710
movq %rax, %rcx
movq -0xb0(%rbp), %rax
cmpq %rcx, %rax
jae 0x4e73
movq -0x40(%rbp), %rdi
movq %rdi, %rax
addq $0x3a8, %rax            # imm = 0x3A8
movq %rax, -0xb8(%rbp)
movq %rdi, %rax
addq $0x48, %rax
movq %rax, -0xc0(%rbp)
movq %rdi, %rax
addq $0x318, %rax            # imm = 0x318
movq %rax, -0xc8(%rbp)
addq $0x360, %rdi            # imm = 0x360
movslq -0xc(%rbp), %rsi
callq 0x5760
movq -0xc8(%rbp), %rdi
movslq (%rax), %rsi
callq 0x5760
movq -0xc0(%rbp), %rdi
movq %rax, %rcx
movq -0x40(%rbp), %rax
movl (%rcx), %esi
movsd 0x1e8(%rax), %xmm0
callq 0xa3b0
movq -0xb8(%rbp), %rdi
movsd %xmm0, -0x30(%rbp)
leaq -0x30(%rbp), %rsi
callq 0x5730
jmp 0x4e75
movl -0xc(%rbp), %eax
addl $0x1, %eax
movl %eax, -0xc(%rbp)
jmp 0x4b8d
movl $0x0, -0x34(%rbp)
movq -0x40(%rbp), %rdi
movslq -0x34(%rbp), %rax
movq %rax, -0xd0(%rbp)
addq $0x378, %rdi            # imm = 0x378
callq 0x5810
movq %rax, %rcx
movq -0xd0(%rbp), %rax
cmpq %rcx, %rax
jae 0x4ef2
movq -0x40(%rbp), %rdi
addq $0x378, %rdi            # imm = 0x378
movslq -0x34(%rbp), %rsi
callq 0x5780
movsd (%rax), %xmm0
movq 0xd0ed(%rip), %rdi      # 0x11fc0
callq 0x2400
movq %rax, %rdi
leaq 0x73db(%rip), %rsi      # 0xc2bd
callq 0x2200
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0x4e8a
movl $0x0, -0x38(%rbp)
movq -0x40(%rbp), %rdi
movslq -0x38(%rbp), %rax
movq %rax, -0xd8(%rbp)
addq $0x3a8, %rdi            # imm = 0x3A8
callq 0x5810
movq %rax, %rcx
movq -0xd8(%rbp), %rax
cmpq %rcx, %rax
jae 0x4f61
movq -0x40(%rbp), %rdi
addq $0x3a8, %rdi            # imm = 0x3A8
movslq -0x38(%rbp), %rsi
callq 0x5780
movsd (%rax), %xmm0
movq 0xd07e(%rip), %rdi      # 0x11fc0
callq 0x2400
movq %rax, %rdi
leaq 0x736c(%rip), %rsi      # 0xc2bd
callq 0x2200
movl -0x38(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x38(%rbp)
jmp 0x4ef9
movq 0xd058(%rip), %rdi      # 0x11fc0
movq 0xd039(%rip), %rsi      # 0x11fa8
callq 0x2230
addq $0xe0, %rsp
popq %rbp
retq
nopl (%rax)