// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2019 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Asher Elmquist
// =============================================================================
//
// =============================================================================

#include <cuda.h>
#include "pointcloud.cuh"

namespace chrono {
namespace sensor {

// Converts 32bpp ARGB imgIn pixels to 8bpp Grayscale imgOut pixels
__global__ void pointcloud_from_depth_kernel(float* imgIn, float* imgOut, int numPixels, LidarParams params) {
    int index = blockDim.x * blockIdx.x + threadIdx.x;
    if (index < numPixels) {
        int hIndex = index % params.horizontal_samples;
        int vIndex = index / params.horizontal_samples;

        float vAngle = (vIndex / (float)(params.vertical_samples)) * params.vFOV - params.vFOV / 2.;
        float hAngle = (hIndex / (float)(params.horizontal_samples)) * params.hFOV - params.hFOV / 2.;

        float range = imgIn[2 * index];

        float proj_xy = range * cos(vAngle);

        float x = proj_xy * cos(hAngle);
        float y = proj_xy * sin(hAngle);
        float z = range * sin(vAngle);
        imgOut[4 * index] = x;
        imgOut[4 * index + 1] = y;
        imgOut[4 * index + 2] = z;
        imgOut[4 * index + 3] = imgIn[2 * index + 1];
    }
}

void cuda_pointcloud_from_depth(void* bufDI, void* bufOut, int width, int height, LidarParams params) {
    int numPixels = width * height;
    const int nThreads = 512;
    int nBlocks = (numPixels + nThreads - 1) / nThreads;

    pointcloud_from_depth_kernel<<<nBlocks, nThreads>>>((float*)bufDI, (float*)bufOut, numPixels, params);
}

}  // namespace sensor
}  // namespace chrono

	.file	"tmpxft_002316dc_00000000-6_pointcloud.cudafe1.cpp"
	.text
#APP

#NO_APP
	.type	_ZL26__cudaUnregisterBinaryUtilv, @function
_ZL26__cudaUnregisterBinaryUtilv:
.LFB2011:
	.cfi_startproc
	endbr64
	movq	_ZL20__cudaFatCubinHandle(%rip), %rdi
	jmp	__cudaUnregisterFatBinary@PLT
	.cfi_endproc
.LFE2011:
	.size	_ZL26__cudaUnregisterBinaryUtilv, .-_ZL26__cudaUnregisterBinaryUtilv
	.globl	_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE
	.type	_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE, @function
_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE:
.LFB2033:
	.cfi_startproc
	endbr64
	subq	$152, %rsp
	.cfi_def_cfa_offset 160
	movq	%rdi, 24(%rsp)
	leaq	56(%rsp), %rdi
	movq	%rsi, 16(%rsp)
	leaq	68(%rsp), %rsi
	movl	%edx, 12(%rsp)
	leaq	40(%rsp), %rdx
	movq	%fs:40, %rax
	movq	%rax, 136(%rsp)
	xorl	%eax, %eax
	leaq	24(%rsp), %rax
	movq	%rcx, 128(%rsp)
	leaq	48(%rsp), %rcx
	movq	%rax, 104(%rsp)
	leaq	16(%rsp), %rax
	movq	%rax, 112(%rsp)
	leaq	12(%rsp), %rax
	movq	%rax, 120(%rsp)
	movabsq	$4294967297, %rax
	movq	%rax, 56(%rsp)
	movl	$1, 64(%rsp)
	movq	%rax, 68(%rsp)
	movl	$1, 76(%rsp)
	call	__cudaPopCallConfiguration@PLT
	testl	%eax, %eax
	jne	.L2
	pushq	48(%rsp)
	.cfi_def_cfa_offset 168
	leaq	_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE(%rip), %rdi
	pushq	48(%rsp)
	.cfi_def_cfa_offset 176
	movq	84(%rsp), %rcx
	movl	92(%rsp), %r8d
	movq	72(%rsp), %rsi
	movl	80(%rsp), %edx
	leaq	120(%rsp), %r9
	call	cudaLaunchKernel@PLT
	popq	%rax
	.cfi_def_cfa_offset 168
	popq	%rdx
	.cfi_def_cfa_offset 160
.L2:
	movq	136(%rsp), %rax
	subq	%fs:40, %rax
	je	.L4
	call	__stack_chk_fail@PLT
.L4:
	addq	$152, %rsp
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE2033:
	.size	_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE, .-_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE
	.globl	_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE
	.type	_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE, @function
_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE:
.LFB2034:
	.cfi_startproc
	endbr64
	subq	$24, %rsp
	.cfi_def_cfa_offset 32
	movq	%rcx, 8(%rsp)
	movq	%rsp, %rcx
	movq	%xmm0, (%rsp)
	call	_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE
	addq	$24, %rsp
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE2034:
	.size	_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE, .-_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE
	.globl	_ZN6chrono6sensor26cuda_pointcloud_from_depthEPvS1_iiNS0_11LidarParamsE
	.type	_ZN6chrono6sensor26cuda_pointcloud_from_depthEPvS1_iiNS0_11LidarParamsE, @function
