//#include <helper_cuda.h>
#include "project_kernel.cuh"
#include <stdio.h>

__constant__ float K[3][3];
__constant__ float Kinv[3][3];
__constant__ float eps2;
__constant__ int npoints;
__device__  float image[480*640];

__global__ void project_kernel(float *d_x, float *d_y, float *d_z, float *d_nx,
                               float *d_ny, float *d_nz, float *d_r, float *d_rgba)
{
    // each kernel function gets a surfel in the cloud

    // assume global K, assume points have already been transformed into camera frame?

    // now, based on normal and radius determine the pixels covered
    // we could probably use the projection matrix on the ellipse somehow
    //int tid = threadIdx.x + blockDim.x*blockIdx.x;
    int i = threadIdx.x + blockDim.x*blockIdx.x;

    int index = 30000;

    if (i == index) {
        printf("npoints: %d\n", npoints);
        printf("eps2: %f\n", eps2);
    }

    if (i >= npoints || d_z[i] < 0.0f) {
        if (i == index) {
            printf("Index: %d, Depth: %f\n", i, d_z[i]);
            printf("K: [%f, %f, %f; %f, %f, %f; %f, %f, %f]\n", K[0][0], K[0][1], K[0][2], K[1][0], K[1][1], K[1][2], K[2][0], K[2][1], K[2][2]);
        }
        return;
    }

    float px = K[0][0]*d_x[i]/d_z[i] + K[0][2];
    float py = K[1][1]*d_y[i]/d_z[i] + K[1][2];
    if (px < 0 || px > 640 || py < 0 || py > 480) {
        return;
    }

    float r2 = 1.0f/(d_r[i]*d_r[i]);

    float NN[3][3];
    NN[0][0] = d_nx[i]*d_nx[i]; NN[0][1] = d_nx[i]*d_ny[i]; NN[0][2] = d_nx[i]*d_nz[i];
    NN[1][0] = d_ny[i]*d_nx[i]; NN[1][1] = d_ny[i]*d_ny[i]; NN[1][2] = d_ny[i]*d_nz[i];
    NN[2][0] = d_nz[i]*d_nx[i]; NN[2][1] = d_nz[i]*d_ny[i]; NN[2][2] = d_nz[i]*d_nz[i];

    float A[3][3];
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            A[row][col] = (eps2 - r2)*NN[row][col];
        }
        A[row][row] += r2;
    }

    float c[3] = {d_x[i], d_y[i], d_z[i]};

    float Ac[3];
    for (int row = 0; row < 3; ++row) {
        Ac[row] = 0.0f;
        for (int col = 0; col < 3; ++col) {
            Ac[row] += A[row][col]*c[col];
        }
    }

    float cAc = 0.0f;
    for (int row = 0; row < 3; ++row) {
        cAc += c[row]*Ac[row];
    }

    // now we reuse A to compute AA
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            A[row][col] = (cAc - 1.0f)*A[row][col] - Ac[row]*Ac[col];
        }
    }

    // and finally, let's reuse NN and A to compute the AA with camera matrix
    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            // A*Kinv
            NN[row][col] = A[row][0]*Kinv[0][col] + A[row][1]*Kinv[1][col] + A[row][2]*Kinv[2][col];
        }
    }

    for (int row = 0; row < 3; ++row) {
        for (int col = 0; col < 3; ++col) {
            // this time Kinv^T*A
            A[row][col] = Kinv[0][row]*NN[0][col] + Kinv[1][row]*NN[1][col] + Kinv[2][row]*NN[2][col];
        }
    }

    int minx = 640; int maxx = 0; int miny = 480; int maxy = 0;
    float ox[4] = {d_r[i], -d_r[i], 0.0f, 0.0f};
    float oy[4] = {0.0f, 0.0f, d_r[i], -d_r[i]};

    for (int col = 0; col < 4; ++col) {
        px = K[0][0]*(d_x[i]+ox[col])/d_z[i] + K[0][2];
        py = K[1][1]*(d_y[i]+oy[col])/d_z[i] + K[1][2];
        if (px < minx) {
            minx = px;
        }
        if (py < miny) {
            miny = py;
        }
        if (px > maxx) {
            maxx = px;
        }
        if (py > maxy) {
            maxy = py;
        }
    }

    float x, y, disc;
    for (int row = miny; row < maxy; ++row) {

        for (int col = minx; col < maxx; ++col) {
            x = col; y = row;
            // C2 + B2.transpose()*v + v.transpose()*A2*v
            disc = A[2][2] + 2.0f*(A[0][2]*x + A[1][2]*y) + A[0][0]*x*x + 2.0f*A[0][1]*x*y + A[1][1]*y*y;
            if (disc < 0.0f) {

                if (i == index) {
                    printf("disc: %f!", disc);
                }

                x = 1.0f;
                image[640*row+col] = d_rgba[i];
            }
        }
    }

}

