// File: /workspace/github_cpp_makefile_star2_O2/richardposner[P]BioNetFit2/src/code/Swarm.cpp
// Function: Swarm::calcParticlePosQPSO(unsigned int, std::vector<double, std::allocator<double>>)
vector<double> Swarm::calcParticlePosQPSO(unsigned int particle, vector<double> mBests) {

	vector<double> nextPositions;
	vector<double> neighborhoodBests = getNeighborhoodBestPositions(particle);
	for (unsigned int d = 0; d < mBests.size(); ++d) {
		//cout << particle << " before: " << particleCurrParamSets_[particle][d] << endl;
		//cout << particle << " mbest: " << mBests[d] << endl;
		//cout << particle << " best: " << particleBestParamSets_[particle][d] << endl;
		//cout << particle << " swarmbest: " << getNeighborhoodBestPositions(particle)[d] << endl;
		double fi1 = ((double) rand() / (RAND_MAX));
		//cout << "f1: " << fi1 << endl;
		//double fi2 = ((double) rand() / (RAND_MAX));
		//double p = ((fi1 * particleBestFits_.at(particle)) + (fi2 * swarmBestFits_.begin()->first)) / (fi1 + fi2);
		// TODO: Work out whether or not we should be using abs() for this and below
		double p = fi1 * abs(particleBestParamSets_[particle][d]) + (1 - fi1) * neighborhoodBests[d];
		double u = ((double) rand() / (RAND_MAX));

		// TODO: Linearly decrease beta from 1.0 to 0.5? See Liu et al.

		// Liu et all, 2005, eq 2a
		if (u > 0.5) {
			nextPositions.push_back(p - beta_ * abs(mBests[d] - abs(particleCurrParamSets_.at(particle)[d])) * log(1/u));
			//cout << particle << " p: " << p << endl;
			//cout << "mbest - curr: " << mBests[d] - particleCurrParamSets_.at(particle)[d] << endl;
			//cout << "log: " << log(1/u) << endl;
			//cout << particle << " after: " << p - beta_ * abs(mBests[d] - particleCurrParamSets_.at(particle)[d]) * log(1/u) << endl;
		}
		else {
			nextPositions.push_back(p + beta_ * abs(mBests[d] - abs(particleCurrParamSets_.at(particle)[d])) * log(1/u));
			//cout << particle << " after: " << p + beta_ * abs(mBests[d] - particleCurrParamSets_.at(particle)[d]) * log(1/u) << endl;
		}
	}

	return nextPositions;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movq %rcx, %r14
movq %rsi, %r12
movl %edx, 0x34(%rsp)
xorpd %xmm0, %xmm0
movupd %xmm0, (%rdi)
movq %rdi, 0x8(%rsp)
andq $0x0, 0x10(%rdi)
leaq 0x70(%rsp), %rdi
callq 0x752f0
leaq 0x1cc0(%r12), %rax
movq %rax, 0x58(%rsp)
leaq 0x1400(%r12), %r13
xorl %r15d, %r15d
leaq 0x34(%rsp), %rbp
movl %r15d, %r15d
movq 0x8(%r14), %rax
subq (%r14), %rax
sarq $0x3, %rax
cmpq %r15, %rax
jbe 0x752a7
callq 0x28280
movl %eax, %ebx
movq 0x58(%rsp), %rdi
movq %rbp, %rsi
callq 0x86502
cvtsi2sd %ebx, %xmm0
movsd 0x610eb(%rip), %xmm1    # 0xd6200
divsd %xmm1, %xmm0
movq (%rax), %rax
movsd (%rax,%r15,8), %xmm2
andpd 0x60a76(%rip), %xmm2    # 0xd5ba0
movsd 0x610e6(%rip), %xmm1    # 0xd6218
subsd %xmm0, %xmm1
movq 0x70(%rsp), %rax
mulsd (%rax,%r15,8), %xmm1
mulsd %xmm0, %xmm2
addsd %xmm1, %xmm2
movapd %xmm2, 0x60(%rsp)
callq 0x28280
cvtsi2sd %eax, %xmm0
divsd 0x610a0(%rip), %xmm0    # 0xd6200
movsd %xmm0, 0x38(%rsp)
ucomisd 0x5f91a(%rip), %xmm0    # 0xd4a88
jbe 0x7520e
movsd 0x1da0(%r12), %xmm0
movsd %xmm0, 0x40(%rsp)
movq (%r14), %rax
movsd (%rax,%r15,8), %xmm0
movapd %xmm0, 0x10(%rsp)
movq %r13, %rdi
movq %rbp, %rsi
callq 0x861e4
movq (%rax), %rax
movsd (%rax,%r15,8), %xmm0
movapd 0x609f1(%rip), %xmm1    # 0xd5ba0
andpd %xmm1, %xmm0
movapd 0x10(%rsp), %xmm2
subsd %xmm0, %xmm2
andpd %xmm1, %xmm2
movapd %xmm2, 0x10(%rsp)
movsd 0x61049(%rip), %xmm0    # 0xd6218
divsd 0x38(%rsp), %xmm0
callq 0x28830
movapd 0x10(%rsp), %xmm1
mulsd 0x40(%rsp), %xmm1
mulsd %xmm0, %xmm1
movapd 0x60(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x28(%rsp)
movq 0x8(%rsp), %rdi
leaq 0x28(%rsp), %rsi
callq 0x5c482
jmp 0x7529f
movsd 0x1da0(%r12), %xmm0
movsd %xmm0, 0x10(%rsp)
movq (%r14), %rax
movsd (%rax,%r15,8), %xmm0
movapd %xmm0, 0x40(%rsp)
movq %r13, %rdi
movq %rbp, %rsi
callq 0x861e4
movq (%rax), %rax
movsd (%rax,%r15,8), %xmm0
movapd 0x60957(%rip), %xmm1    # 0xd5ba0
andpd %xmm1, %xmm0
movapd 0x40(%rsp), %xmm2
subsd %xmm0, %xmm2
andpd %xmm1, %xmm2
movsd 0x10(%rsp), %xmm0
mulsd %xmm2, %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x60fa5(%rip), %xmm0    # 0xd6218
divsd 0x38(%rsp), %xmm0
callq 0x28830
mulsd 0x10(%rsp), %xmm0
addsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x28(%rsp)
movq 0x8(%rsp), %rdi
leaq 0x28(%rsp), %rsi
callq 0x5c482
incl %r15d
jmp 0x750de
leaq 0x70(%rsp), %rdi
callq 0x5c44e
movq 0x8(%rsp), %rax
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %r14
jmp 0x752de
jmp 0x752d1
jmp 0x752d1
movq %rax, %r14
leaq 0x70(%rsp), %rdi
callq 0x5c44e
movq 0x8(%rsp), %rdi
callq 0x5c44e
movq %r14, %rdi
callq 0x292f0