import * as THREE from './lib/three.module.js';
import { matmul, euler_angle_to_rotate_matrix, transpose, psr_to_xyz, array_as_vector_range, array_as_vector_index_range, vector_range, euler_angle_to_rotate_matrix_3by3} from "./util.js"
import { PCDLoader } from './lib/PCDLoader.js';
import {globalObjectCategory} from './obj_cfg.js';


import {settings} from "./settings.js"

function Lidar(sceneMeta, world, frameInfo){
    this.world = world;
    this.data = world.data;
    this.frameInfo = frameInfo;    
    this.sceneMeta = sceneMeta;

    this.points = null;
    this.points_load_time = 0;

    this.remove_high_ponts = function(pcd, z){
        let position = [];
        let color = [];
        let normal = [];
        let intensity = [];
        //3, 3, 3, 1

        for (let i = 0; i < pcd.position.length/3; i++){
            if (pcd.position[i*3+2] < z){
                position.push(pcd.position[i*3+0]);
                position.push(pcd.position[i*3+1]);
                position.push(pcd.position[i*3+2]);
                
                if (pcd.color.length>0){
                    color.push(pcd.color[i*3+0]);
                    color.push(pcd.color[i*3+1]);
                    color.push(pcd.color[i*3+2]);
                }

                if (pcd.normal.length>0){
                    normal.push(pcd.normal[i*3+0]);
                    normal.push(pcd.normal[i*3+1]);
                    normal.push(pcd.normal[i*3+2]);
                }

                if (pcd.intensity){
                    intensity.push(pcd.intensity[i]);
                }
            }
        }

        pcd.position = position;
        pcd.intensity = intensity;
        pcd.color = color;
        pcd.normal = normal;

        return pcd;
    };

    

    this.preload=function(on_preload_finished){
        this.on_preload_finished = on_preload_finished;

        var loader = new PCDLoader();

        var _self = this;
        loader.load( this.frameInfo.get_pcd_path(), 
            //ok
            function ( pcd ) {
                
                
                

                _self.points_parse_time = new Date().getTime();
                console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "parse pionts ", _self.points_parse_time - _self.create_time, "ms");

                // if (_self.frameInfo.transform_matrix){

                //     var arr = position;
                //     var num = position.length;
                //     var ni = 3;

                //     for (var i=0; i<num/ni; i++){
                //         var np = _self.frameInfo.transform_point(_self.frameInfo.transform_matrix, arr[i*ni+0], arr[i*ni+1], arr[i*ni+2]);
                //         arr[i*ni+0]=np[0];
                //         arr[i*ni+1]=np[1];
                //         arr[i*ni+2]=np[2];
                //     }

                //     //points.geometry.computeBoundingSphere();
                // }

                
                if (_self.data.cfg.enableFilterPoints)// do some filtering work here
                {
                    pcd = _self.remove_high_ponts(pcd, _self.data.cfg.filterPointsZ);
                }

                

                
                
                let position = pcd.position;


                // build geometry
                _self.world.data.dbg.alloc();
                var geometry = new THREE.BufferGeometry();
                if ( position.length > 0 ) 
                    geometry.setAttribute( 'position', new THREE.Float32BufferAttribute( position, 3 ) );

                let normal = pcd.normal;
                // normal and colore are note used in av scenes.
                if ( normal.length > 0 ) 
                    geometry.setAttribute( 'normal', new THREE.Float32BufferAttribute( normal, 3 ) );
                
                let color = pcd.color;
                if ( color.length == 0 ) {
                    color = []

                    // by default we set all points to same color
                    for (let i =0; i< position.length; ++i){                                
                        color.push(_self.data.cfg.point_brightness);                                
                    }


                    // if enabled intensity we color points by intensity.
                    if (_self.data.cfg.color_points=="intensity" && pcd.intensity.length>0){
                        // map intensity to color
                        for (var i =0; i< pcd.intensity.length; ++i){
                            let intensity = pcd.intensity[i];
                            intensity *= 8;
                            
                            if (intensity > 1)
                                intensity = 1.0;
                            
                            
                            //color.push( 2 * Math.abs(0.5-intensity));
                            
                            color[i*3] =  intensity;
                            color[i*3+1] = intensity;
                            color[i*3+2] = 1 - intensity; 
                        }
                    }

                    // save color, in case color needs to be restored.
                    pcd.color = color;
                }

                geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(color, 3 ) );

                geometry.computeBoundingSphere();
                // build material

                var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );

                /*
                
                if ( color.length > 0 ) {
                    material.vertexColors = color;
                } else {
                    //material.color.setHex(0xffffff);
                    material.color.r = 0.6;
                    material.color.g = 0.6;
                    material.color.b = 0.6;
                }
                */

                //material.size = 2;
                material.sizeAttenuation = false;

                // build mesh

                var mesh = new THREE.Points( geometry, material );                        
                mesh.name = "pcd";

                //return mesh;
                // add to parent.
                _self.world.webglGroup.add(mesh);
                
                _self.points = mesh;
                _self.pcd = pcd;
                //_self.points_backup = mesh;

                _self.build_points_index();
                _self.points_load_time = new Date().getTime();

                console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "loaded pionts ", _self.points_load_time - _self.create_time, "ms");

                _self._afterPreload();
            },

            // on progress,
            function(){

            },

            // on error
            function(){
                //error
                console.log("load pcd failed.");
                _self._afterPreload();
            },

            // on file loaded
            function(){
                _self.points_readfile_time = new Date().getTime();
                console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "read file ", _self.points_readfile_time - _self.create_time, "ms");
            }
        );
    };

    this.deleteAll = function(){
        return this.remove_all_points();
    }

    this._afterPreload = function(){
        this.preloaded = true;
        console.log("lidar preloaded");
        //go ahead, may load picture
        if (this.on_preload_finished){
            this.on_preload_finished();
        }                
        if (this.go_cmd_received){
            this.go(this.webglScene, this.on_go_finished);
        }
    };


    this.loaded = false;
    this.webglScene  = null;
    this.go_cmd_received = false;
    this.on_go_finished = null;

    this.go = function(webglScene, on_go_finished){
        this.webglScene = webglScene;

        if (this.preloaded){
            
            if (!this.world.data.cfg.show_background){
                this.hide_background();
            }

            
            if (this.data.cfg.color_obj != "no"){
                this.color_objects();
            }

            if (on_go_finished)
                on_go_finished();
        } else {
            this.go_cmd_received = true;
            this.on_go_finished = on_go_finished;
        }
    };

    this.unload = function(){
        this.cancel_highlight();

        if (this.points){
            // this.world.webglGroup.remove(this.points);

            // if (this.points.points_backup){
            //     let backup = this.points.points_backup;
            //     this.points.points_backup = null;
            //     this.remove_all_points();
            //     this.points = backup;

            // }
        }
    };

    this.deleteAll = function(){
        this.remove_all_points();
    }

    this.set_point_size=function(v){
        if (this.points){
            this.points.material.size = v;

            // this could happen if the points are still loading
            if (this.points.points_backup){
                this.points.points_backup.material.size = v;

                if (this.points.points_backup.points_backup){
                    this.points.points_backup.points_backup.material.size = v;
                }
            }
        }

    };

    this.color_objects = function(){
        if (this.data.cfg.color_obj != "no"){
            this.world.annotation.boxes.map((b)=>{
                if (!b.annotator)
                    this.set_box_points_color(b);
            })
        }
    };

    // color points according to object category
    this.color_points=function(){
        // color all points inside these boxes
        let color = this.points.geometry.getAttribute("color").array;

        // step 1, color all points.
        if (this.data.cfg.color_points=="intensity" && this.pcd.intensity.length>0){
            // by intensity
            for (var i =0; i< this.pcd.intensity.length; ++i){
                let intensity = this.pcd.intensity[i];
                intensity *= 8;
                
                if (intensity > 1)
                    intensity = 1.0;
                
                
                //color.push( 2 * Math.abs(0.5-intensity));
                
                color[i*3] =  intensity;
                color[i*3+1] = intensity;
                color[i*3+2] = 1 - intensity; 
            }
        }
        else
        {
            // mono color
            for (let i =0; i< this.pcd.position.length; ++i){                                
                color[i] = this.data.cfg.point_brightness;
            }
        }

        // step 2 color objects
        this.color_objects();
        
        //this.update_points_color();
    };

    this.transformPointsByEgoPose = function(points){

        if (!this.world.transLidar)
            return points;


        let newPoints=[];
        for (let i=0; i<points.length; i+=3)
        {
            let p = matmul(this.world.transLidar, [points[i], points[i+1], points[i+2], 1], 4);
            newPoints.push(p[0]);
            newPoints.push(p[1]);
            newPoints.push(p[2]);
        }
        return newPoints;
    }

   
    this.get_all_pionts=function(){
        return this.points.geometry.getAttribute("position");
    };
    
    this.computeCenter = function(){

        if (! this.center)
        {
            let position = this.points.geometry.getAttribute("position");
            // computer center position
            let center = {x:0,y:0,z:0};
            for (let i = 0; i<position.count; i++)
            {
                center.x += position.array[i*3];
                center.y += position.array[i*3+1];
                center.z += position.array[i*3+2];
            }

            center.x /= position.count;
            center.y /= position.count;
            center.z /= position.count;

            this.center = center;
        }

        return this.center;
    };

    this.build_points_index=function(){
        var ps = this.points.geometry.getAttribute("position");
        var points_index = {};

        if (ps){ // points may be empty
            for (var i = 0; i<ps.count; i++){
                var k = this.get_position_key(ps.array[i*3], ps.array[i*3+1], ps.array[i*3+2]);
                k = this.key_to_str(k);

                if (points_index[k]){
                    points_index[k].push(i);
                } else {
                    points_index[k]=[i];
                }                    
            }
        }

        this.points.points_index = points_index;
    };

    this.points_index_grid_size= 1,

    this.get_position_key=function(x,y,z){
        return [Math.floor(x/this.points_index_grid_size),
                Math.floor(y/this.points_index_grid_size),
                Math.floor(z/this.points_index_grid_size),];
    };
    this.key_to_str=function(k){
        return k[0]+","+k[1]+","+k[2];
    };

    // candidate pionts, covering the box(center, scale), but larger.
    this.get_covering_position_indices=function(points, center, scale, rotation, scale_ratio){
        /*
        var ck = this.get_position_key(center.x, center.y, center.z);
        var radius = Math.sqrt(scale.x*scale.x + scale.y*scale.y + scale.z*scale.z)/2;
        var radius_grid = Math.ceil(radius/this.points_index_grid_size);// + 1;

        var indices = [];
        for(var x = -radius_grid; x <= radius_grid; x++){
            for(var y = -radius_grid; y <= radius_grid; y++){
                for(var z = -radius_grid; z <= radius_grid; z++){
                    var temp = points.points_index[this.key_to_str([ck[0]+x, ck[1]+y, ck[2]+z])];
                    if (temp)
                        indices = indices.concat(temp);
                }
            }
        }

        console.log("found indices 1: " + indices.length);
        //return indices;
        */

        if (typeof(scale_ratio) == "number"){
            scale_ratio = {
                x: scale_ratio,
                y: scale_ratio,
                z: scale_ratio,
            };
        };

        var indices=[];
        
        var scaled_scale = {
            x: scale.x*scale_ratio.x,
            y: scale.y*scale_ratio.y,
            z: scale.z*scale_ratio.z,
        }

        var box_corners = psr_to_xyz(center, scaled_scale, rotation);
        var extreme = array_as_vector_range(box_corners, 4);

        var indices = [];
        for(var x = Math.floor(extreme.min[0]/this.points_index_grid_size); x <= Math.floor(extreme.max[0]/this.points_index_grid_size); x++){
            for(var y = Math.floor(extreme.min[1]/this.points_index_grid_size); y <= Math.floor(extreme.max[1]/this.points_index_grid_size); y++){
                for(var z = Math.floor(extreme.min[2]/this.points_index_grid_size); z <= Math.floor(extreme.max[2]/this.points_index_grid_size); z++){
                    var temp = points.points_index[this.key_to_str([x, y, z])];
                    if (temp)
                        indices = indices.concat(temp);
                }
            }
        }

        //console.log("found indices 2: " + indices.length);
        return indices;
    };

    this.toggle_background=function(){
        if (this.points.points_backup){ // cannot differentiate highlighted-scene and no-background-whole-scene
            this.cancel_highlight();
            return;
        } 
        else{
            this.hide_background();
        }
    };

    // hide all points not inside any box
    this.hide_background=function(){
        if (this.points.points_backup){
            //already hidden, or in highlight mode
            return;
        }

        var _self = this;
        var pos = this.points.geometry.getAttribute("position");
        var color = this.points.geometry.getAttribute("color");

        var hl_point=[];
        var hl_color=[];
        var highlight_point_indices = [];
        this.world.annotation.boxes.forEach(function(box){
            var indices= _self._get_points_index_of_box(_self.points, box, 1);

            indices.forEach(function(i){
                hl_point.push(pos.array[i*3]);
                hl_point.push(pos.array[i*3+1]);
                hl_point.push(pos.array[i*3+2]);

                hl_color.push(color.array[i*3]);
                hl_color.push(color.array[i*3+1]);
                hl_color.push(color.array[i*3+2]);
            })

            highlight_point_indices = highlight_point_indices.concat(indices);
        });
        

        // build new geometry
        this.world.data.dbg.alloc();
        var geometry = new THREE.BufferGeometry();
        
        if (hl_point.length > 0 ) {
            geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(hl_point, 3 ) );
            geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(hl_color, 3 ) );
        }
        
        
        geometry.computeBoundingSphere();               

        var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );

        material.sizeAttenuation = false;

        var mesh = new THREE.Points( geometry, material );                        
        mesh.name = "pcd";
        mesh.points_backup = this.points;
        mesh.highlight_point_indices = highlight_point_indices;

        //swith geometry
        this.world.webglGroup.remove(this.points);

        this.points = mesh;       
        this.build_points_index();         
        this.world.webglGroup.add(mesh);
    };

    this.cancel_highlight=function(box){
        if (this.points && this.points.points_backup){
            
            this.world.annotation.set_box_opacity(this.data.cfg.box_opacity);

            //copy colors, maybe changed.
            if (this.data.cfg.color_obj != "no"){
                var highlight_point_color = this.points.geometry.getAttribute("color");
                var backup_point_color = this.points.points_backup.geometry.getAttribute("color");                    
                
                this.points.highlight_point_indices.forEach(function(n, i){
                    backup_point_color.array[n*3] = highlight_point_color.array[i*3];
                    backup_point_color.array[n*3+1] = highlight_point_color.array[i*3+1];
                    backup_point_color.array[n*3+2] = highlight_point_color.array[i*3+2];
                });
            }


            //switch
            var points_backup = this.points.points_backup;
            this.points.points_backup = null;
            
            this.world.webglGroup.remove(this.points);
            this.remove_all_points(); //this.points is null now
            this.points = points_backup;
            
            if (box){
                // in highlighted mode, the box my be moved outof the highlighted area, so 
                // we need to color them again.
                if (this.data.cfg.color_obj != "no")
                    this.set_box_points_color(box);
            }

            if (this.data.cfg.color_obj != "no")
                this.update_points_color();
                
            this.world.webglGroup.add(this.points);
        }
    };

    this.reset_points=function(points){  // coordinates of points
        
        
        this.world.data.dbg.alloc();
        var geometry = new THREE.BufferGeometry();
        
        
        geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(points, 3 ) );
        geometry.computeBoundingSphere();               

        var material = new THREE.PointsMaterial( { size: this.data.cfg.point_size} );

        material.sizeAttenuation = false;

        var mesh = new THREE.Points( geometry, material );                        
        mesh.name = "pcd";

        //swith geometry
        this.world.webglGroup.remove(this.points);
        this.remove_all_points();

        this.points = mesh;
        this.world.webglGroup.add(mesh);
    };

    this.highlight_box_points=function(box){
        if (this.points.highlighted_box){
            //already highlighted.
            return;
        }

        
        // hide all other boxes
        this.world.annotation.set_box_opacity(0);

        // keep myself
        box.material.opacity = 1;


        var _self = this;
        var pos = this.points.geometry.getAttribute("position");
        var color = this.points.geometry.getAttribute("color");

                        


        var hl_point=[];
        var hl_color=[];

        var highlight_point_indices= this._get_points_index_of_box(this.points, box, 3);

        highlight_point_indices.forEach(function(i){
            hl_point.push(pos.array[i*3]);
            hl_point.push(pos.array[i*3+1]);
            hl_point.push(pos.array[i*3+2]);

            hl_color.push(color.array[i*3]);
            hl_color.push(color.array[i*3+1]);
            hl_color.push(color.array[i*3+2]);
        })
        

        // build new geometry
        this.world.data.dbg.alloc();
        var geometry = new THREE.BufferGeometry();
        
        if (hl_point.length > 0 ) {
            geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(hl_point, 3 ) );
            geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(hl_color, 3 ) );
        }
        
        
        geometry.computeBoundingSphere();               

        var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );

        material.sizeAttenuation = false;

        var mesh = new THREE.Points( geometry, material );                        
        mesh.name = "highlighted_pcd";

        //swith geometry
        this.world.webglGroup.remove(this.points);

        mesh.points_backup = this.points;
        mesh.highlight_point_indices = highlight_point_indices;
        mesh.highlighted_box = box;

        this.points = mesh;
        this.build_points_index();
        this.world.webglGroup.add(mesh);
    };

    this.get_points_indices_of_box=function(box){
        return this._get_points_of_box(this.points, box, 1).index;
    };

    this.get_points_of_box_in_box_coord=function(box){
        return this._get_points_of_box(this.points, box, 1).position;
    };

    // IMPORTANT
    // ground plane affects auto-adjustment
    // we don't count in the ponits of lowest part to reduce the affection.
    // note how the 'lower part' is defined, we count 
    // lowest_part_type has two options: lowest_point, or lowest_box
    this.get_points_dimmension_of_box=function(box, use_box_bottom_as_limit){
        var p = this._get_points_of_box(this.points, box, 1).position;  //position is relative to box coordinates

        var lowest_limit = - box.scale.z/2;

        if (!use_box_bottom_as_limit){
            var extreme1 = vector_range(p, 3);
            lowest_limit = extreme1.min[2];
        }
        

        //filter out lowest part
        var p = p.filter(function(x){
            return x[2] - settings.ground_filter_height > lowest_limit;
        })

        //compute range again.
        var extreme2 = vector_range(p, 3);

        return {
            max:{
                x: extreme2.max[0],
                y: extreme2.max[1],
                z: extreme2.max[2],
            },
            min:{
                x: extreme2.min[0],
                y: extreme2.min[1],
                z: lowest_limit,
            }
        }
    };

    // given points and box, calculate new box scale
    this.get_dimension_of_points=function(indices, box){
        var p = this._get_points_of_box(this.points, box, 1, indices).position;                
        var extreme1 = vector_range(p, 3);

        //filter out lowest part, to calculate x-y size.
        var p = p.filter(function(x){
            return x[2] - settings.ground_filter_height > extreme1.min[2];
        })

        //compute range again.
        var extreme2 = vector_range(p, 3);

        return {
            max:{
                x: extreme2.max[0],
                y: extreme2.max[1],
                z: extreme1.max[2],   // orignal extreme.
            },
            min:{
                x: extreme2.min[0],
                y: extreme2.min[1],
                z: extreme1.min[2],
            }
        }
    };

    //centered, but without rotation
    this.get_points_relative_coordinates_of_box_wo_rotation=function(box, scale_ratio){
        return this._get_points_of_box(this.points, box, scale_ratio).position_wo_rotation;
    };


    this.get_points_of_box=function(box, scale_ratio){
        return this._get_points_of_box(this.points, box, scale_ratio);
    };


    this.get_points_relative_coordinates_of_box=function(box, scale_ratio){
        var ret = this._get_points_of_box(this.points, box, scale_ratio);
        return ret.position;
    };


    this._get_points_index_of_box=function(points, box, scale_ratio){
        return this._get_points_of_box(points, box, scale_ratio).index;
    };

    // this 
    this._get_points_of_box=function(points, box, scale_ratio, point_indices){

        if (!scale_ratio){
            scale_ratio = 1;
        }
        var pos_array = points.geometry.getAttribute("position").array;

        
        var relative_position = [];
        var relative_position_wo_rotation = [];
        
        var r = box.rotation;
        var trans = transpose(euler_angle_to_rotate_matrix(r, {x:0, y:0, z:0}), 4);

        var indices=[];
        var cand_point_indices = point_indices;
        if (!point_indices)
        {
            cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, scale_ratio);
        }

        cand_point_indices.forEach(function(i){
        //for (var i  = 0; i < pos.count; i++){
            var x = pos_array[i*3];
            var y = pos_array[i*3+1];
            var z = pos_array[i*3+2];

            var p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];

            var tp = matmul(trans, p, 4);

            if (!point_indices){
                // if indices is provided by caller, don't filter
                if ((Math.abs(tp[0]) > box.scale.x/2 * scale_ratio+0.01) 
                    || (Math.abs(tp[1]) > box.scale.y/2 * scale_ratio+0.01)
                    || (Math.abs(tp[2]) > box.scale.z/2 *scale_ratio+0.01) ){
                    return;
                }

                indices.push(i);
            }
            
            relative_position.push([tp[0],tp[1],tp[2]]);
            relative_position_wo_rotation.push([p[0], p[1], p[2]])
            
        });
        
        //console.log("found indices: " + indices.length);

        return {
            index: indices,
            position: relative_position,
            position_wo_rotation: relative_position_wo_rotation,
        }
    };

    this.findTop = function(box, init_scale_ratio){
        var points = this.points;
        var pos_array = points.geometry.getAttribute("position").array;
        
        var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);


        var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
        // all cand points are translated into box coordinates

        let translated_cand_points = cand_point_indices.map(function(i){
            let x = pos_array[i*3];
            let y = pos_array[i*3+1];
            let z = pos_array[i*3+2];

            let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
            let tp = matmul(trans, p, 4);
            return tp;
        });


        let maxZ = -1000;

        
        translated_cand_points.forEach((tp, i)=>{
            if (Math.abs(tp[0]) < box.scale.x * init_scale_ratio.x/2 && 
                Math.abs(tp[1]) < box.scale.y * init_scale_ratio.y/2 && 
                Math.abs(tp[2]) < box.scale.z * init_scale_ratio.z/2)
            {
                if (tp[2] > maxZ)                    
                    maxZ = tp[2];
            }
        });

        return maxZ;
    }

    // find bottom and top points, in range of init_scale_ratio
    this.findBottom = function(box, init_scale_ratio){
        
        var points = this.points;
        var pos_array = points.geometry.getAttribute("position").array;
        
        var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);


        var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
        // all cand points are translated into box coordinates

        let translated_cand_points = cand_point_indices.map(function(i){
            let x = pos_array[i*3];
            let y = pos_array[i*3+1];
            let z = pos_array[i*3+2];

            let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
            let tp = matmul(trans, p, 4);
            return tp;
        });


        let minZ = 1000;

        
        translated_cand_points.forEach((tp, i)=>{
            if (Math.abs(tp[0]) < box.scale.x * init_scale_ratio.x/2 && 
                Math.abs(tp[1]) < box.scale.y * init_scale_ratio.y/2 && 
                Math.abs(tp[2]) < box.scale.z * init_scale_ratio.z/2)
            {
                if (tp[2] < minZ)                    
                    minZ = tp[2];
            }
        });

        return minZ;
    };

    this.grow_box = function(box, min_distance, init_scale_ratio){
        console.log("grow box, min_distance", min_distance, box.scale, init_scale_ratio);
        let start_time = new Date().getTime();
        var points = this.points;
        var pos_array = points.geometry.getAttribute("position").array;
        
        var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);


        var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
        
        //todo: different definition.
        let groundLevel = 0.3;

        if (this.data.cfg.enableDynamicGroundLevel)
        {
            groundLevel = Math.min(box.scale.z/3,  Math.max(0.2, box.scale.x/10, box.scale.y/10));
            console.log('ground level', groundLevel, box.scale);
        }
        

        // all cand points are translated into box coordinates

        let translated_cand_points = cand_point_indices.map(function(i){
                let x = pos_array[i*3];
                let y = pos_array[i*3+1];
                let z = pos_array[i*3+2];

                let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
                let tp = matmul(trans, p, 4);
                return tp;
            });



        let extreme= {
            max: {        
                x:-100000,
                y:-100000,
                z:-100000,
            },
    
            min: {        
                x:1000000,
                y:1000000,
                z:1000000,
            },
        };


        let inside_points = 0;
        translated_cand_points.forEach((tp, i)=>{
            if ((Math.abs(tp[0]) > box.scale.x/2+0.01) 
                || (Math.abs(tp[1]) > box.scale.y/2+0.01)
                || (Math.abs(tp[2]) > box.scale.z/2+0.01) ){
                
                
                return;
            } else{

                if ((box.scale.z < 0.6) || ((box.scale.z > 0.6) && (tp[2] > -box.scale.z/2 + groundLevel)))
                {
                    inside_points += 1;

                    if (tp[0] > extreme.max.x) {
                        extreme.max.x = tp[0];
                    } 
                    
                    if (tp[0] < extreme.min.x){
                        extreme.min.x = tp[0];
                    }
            
                    if (tp[1] > extreme.max.y){
                        extreme.max.y = tp[1];
                    }
                    
                    if (tp[1] < extreme.min.y){
                        extreme.min.y = tp[1];
                    }
                }
        
                if (tp[2] > extreme.max.z){
                    extreme.max.z = tp[2];
                }
                
                if (tp[2] < extreme.min.z){
                    extreme.min.z = tp[2];
                }

            }
        });

        if (inside_points < 10)  //too few points, give up.
        {
            return {
                max:{
                    x: box.scale.x/2,
                    y: box.scale.y/2,
                    z: box.scale.z/2,
                },
                min:{
                    x: -box.scale.x/2,
                    y: -box.scale.y/2,
                    z: -box.scale.z/2,
                }
            };
        }

        //let translated_cand_points_with_ground = translated_cand_points;

        // filter ground points
        // translated_cand_points = translated_cand_points.filter(function(tp, i){
        //     return tp[2] > -box.scale.z/2 + groundLevel;
        // });


        let extreme_adjusted = true;
        let loop_count = 0;
        while (extreme_adjusted){
            loop_count++;
            if (loop_count > 100000)
            {
                console.log("deep loops in grow_box");
                break;
            }

            extreme_adjusted = false;

            // x+
            let find_point = translated_cand_points.find(tp=>{
                return  tp[0] > extreme.max.x && tp[0] < extreme.max.x + min_distance/2 && 
                        tp[1] < extreme.max.y && tp[1] > extreme.min.y &&  
                        tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
            });

            if (find_point){
                extreme.max.x += min_distance/2;
                extreme_adjusted = true;
            }

            // x - 
            find_point = translated_cand_points.find(tp=>{
                return tp[0] < extreme.min.x && tp[0] > extreme.min.x - min_distance/2 && 
                       tp[1] < extreme.max.y && tp[1] > extreme.min.y  &&
                       tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
            });

            if (find_point){
                extreme.min.x -= min_distance/2;
                extreme_adjusted = true;
            }

            // y+
            find_point = translated_cand_points.find(tp=>{
                return tp[1] > extreme.max.y && tp[1] < extreme.max.y + min_distance/2 && 
                       tp[0] < extreme.max.x && tp[0] > extreme.min.x  &&
                       tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
            });

            if (find_point){
                extreme.max.y += min_distance/2;
                extreme_adjusted = true;
            }

            // y - 
            find_point = translated_cand_points.find(tp=>{
                return tp[1] < extreme.min.y && tp[1] > extreme.min.y - min_distance/2 && 
                       tp[0] < extreme.max.x && tp[0] > extreme.min.x  &&  
                       tp[2] < extreme.max.z && tp[2] > extreme.min.z  + groundLevel;
            });

            if (find_point){
                extreme.min.y -= min_distance/2;
                extreme_adjusted = true;
            }


            // z+
            find_point = translated_cand_points.find(tp=>{
                return tp[0] < extreme.max.x && tp[0] > extreme.min.x &&  
                       tp[1] < extreme.max.y && tp[1] > extreme.min.y &&                        
                       tp[2] > extreme.max.z && tp[2] < extreme.max.z + min_distance/2;
            });

            if (find_point){
                extreme.max.z += min_distance/2;
                extreme_adjusted = true;
            }

            // z- 
            find_point = translated_cand_points.find(tp=>{
                return tp[0] < extreme.max.x && tp[0] > extreme.min.x  &&  
                       tp[1] < extreme.max.y && tp[1] > extreme.min.y &&                        
                       tp[2] < extreme.min.z && tp[2] > extreme.min.z - min_distance/2;
            });

            if (find_point){
                extreme.min.z -= min_distance/2;
                extreme_adjusted = true;
            }

        }


        // refine extreme values
        //1 set initial value
        let refined_extreme= {
            max: {        
                x: extreme.max.x - min_distance/2,
                y: extreme.max.y - min_distance/2,
                z: extreme.max.z - min_distance/2,
            },
    
            min: {        
                x: extreme.min.x + min_distance/2,
                y: extreme.min.y + min_distance/2,
                z: extreme.min.z + min_distance/2,
            },
        };


        //2  find refined values.
        translated_cand_points.forEach(tp=>{
            if (tp[0] > extreme.max.x || tp[0] < extreme.min.x  || 
                tp[1] > extreme.max.y || tp[1] < extreme.min.y  || 
                tp[2] > extreme.max.z || tp[2] < extreme.min.z)
            {
            
            } 
            else{
                if (tp[0] > refined_extreme.max.x && tp[2] > extreme.min.z + groundLevel) {
                    refined_extreme.max.x = tp[0];
                } 
                
                if (tp[0] < refined_extreme.min.x && tp[2] > extreme.min.z + groundLevel){
                    refined_extreme.min.x = tp[0];
                }
        
                if (tp[1] > refined_extreme.max.y && tp[2] > extreme.min.z + groundLevel){
                    refined_extreme.max.y = tp[1];
                }
                
                if (tp[1] < refined_extreme.min.y && tp[2] > extreme.min.z + groundLevel){
                    refined_extreme.min.y = tp[1];
                }
        
                if (tp[2] > refined_extreme.max.z){
                    refined_extreme.max.z = tp[2];
                }
                
                if (tp[2] < refined_extreme.min.z){
                    refined_extreme.min.z = tp[2];
                }

            }
        });

        refined_extreme.min.z -= groundLevel;
        console.log("refined extreme", JSON.stringify(refined_extreme));
        return refined_extreme;
    }

    
    this.get_box_points_number=function(box){
        var indices = this._get_points_index_of_box(this.points, box, 1.0);
        return indices.length;
    };

    this.reset_box_points_color = function(box){
        let color = this.points.geometry.getAttribute("color").array;
        let indices = this._get_points_index_of_box(this.points, box, 1.0);
        if (this.data.cfg.color_points=="intensity")
        {        
            
            indices.forEach((i)=>{
                let intensity = this.pcd.intensity[i];
                intensity *= 8;
                
                if (intensity > 1)
                    intensity = 1.0;

                color[i*3] =  intensity;
                color[i*3+1] = intensity;
                color[i*3+2] = 1 - intensity; 
            });
                
        }
        else
        {
            indices.forEach((i)=>{
                color[i*3] =  this.data.cfg.point_brightness;
                color[i*3+1] = this.data.cfg.point_brightness;
                color[i*3+2] = this.data.cfg.point_brightness; 
            });
        }
    };


    this.set_box_points_color=function(box, target_color){
        //var pos = this.points.geometry.getAttribute("position");
        var color = this.points.geometry.getAttribute("color");

        if (!target_color){
            if (this.data.cfg.color_obj == "category")
            {
                target_color = globalObjectCategory.get_color_by_category(box.obj_type);
            }
            else if (this.data.cfg.color_obj == "id")// by id
            {
                let idx = (box.obj_track_id)?parseInt(box.obj_track_id): box.obj_local_id;
                target_color = globalObjectCategory.get_color_by_id(idx);
            }
            else // no color
            {

            }
        }

        if (target_color)
        {

            var indices = this._get_points_index_of_box(this.points, box, 1.0);
            indices.forEach(function(i){
                    color.array[i*3] = target_color.x;
                    color.array[i*3+1] = target_color.y;
                    color.array[i*3+2] = target_color.z;
            });
        }
        
    };

    this.set_spec_points_color=function(point_indices, target_color){
        //var pos = this.points.geometry.getAttribute("position");
        var color = this.points.geometry.getAttribute("color");
        
        point_indices.forEach(function(i){
            color.array[i*3] = target_color.x;
            color.array[i*3+1] = target_color.y;
            color.array[i*3+2] = target_color.z;
        });
    };

    // this is used when pointbrightness is updated.
    this.recolor_all_points = function(){
        this.set_points_color({
            x: this.data.cfg.point_brightness,
            y: this.data.cfg.point_brightness,
            z: this.data.cfg.point_brightness,
        });        
        this.color_points();  
        this.update_points_color();
    };

    // set all points to specified color
    this.set_points_color=function(target_color){
        var color = this.points.geometry.getAttribute("color");
        for (var i = 0; i<color.count; i++){
            color.array[i*3] = target_color.x;
            color.array[i*3+1] = target_color.y;
            color.array[i*3+2] = target_color.z;
        }
    };


    this.update_points_color=function(){
        if (this.points){ //some time points may fail to load.
            this.points.geometry.getAttribute("color").needsUpdate = true;
            //this.points.geometry.removeAttribute("color");
            //this.points.geometry.setAttribute("color", new THREE.Float32BufferAttribute(color.array, 3 ));
        }
    };


    this.remove_all_points=function(){
        if (this.points){
            this.world.data.dbg.free();
            this.points.geometry.dispose();
            this.points.material.dispose();
        
            
            if (this.points.points_backup){
                this.world.data.dbg.free();
                this.points.points_backup.geometry.dispose();
                this.points.points_backup.material.dispose();

                if (this.points.points_backup.points_backup){
                    this.world.data.dbg.free();
                    this.points.points_backup.points_backup.geometry.dispose();
                    this.points.points_backup.points_backup.material.dispose();
                    this.points.points_backup.points_backup = null;
                }

                this.points.points_backup = null;
            }
            

            this.points = null;
        }else {            
            console.error("destroy empty world!");
        }
    };



    this.select_points_by_view_rect=function(x,y,w,h, camera){
        var points = this.points;
        var pos_array = points.geometry.getAttribute("position").array;

        var indices = [];
        var points = [];
        var p = new THREE.Vector3();

        for (var i=0; i< pos_array.length/3; i++){
            p.set(pos_array[i*3], pos_array[i*3+1], pos_array[i*3+2]);
            p = this.world.lidarPosToScene(p);
            p.project(camera);
            //p.x = p.x/p.z;
            //p.y = p.y/p.z;
            //console.log(p);
            if ((p.x >= x) && (p.x <= x+w) && (p.y>=y) && (p.y<=y+h) && (p.z>0)){
                indices.push(i);
                points.push([pos_array[i*3], pos_array[i*3+1], pos_array[i*3+2]]);
            }
        }

        console.log("select rect points", indices.length);

        //this.set_spec_points_color(indices, {x:1,y:0,z:0});
        //this.update_points_color();

        return points;
    };

    this.get_centroid = function(point_indices){
        let points = this.points;
        let pos_array = points.geometry.getAttribute("position").array;

        let center ={
            x:0,y:0,z:0
        };

        point_indices.forEach(i=>{
            center.x += pos_array[i*3];
            center.y += pos_array[i*3+1];
            center.z += pos_array[i*3+2];
        });

        center.x /= point_indices.length;
        center.y /= point_indices.length;
        center.z /= point_indices.length;
        
        return center;

    };

    this.create_box_by_points = function(point_indices, camera){

        
        let indices = point_indices;
        let points = this.points;
        let pos_array = points.geometry.getAttribute("position").array;

        // todo: copied the following code from next function. refactor it!
        console.log("select rect points", indices.length);

        //compute center, no need to tranform to box coordinates, and can't do it in this stage.
        /*
        var extreme = array_as_vector_index_range(pos_array, 3, indices);

        var center = {
            x: (extreme.max[0]+extreme.min[0])/2,
            y: (extreme.max[1]+extreme.min[1])/2,
            z: (extreme.max[2]+extreme.min[2])/2,
        };
        */
        var rotation_z = camera.rotation.z + Math.PI/2;
        var trans = transpose(euler_angle_to_rotate_matrix({x:0,y:0,z:rotation_z}, {x:0, y:0, z:0}), 4);

        let center ={
            x:0,y:0,z:0
        };

        point_indices.forEach(i=>{
            center.x += pos_array[i*3];
            center.y += pos_array[i*3+1];
            center.z += pos_array[i*3+2];
        });

        center.x /= point_indices.length;
        center.y /= point_indices.length;
        center.z /= point_indices.length;
        center.z = 0;

        
        var relative_position = [];
        indices.forEach(function(i){
        //for (var i  = 0; i < pos.count; i++){
            var x = pos_array[i*3];
            var y = pos_array[i*3+1];
            var z = pos_array[i*3+2];
            var p = [x-center.x, y-center.y, z-center.z, 1];
            var tp = matmul(trans, p, 4);
            relative_position.push([tp[0],tp[1],tp[2]]);
        });

        var relative_extreme = vector_range(relative_position);
        var scale = {
            x: relative_extreme.max[0] - relative_extreme.min[0],
            y: relative_extreme.max[1] - relative_extreme.min[1],
            z: relative_extreme.max[2] - relative_extreme.min[2],
        };

        // enlarge scale a little


        // adjust center
        this.world.annotation.translate_box_position(center, rotation_z, "x", relative_extreme.min[0] + scale.x/2);
        this.world.annotation.translate_box_position(center, rotation_z, "y", relative_extreme.min[1] + scale.y/2);
        this.world.annotation.translate_box_position(center, rotation_z, "z", relative_extreme.min[2] + scale.z/2);
        

        scale.x += 0.02;
        scale.y += 0.02;
        scale.z += 0.02;

        return this.world.annotation.add_box(center, scale, {x:0,y:0,z:rotation_z}, "Unknown", "");
    };

}

export{Lidar}