void project(float *d_x, float *d_y, float *d_z, float *d_nx,
             float *d_ny, float *d_nz, float *d_r, float *d_rgba,
             int nblocks, int nthreads, float neps2, float* nK, float* nKinv, float* himage,
             int hnpoints)
{
    cudaMemcpyToSymbol(K, nK, 9*sizeof(float));
    cudaMemcpyToSymbol(Kinv, nKinv, 9*sizeof(float));
    cudaMemcpyToSymbol(eps2, &neps2, sizeof(float));
    cudaMemcpyToSymbol(npoints, &hnpoints, sizeof(int));
    project_kernel<<<nblocks,nthreads>>>(d_x, d_y, d_z, d_nx, d_ny, d_nz, d_r, d_rgba);
    //float *my_image;
    //cudaGetSymbolAddress((void **)&my_image, image);
    cudaMemcpyFromSymbol(himage, image, 480*640*sizeof(float));
}

	.file	"tmpxft_0037d5c1_00000000-6_project_kernel.cudafe1.cpp"
	.text
#APP

#NO_APP
	.type	_ZL26__cudaUnregisterBinaryUtilv, @function
_ZL26__cudaUnregisterBinaryUtilv:
.LFB2030:
	.cfi_startproc
	endbr64
	movq	_ZL20__cudaFatCubinHandle(%rip), %rdi
	jmp	__cudaUnregisterFatBinary@PLT
	.cfi_endproc
.LFE2030:
	.size	_ZL26__cudaUnregisterBinaryUtilv, .-_ZL26__cudaUnregisterBinaryUtilv
	.globl	_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_
	.type	_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_, @function
_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_:
.LFB2052:
	.cfi_startproc
	endbr64
	subq	$216, %rsp
	.cfi_def_cfa_offset 224
	movq	224(%rsp), %rax
	movq	%rdi, 56(%rsp)
	leaq	88(%rsp), %rdi
	movq	%rsi, 48(%rsp)
	leaq	100(%rsp), %rsi
	movq	%rax, 8(%rsp)
	movq	232(%rsp), %rax
	movq	%rdx, 40(%rsp)
	leaq	72(%rsp), %rdx
	movq	%rcx, 32(%rsp)
	leaq	80(%rsp), %rcx
	movq	%r8, 24(%rsp)
	movq	%r9, 16(%rsp)
	movq	%rax, (%rsp)
	movq	%fs:40, %rax
	movq	%rax, 200(%rsp)
	xorl	%eax, %eax
	leaq	56(%rsp), %rax
	movl	$1, 96(%rsp)
	movq	%rax, 136(%rsp)
	leaq	48(%rsp), %rax
	movq	%rax, 144(%rsp)
	leaq	40(%rsp), %rax
	movq	%rax, 152(%rsp)
	leaq	32(%rsp), %rax
	movq	%rax, 160(%rsp)
	leaq	24(%rsp), %rax
	movq	%rax, 168(%rsp)
	leaq	16(%rsp), %rax
	movq	%rax, 176(%rsp)
	leaq	8(%rsp), %rax
	movq	%rax, 184(%rsp)
	movq	%rsp, %rax
	movq	%rax, 192(%rsp)
	movabsq	$4294967297, %rax
	movq	%rax, 88(%rsp)
	movq	%rax, 100(%rsp)
	movl	$1, 108(%rsp)
	call	__cudaPopCallConfiguration@PLT
	testl	%eax, %eax
	jne	.L2
	pushq	80(%rsp)
	.cfi_def_cfa_offset 232
	leaq	_Z14project_kernelPfS_S_S_S_S_S_S_(%rip), %rdi
	pushq	80(%rsp)
	.cfi_def_cfa_offset 240
	movq	116(%rsp), %rcx
	movl	124(%rsp), %r8d
	movq	104(%rsp), %rsi
	movl	112(%rsp), %edx
	leaq	152(%rsp), %r9
	call	cudaLaunchKernel@PLT
	popq	%rax
	.cfi_def_cfa_offset 232
	popq	%rdx
	.cfi_def_cfa_offset 224
