// File: markondej[P]phase_correl/phase_correl.cpp
// Function: PhaseCorrelation::ComputeShift(GrayscaleImage const&, GrayscaleImage const&, int&, int&)
static void ComputeShift(const GrayscaleImage &image1, const GrayscaleImage &image2, int &deltax, int &deltay) {
        if ((image1.GetWidth() != image2.GetWidth()) || (image1.GetHeight() != image2.GetHeight()) || !image1.GetWidth() || (image1.GetWidth() & (image1.GetWidth() - 1)) || !image1.GetHeight() || (image1.GetHeight() & (image1.GetHeight() - 1))) {
            throw std::runtime_error("Image sizes do not match");
        }

        if (!image1.GetWidth() || (image1.GetWidth() & (image1.GetWidth() - 1)) || !image1.GetHeight() || (image1.GetHeight() & (image1.GetHeight() - 1))) {
            throw std::runtime_error("Wrong image size");
        }

        std::size_t width = image1.GetWidth(), height = image1.GetHeight();

        std::vector<std::complex<double>> data1, data2;
        data1.resize(width * height);
        data2.resize(width * height);

        // Convert image pixels to complex number format, use only real part
        for (std::size_t i = 0; i < width * height; i++) {
            data1[i] = { static_cast<double>(image1.GetData()[i]), 0.0 };
            data2[i] = { static_cast<double>(image2.GetData()[i]), 0.0 };
        } 

        // Perform 2D FFT on each image
        FFT2D(data1, width);
        FFT2D(data2, width);

        // Compute normalized cross power spectrum
        for (std::size_t i = 0; i < width * height; i++) {
            ComputeNormalized(data1[i], data2[i], data1[i]);
        }

        // Perform inverse 2D FFT on obtained matrix
        FFT2D(data1, width, true);

        // Search for peak
        std::size_t offset = 0;
        double max = 0.0; deltax = 0; deltay = 0;
        for (std::size_t j = 0; j < height; j++)
            for (std::size_t i = 0; i < width; i++) {
                double d = sqrt(pow(data1[offset].real(), 2) + pow(data1[offset].imag(), 2));
                if (d > max) {
                    max = d; deltax = static_cast<int>(i); deltay = static_cast<int>(j);
                }
                offset++;
            }

        if (deltax > static_cast<int>(width >> 1))
            deltax -= static_cast<int>(width);
        if (deltay > static_cast<int>(height >> 1))
            deltay -= static_cast<int>(height);
    }