_ZN6chrono6sensor26cuda_pointcloud_from_depthEPvS1_iiNS0_11LidarParamsE:
.LFB2008:
	.cfi_startproc
	endbr64
	pushq	%r14
	.cfi_def_cfa_offset 16
	.cfi_offset 14, -16
	xorl	%r9d, %r9d
	movq	%r8, %r14
	xorl	%r8d, %r8d
	pushq	%r13
	.cfi_def_cfa_offset 24
	.cfi_offset 13, -24
	movq	%xmm0, %r13
	pushq	%r12
	.cfi_def_cfa_offset 32
	.cfi_offset 12, -32
	movq	%rsi, %r12
	pushq	%rbp
	.cfi_def_cfa_offset 40
	.cfi_offset 6, -40
	movq	%rdi, %rbp
	pushq	%rbx
	.cfi_def_cfa_offset 48
	.cfi_offset 3, -48
	movl	%edx, %ebx
	imull	%ecx, %ebx
	movl	$512, %ecx
	subq	$48, %rsp
	.cfi_def_cfa_offset 96
	movq	%fs:40, %rax
	movq	%rax, 40(%rsp)
	xorl	%eax, %eax
	leal	511(%rbx), %eax
	cltd
	idivl	%ecx
	movl	$8388609, %edx
	movl	$1, %ecx
	salq	$9, %rdx
	movl	%eax, (%rsp)
	movabsq	$4294967297, %rax
	movq	%rax, 4(%rsp)
	movq	(%rsp), %rdi
	movl	8(%rsp), %esi
	call	__cudaPushCallConfiguration@PLT
	testl	%eax, %eax
	jne	.L9
	leaq	24(%rsp), %rcx
	movl	%ebx, %edx
	movq	%r12, %rsi
	movq	%rbp, %rdi
	movq	%r13, 24(%rsp)
	movq	%r14, 32(%rsp)
	call	_Z86__device_stub__ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsEPfS_iRN6chrono6sensor11LidarParamsE
.L9:
	movq	40(%rsp), %rax
	subq	%fs:40, %rax
	je	.L11
	call	__stack_chk_fail@PLT
.L11:
	addq	$48, %rsp
	.cfi_def_cfa_offset 48
	popq	%rbx
	.cfi_def_cfa_offset 40
	popq	%rbp
	.cfi_def_cfa_offset 32
	popq	%r12
	.cfi_def_cfa_offset 24
	popq	%r13
	.cfi_def_cfa_offset 16
	popq	%r14
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE2008:
	.size	_ZN6chrono6sensor26cuda_pointcloud_from_depthEPvS1_iiNS0_11LidarParamsE, .-_ZN6chrono6sensor26cuda_pointcloud_from_depthEPvS1_iiNS0_11LidarParamsE
	.section	.rodata.str1.1,"aMS",@progbits,1
.LC0:
	.string	"_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE"
	.section	.text.startup,"ax",@progbits
	.type	_ZL24__sti____cudaRegisterAllv, @function
_ZL24__sti____cudaRegisterAllv:
.LFB2036:
	.cfi_startproc
	endbr64
	pushq	%rax
	.cfi_def_cfa_offset 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
	leaq	_ZN6chrono6sensor28pointcloud_from_depth_kernelEPfS1_iNS0_11LidarParamsE(%rip), %rsi
	pushq	$0
	.cfi_def_cfa_offset 40
	movq	%rdx, %rcx
	pushq	$0
	.cfi_def_cfa_offset 48
	movq	%rax, _ZL20__cudaFatCubinHandle(%rip)
	call	__cudaRegisterFunction@PLT
	movq	_ZL20__cudaFatCubinHandle(%rip), %rdi
	addq	$32, %rsp
	.cfi_def_cfa_offset 16
	call	__cudaRegisterFatBinaryEnd@PLT
	leaq	_ZL26__cudaUnregisterBinaryUtilv(%rip), %rdi
	popq	%rdx
	.cfi_def_cfa_offset 8
	jmp	atexit@PLT
	.cfi_endproc
.LFE2036:
	.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
	.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:


#include <hip/hip_runtime.h>
// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2019 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Asher Elmquist
// =============================================================================
//
// =============================================================================

namespace chrono {
namespace sensor {

struct LidarParams {
    float hFOV;
    float vFOV;
    int horizontal_samples;
    int vertical_samples;
};

void cuda_pointcloud_from_depth(void* bufDI, void* bufOut, int width, int height, LidarParams params);
}  // namespace sensor
}  // namespace chrono

	.text
	.file	"pointcloud.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