.L2:
	movq	200(%rsp), %rax
	subq	%fs:40, %rax
	je	.L4
	call	__stack_chk_fail@PLT
.L4:
	addq	$216, %rsp
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE2052:
	.size	_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_, .-_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_
	.globl	_Z14project_kernelPfS_S_S_S_S_S_S_
	.type	_Z14project_kernelPfS_S_S_S_S_S_S_, @function
_Z14project_kernelPfS_S_S_S_S_S_S_:
.LFB2053:
	.cfi_startproc
	endbr64
	jmp	_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_
	.cfi_endproc
.LFE2053:
	.size	_Z14project_kernelPfS_S_S_S_S_S_S_, .-_Z14project_kernelPfS_S_S_S_S_S_S_
	.globl	_Z7projectPfS_S_S_S_S_S_S_iifS_S_S_i
	.type	_Z7projectPfS_S_S_S_S_S_S_iifS_S_S_i, @function
_Z7projectPfS_S_S_S_S_S_S_iifS_S_S_i:
.LFB2027:
	.cfi_startproc
	endbr64
	pushq	%r15
	.cfi_def_cfa_offset 16
	.cfi_offset 15, -16
	movq	%r9, %r15
	pushq	%r14
	.cfi_def_cfa_offset 24
	.cfi_offset 14, -24
	movq	%r8, %r14
	movl	$1, %r8d
	pushq	%r13
	.cfi_def_cfa_offset 32
	.cfi_offset 13, -32
	movq	%rcx, %r13
	xorl	%ecx, %ecx
	pushq	%r12
	.cfi_def_cfa_offset 40
	.cfi_offset 12, -40
	movq	%rdx, %r12
	movl	$36, %edx
	pushq	%rbp
	.cfi_def_cfa_offset 48
	.cfi_offset 6, -48
	movq	%rsi, %rbp
	pushq	%rbx
	.cfi_def_cfa_offset 56
	.cfi_offset 3, -56
	movq	%rdi, %rbx
	leaq	_ZL1K(%rip), %rdi
	subq	$56, %rsp
	.cfi_def_cfa_offset 112
	movq	144(%rsp), %rsi
	movss	%xmm0, 12(%rsp)
	call	cudaMemcpyToSymbol@PLT
	movq	152(%rsp), %rsi
	xorl	%ecx, %ecx
	movl	$1, %r8d
	movl	$36, %edx
	leaq	_ZL4Kinv(%rip), %rdi
	call	cudaMemcpyToSymbol@PLT
	xorl	%ecx, %ecx
	leaq	12(%rsp), %rsi
	movl	$1, %r8d
	movl	$4, %edx
	leaq	_ZL4eps2(%rip), %rdi
	call	cudaMemcpyToSymbol@PLT
	xorl	%ecx, %ecx
	movl	$1, %r8d
	leaq	168(%rsp), %rsi
	movl	$4, %edx
	leaq	_ZL7npoints(%rip), %rdi
	call	cudaMemcpyToSymbol@PLT
	movl	136(%rsp), %eax
	xorl	%r9d, %r9d
	xorl	%r8d, %r8d
	movl	128(%rsp), %edx
	movl	%eax, 36(%rsp)
	movabsq	$4294967297, %rax
	movq	%rax, 40(%rsp)
	movl	44(%rsp), %ecx
	movl	%edx, 24(%rsp)
	movq	36(%rsp), %rdx
	movq	%rax, 28(%rsp)
	movq	24(%rsp), %rdi
	movl	32(%rsp), %esi
	call	__cudaPushCallConfiguration@PLT
	testl	%eax, %eax
	jne	.L9
	pushq	120(%rsp)
	.cfi_def_cfa_offset 120
	movq	%r12, %rdx
	movq	%r15, %r9
	movq	%r14, %r8
	pushq	120(%rsp)
	.cfi_def_cfa_offset 128
	movq	%r13, %rcx
	movq	%rbp, %rsi
	movq	%rbx, %rdi
	call	_Z48__device_stub__Z14project_kernelPfS_S_S_S_S_S_S_PfS_S_S_S_S_S_S_
	popq	%rax
	.cfi_def_cfa_offset 120
	popq	%rdx
	.cfi_def_cfa_offset 112