// Assembly:
subq $0x158, %rsp            # imm = 0x158
movq %rdi, 0x150(%rsp)
movq %rsi, 0x148(%rsp)
movq %rdx, 0x140(%rsp)
movq %rcx, 0x138(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, 0x88(%rsp)
movq 0x148(%rsp), %rdi
callq 0x1ce0
movq %rax, %rcx
movq 0x88(%rsp), %rax
cmpq %rcx, %rax
jne 0x16a1
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, 0x80(%rsp)
movq 0x148(%rsp), %rdi
callq 0x1cf0
movq %rax, %rcx
movq 0x80(%rsp), %rax
cmpq %rcx, %rax
jne 0x16a1
movq 0x150(%rsp), %rdi
callq 0x1ce0
cmpq $0x0, %rax
je 0x16a1
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, 0x78(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, %rcx
movq 0x78(%rsp), %rax
subq $0x1, %rcx
andq %rcx, %rax
cmpq $0x0, %rax
jne 0x16a1
movq 0x150(%rsp), %rdi
callq 0x1cf0
cmpq $0x0, %rax
je 0x16a1
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, 0x70(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, %rcx
movq 0x70(%rsp), %rax
subq $0x1, %rcx
andq %rcx, %rax
cmpq $0x0, %rax
je 0x16ff
movl $0x10, %edi
callq 0x1090
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x68(%rsp)
leaq 0x39bc(%rip), %rsi      # 0x5079
callq 0x1070
jmp 0x16c4
movq 0x68(%rsp), %rdi
movq 0x6920(%rip), %rsi      # 0x7ff0
movq 0x68f1(%rip), %rdx      # 0x7fc8
callq 0x1190
movq 0x68(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x130(%rsp)
movl %eax, 0x12c(%rsp)
callq 0x10c0
jmp 0x1cc8
movq 0x150(%rsp), %rdi
callq 0x1ce0
cmpq $0x0, %rax
je 0x178d
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, 0x60(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, %rcx
movq 0x60(%rsp), %rax
subq $0x1, %rcx
andq %rcx, %rax
cmpq $0x0, %rax
jne 0x178d
movq 0x150(%rsp), %rdi
callq 0x1cf0
cmpq $0x0, %rax
je 0x178d
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, 0x58(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, %rcx
movq 0x58(%rsp), %rax
subq $0x1, %rcx
andq %rcx, %rax
cmpq $0x0, %rax
je 0x17eb
movl $0x10, %edi
callq 0x1090
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x50(%rsp)
leaq 0x38e9(%rip), %rsi      # 0x5092
callq 0x1070
jmp 0x17b0
movq 0x50(%rsp), %rdi
movq 0x6834(%rip), %rsi      # 0x7ff0
movq 0x6805(%rip), %rdx      # 0x7fc8
callq 0x1190
movq 0x50(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x130(%rsp)
movl %eax, 0x12c(%rsp)
callq 0x10c0
jmp 0x1cc8
movq 0x150(%rsp), %rdi
callq 0x1ce0
movq %rax, 0x120(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1cf0
movq %rax, 0x118(%rsp)
leaq 0x100(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x1d00
leaq 0xe8(%rsp), %rdi
callq 0x1d00
movq 0x48(%rsp), %rdi
movq 0x120(%rsp), %rsi
movq 0x118(%rsp), %rax
imulq %rax, %rsi
callq 0x1d10
jmp 0x1854
movq 0x120(%rsp), %rsi
movq 0x118(%rsp), %rax
imulq %rax, %rsi
leaq 0xe8(%rsp), %rdi
callq 0x1d10
jmp 0x1877
movq $0x0, 0xe0(%rsp)
movq 0xe0(%rsp), %rax
movq 0x120(%rsp), %rcx
imulq 0x118(%rsp), %rcx
cmpq %rcx, %rax
jae 0x19b3
movq 0x150(%rsp), %rdi
callq 0x1db0
movq %rax, 0x40(%rsp)
jmp 0x18b9
movq 0x40(%rsp), %rax
movq 0xe0(%rsp), %rcx
movzbl (%rax,%rcx), %eax
cvtsi2sd %eax, %xmm0
leaq 0xd0(%rsp), %rdi
xorps %xmm1, %xmm1
callq 0x1dc0
jmp 0x18e0
movq 0xe0(%rsp), %rsi
leaq 0x100(%rsp), %rdi
callq 0x1df0
movups 0xd0(%rsp), %xmm0
movups %xmm0, (%rax)
movq 0x148(%rsp), %rdi
callq 0x1db0
movq %rax, 0x38(%rsp)
jmp 0x1914
movq 0x38(%rsp), %rax
movq 0xe0(%rsp), %rcx
movzbl (%rax,%rcx), %eax
cvtsi2sd %eax, %xmm0
leaq 0xc0(%rsp), %rdi
xorps %xmm1, %xmm1
callq 0x1dc0
jmp 0x193b
movq 0xe0(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
callq 0x1df0
movq 0xc0(%rsp), %rcx
movq %rcx, (%rax)
movq 0xc8(%rsp), %rcx
movq %rcx, 0x8(%rax)
movq 0xe0(%rsp), %rax
addq $0x1, %rax
movq %rax, 0xe0(%rsp)
jmp 0x1883
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x130(%rsp)
movl %eax, 0x12c(%rsp)
leaq 0xe8(%rsp), %rdi
callq 0x2150
leaq 0x100(%rsp), %rdi
callq 0x2150
jmp 0x1cc8
movq 0x120(%rsp), %rsi
leaq 0x100(%rsp), %rdi
xorl %edx, %edx
callq 0x1e10
jmp 0x19cc
movq 0x120(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
xorl %edx, %edx
callq 0x1e10
jmp 0x19e5
movq $0x0, 0xb8(%rsp)
movq 0xb8(%rsp), %rax
movq 0x120(%rsp), %rcx
imulq 0x118(%rsp), %rcx
cmpq %rcx, %rax
jae 0x1a89
movq 0xb8(%rsp), %rsi
leaq 0x100(%rsp), %rdi
movq %rdi, 0x20(%rsp)
callq 0x1df0
movq %rax, 0x28(%rsp)
movq 0xb8(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
callq 0x1df0
movq 0x20(%rsp), %rdi
movq %rax, 0x30(%rsp)
movq 0xb8(%rsp), %rsi
callq 0x1df0
movq 0x28(%rsp), %rdi
movq 0x30(%rsp), %rsi
movq %rax, %rdx
callq 0x1ed0
jmp 0x1a6e
jmp 0x1a70
movq 0xb8(%rsp), %rax
addq $0x1, %rax
movq %rax, 0xb8(%rsp)
jmp 0x19f1
movq 0x120(%rsp), %rsi
leaq 0x100(%rsp), %rdi
movl $0x1, %edx
callq 0x1e10
jmp 0x1aa5
movq $0x0, 0xb0(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0xa8(%rsp)
movq 0x140(%rsp), %rax
movl $0x0, (%rax)
movq 0x138(%rsp), %rax
movl $0x0, (%rax)
movq $0x0, 0xa0(%rsp)
movq 0xa0(%rsp), %rax
cmpq 0x118(%rsp), %rax
jae 0x1c44
movq $0x0, 0x98(%rsp)
movq 0x98(%rsp), %rax
cmpq 0x120(%rsp), %rax
jae 0x1c29
movq 0xb0(%rsp), %rsi
leaq 0x100(%rsp), %rdi
callq 0x1df0
movq %rax, %rdi
callq 0x2130
movsd %xmm0, 0x18(%rsp)
jmp 0x1b42
movsd 0x18(%rsp), %xmm0
movsd 0x34b8(%rip), %xmm1     # 0x5008
callq 0x11b0
movsd %xmm0, 0x8(%rsp)
movq 0xb0(%rsp), %rsi
leaq 0x100(%rsp), %rdi
callq 0x1df0
movq %rax, %rdi
callq 0x2140
movsd %xmm0, 0x10(%rsp)
jmp 0x1b80
movsd 0x10(%rsp), %xmm0
movsd 0x347a(%rip), %xmm1     # 0x5008
callq 0x11b0
movaps %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
addsd %xmm1, %xmm0
callq 0x10e0
movsd %xmm0, 0x90(%rsp)
movsd 0x90(%rsp), %xmm0
ucomisd 0xa8(%rsp), %xmm0
jbe 0x1bfc
movsd 0x90(%rsp), %xmm0
movsd %xmm0, 0xa8(%rsp)
movq 0x98(%rsp), %rax
movl %eax, %ecx
movq 0x140(%rsp), %rax
movl %ecx, (%rax)
movq 0xa0(%rsp), %rax
movl %eax, %ecx
movq 0x138(%rsp), %rax
movl %ecx, (%rax)
movq 0xb0(%rsp), %rax
addq $0x1, %rax
movq %rax, 0xb0(%rsp)
movq 0x98(%rsp), %rax
addq $0x1, %rax
movq %rax, 0x98(%rsp)
jmp 0x1b07
jmp 0x1c2b
movq 0xa0(%rsp), %rax
addq $0x1, %rax
movq %rax, 0xa0(%rsp)
jmp 0x1ae5
movq 0x140(%rsp), %rax
movl (%rax), %eax
movq 0x120(%rsp), %rcx
shrq %rcx
cmpl %ecx, %eax
jle 0x1c75
movq 0x120(%rsp), %rax
movl %eax, %edx
movq 0x140(%rsp), %rax
movl (%rax), %ecx
subl %edx, %ecx
movl %ecx, (%rax)
movq 0x138(%rsp), %rax
movl (%rax), %eax
movq 0x118(%rsp), %rcx
shrq %rcx
cmpl %ecx, %eax
jle 0x1ca6
movq 0x118(%rsp), %rax
movl %eax, %edx
movq 0x138(%rsp), %rax
movl (%rax), %ecx
subl %edx, %ecx
movl %ecx, (%rax)
leaq 0xe8(%rsp), %rdi
callq 0x2150
leaq 0x100(%rsp), %rdi
callq 0x2150
addq $0x158, %rsp            # imm = 0x158
retq
movq 0x130(%rsp), %rdi
callq 0x11c0
nopw %cs:(%rax,%rax)
nop