.L9:
	movq	160(%rsp), %rdi
	movl	$2, %r8d
	xorl	%ecx, %ecx
	movl	$1228800, %edx
	leaq	_ZL5image(%rip), %rsi
	call	cudaMemcpyFromSymbol@PLT
	addq	$56, %rsp
	.cfi_def_cfa_offset 56
	popq	%rbx
	.cfi_def_cfa_offset 48
	popq	%rbp
	.cfi_def_cfa_offset 40
	popq	%r12
	.cfi_def_cfa_offset 32
	popq	%r13
	.cfi_def_cfa_offset 24
	popq	%r14
	.cfi_def_cfa_offset 16
	popq	%r15
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE2027:
	.size	_Z7projectPfS_S_S_S_S_S_S_iifS_S_S_i, .-_Z7projectPfS_S_S_S_S_S_S_iifS_S_S_i
	.section	.rodata.str1.1,"aMS",@progbits,1
.LC0:
	.string	"_Z14project_kernelPfS_S_S_S_S_S_S_"
.LC1:
	.string	"K"
.LC2:
	.string	"Kinv"
.LC3:
	.string	"eps2"
.LC4:
	.string	"npoints"
.LC5:
	.string	"image"
	.section	.text.startup,"ax",@progbits
	.type	_ZL24__sti____cudaRegisterAllv, @function
_ZL24__sti____cudaRegisterAllv:
.LFB2055:
	.cfi_startproc
	endbr64
	pushq	%rbx
	.cfi_def_cfa_offset 16
	.cfi_offset 3, -16
	leaq	_ZL15__fatDeviceText(%rip), %rdi
	call	__cudaRegisterFatBinary@PLT
	pushq	$0
	.cfi_def_cfa_offset 24
	xorl	%r9d, %r9d
	orl	$-1, %r8d
	pushq	$0
	.cfi_def_cfa_offset 32
	leaq	.LC0(%rip), %rdx
	movq	%rax, %rdi
	movq	%rax, %rbx
	pushq	$0
	.cfi_def_cfa_offset 40
	movq	%rdx, %rcx
	leaq	_Z14project_kernelPfS_S_S_S_S_S_S_(%rip), %rsi
	pushq	$0
	.cfi_def_cfa_offset 48
	movq	%rax, _ZL20__cudaFatCubinHandle(%rip)
	call	__cudaRegisterFunction@PLT
	addq	$32, %rsp
	.cfi_def_cfa_offset 16
	movq	%rbx, %rdi
	xorl	%r8d, %r8d
	pushq	$0
	.cfi_def_cfa_offset 24
	leaq	.LC1(%rip), %rdx
	movl	$36, %r9d
	leaq	_ZL1K(%rip), %rsi
	pushq	$1
	.cfi_def_cfa_offset 32
	movq	%rdx, %rcx
	call	__cudaRegisterVar@PLT
	popq	%rax
	.cfi_def_cfa_offset 24
	popq	%rdx
	.cfi_def_cfa_offset 16
	pushq	$0
	.cfi_def_cfa_offset 24
	pushq	$1
	.cfi_def_cfa_offset 32
	leaq	.LC2(%rip), %rdx
	movq	%rbx, %rdi
	xorl	%r8d, %r8d
	movq	%rdx, %rcx
	movl	$36, %r9d
	leaq	_ZL4Kinv(%rip), %rsi
	call	__cudaRegisterVar@PLT
	popq	%rcx
	.cfi_def_cfa_offset 24
	popq	%rsi
	.cfi_def_cfa_offset 16
	pushq	$0
	.cfi_def_cfa_offset 24
	pushq	$1
	.cfi_def_cfa_offset 32
	leaq	.LC3(%rip), %rdx
	movq	%rbx, %rdi
	xorl	%r8d, %r8d
	movq	%rdx, %rcx
	movl	$4, %r9d
	leaq	_ZL4eps2(%rip), %rsi
	call	__cudaRegisterVar@PLT
	popq	%rdi
	.cfi_def_cfa_offset 24
	popq	%r8
	.cfi_def_cfa_offset 16
	pushq	$0
	.cfi_def_cfa_offset 24
	pushq	$1
	.cfi_def_cfa_offset 32
	leaq	.LC4(%rip), %rdx
	movq	%rbx, %rdi
	xorl	%r8d, %r8d
	movq	%rdx, %rcx
	movl	$4, %r9d
	leaq	_ZL7npoints(%rip), %rsi
	call	__cudaRegisterVar@PLT
	popq	%r9
	.cfi_def_cfa_offset 24
	popq	%r10
	.cfi_def_cfa_offset 16
	pushq	$0
	.cfi_def_cfa_offset 24
	pushq	$0
	.cfi_def_cfa_offset 32
	leaq	.LC5(%rip), %rdx
	movq	%rbx, %rdi
	xorl	%r8d, %r8d
	movl	$1228800, %r9d
	movq	%rdx, %rcx
	leaq	_ZL5image(%rip), %rsi
	call	__cudaRegisterVar@PLT
	movq	_ZL20__cudaFatCubinHandle(%rip), %rdi
	popq	%r11
	.cfi_def_cfa_offset 24
	popq	%rbx
	.cfi_def_cfa_offset 16
	call	__cudaRegisterFatBinaryEnd@PLT
	leaq	_ZL26__cudaUnregisterBinaryUtilv(%rip), %rdi
	popq	%rbx
	.cfi_def_cfa_offset 8
	jmp	atexit@PLT
	.cfi_endproc
.LFE2055:
	.size	_ZL24__sti____cudaRegisterAllv, .-_ZL24__sti____cudaRegisterAllv
	.section	.init_array,"aw"
	.align 8
	.quad	_ZL24__sti____cudaRegisterAllv
	.section	.nvFatBinSegment,"aw"
	.align 8
	.type	_ZL15__fatDeviceText, @object
	.size	_ZL15__fatDeviceText, 24
_ZL15__fatDeviceText:
	.long	1180844977
	.long	1
	.quad	fatbinData
	.quad	0
	.local	_ZL20__cudaFatCubinHandle
	.comm	_ZL20__cudaFatCubinHandle,8,8
	.local	_ZL5image
	.comm	_ZL5image,1228800,32
	.local	_ZL7npoints
	.comm	_ZL7npoints,4,4
	.local	_ZL4eps2
	.comm	_ZL4eps2,4,4
	.local	_ZL4Kinv
	.comm	_ZL4Kinv,36,32
	.local	_ZL1K
	.comm	_ZL1K,36,32
	.ident	"GCC: (Ubuntu 13.3.0-6ubuntu2~24.04) 13.3.0"
	.section	.note.GNU-stack,"",@progbits
	.section	.note.gnu.property,"a"
	.align 8
	.long	1f - 0f
	.long	4f - 1f
	.long	5
0:
	.string	"GNU"
1:
	.align 8
	.long	0xc0000002
	.long	3f - 2f
2:
	.long	0x3
3:
	.align 8
4:

#ifndef PROJECT_KERNEL_CUH
#define PROJECT_KERNEL_CUH

#include <hip/hip_runtime.h>

void project(float *d_x, float *d_y, float *d_z, float *d_nx,
             float *d_ny, float *d_nz, float *d_r, float *d_rgba,
             int nblocks, int nthreads, float neps2, float* nK, float* nKinv, float* himage,
             int hnpoints);

#endif // PROJECT_KERNEL_CUH

	.text
	.file	"project_kernel.hip"
	.type	__hip_cuid_,@object             # @__hip_cuid_
	.bss
	.globl	__hip_cuid_
__hip_cuid_:
	.byte	0                               # 0x0
	.size	__hip_cuid_, 1

	.ident	"AMD clang version 18.0.0git (https://github.com/RadeonOpenCompute/llvm-project roc-6.3.2 25012 e5bf7e55c91490b07c49d8960fa7983d864936c4)"
	.section	".note.GNU-stack","",@progbits
	.addrsig
	.addrsig_sym __hip_cuid_

	.text
	.p2alignl 7, 3214868480
	.fill 96, 4, 3214868480
	.type	__hip_cuid_,@object             ; @__hip_cuid_
	.section	.bss,"aw",@nobits
	.globl	__hip_cuid_
__hip_cuid_:
	.byte	0                               ; 0x0
	.size	__hip_cuid_, 1

	.ident	"AMD clang version 18.0.0git (https://github.com/RadeonOpenCompute/llvm-project roc-6.3.2 25012 e5bf7e55c91490b07c49d8960fa7983d864936c4)"
	.section	".note.GNU-stack","",@progbits
	.addrsig
	.addrsig_sym __hip_cuid_
	.amdgcn_target "amdgcn-amd-amdhsa--gfx1100"
	.amdgpu_metadata
---
amdhsa.kernels:  []
amdhsa.target:   amdgcn-amd-amdhsa--gfx1100
amdhsa.version:
  - 1
  - 2
...

	.end_amdgpu_metadata
