/* The copyright in this software is being made available under the BSD
 * License, included below. This software may be subject to other third party
 * and contributor rights, including patent rights, and no such rights are
 * granted under this license.  
 *
 * Copyright (c) 2010-2012, ITU/ISO/IEC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  * Neither the name of the ITU/ISO/IEC nor the names of its contributors may
 *    be used to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 */



/*!
 ************************************************************************
 *  \file
 *     joint_view_filter.c
 *
 *  \brief
 *    Joint view filtering functions.
 *
 *  \author
 *      -liri           <lrzkd@mail.ustc.edu.cn>
 *		-suwenyi		<suwy@mail.ustc.edu.cn>
 *		-Dmytro			<Dmytro.Rusanovskyy@nokia.com>
 *
 ************************************************************************
 */




#include "global.h"
#include "memalloc.h"
#include "configfile.h"
#include "output.h"
#include "ViewSynAPI.h"
#include "joint_view_filter.h"
#include "image.h"
#include "depth_resample.h"

#if POZNAN_NONLINEAR_DEPTH
#include "nonlinear_depth.h"
#endif

#if JOINT_VIEW_FILTER


static void project_depth_to_left_view (double **depth, double **projected_depth, int **map_pixels, 
										double view_distance, VideoParameters*p_Vid);
static void project_depth_to_right_view(double **depth, double **projected_depth, int **map_pixels, 
										double view_distance, VideoParameters*p_Vid);
static void convert_depthmap_to_depth(imgpel **depth_map, double **depth, 
									  double depth_near, double depth_far,
									  int width, int height
#if POZNAN_NONLINEAR_DEPTH                                    
  ,int nonlinear_depth_num, char nonlinear_depth_points[256]
#endif 
);
static void convert_depth_to_depthmap(double **depth, imgpel **depth_map, 
									  double depth_near, double depth_far, 
									  int width, int height
#if POZNAN_NONLINEAR_DEPTH                                    
  ,int nonlinear_depth_num, char nonlinear_depth_points[256]
#endif  
);
static int  joint_view_filter_simple(imgpel ***input_depthmap, imgpel ***output_depthmap, 
									 VideoParameters*p_Vid, 
									 double deviation);  
static	void project_depth_to_right_view(double **depth, double **projected_depth, int **map_pixels, 
										 double view_distance, VideoParameters*p_Vid)   ;
static double adaptive_calc_thresholding(imgpel ***input_depthmap, int num_of_views, int height, int width);


static	void convert_depthmap_to_depth(imgpel **depth_map, double **depth, double depth_near, double depth_far, int width, int height
#if POZNAN_NONLINEAR_DEPTH                                    
  ,int nonlinear_depth_num, char nonlinear_depth_points[256]
#endif 
)
{
#if POZNAN_NONLINEAR_DEPTH
	int w, h;
	double inv_z = 1 / depth_near - 1 / depth_far;
	imgpel LUT_nonlinear_backward[NONLINEAR_DEPTH_LUT_SIZE];
	double LUT_double[256];
	nonlinear_depth_init_lut(LUT_nonlinear_backward, nonlinear_depth_num, nonlinear_depth_points, 0);

	for (w=0; w<256; ++w)  
		LUT_double[w] = 1 / ( LUT_nonlinear_backward[w]/255.0 * inv_z + 1 / depth_far );

	for (w = 0; w < width; w++)
		for (h = 0; h < height; h++)
			depth[h][w] = LUT_double[depth_map[h][w]];
#else
	int w, h;
	double inv_z = 1 / depth_near - 1 / depth_far;

	for (w = 0; w < width; w++)
		for (h = 0; h < height; h++)
			depth[h][w] = 1 / (depth_map[h][w] * inv_z / 255 + 1 / depth_far );
#endif
}


static	void convert_depth_to_depthmap(double **depth, imgpel **depthmap, double depth_near, double depth_far, int width, int height
#if POZNAN_NONLINEAR_DEPTH                                    
  ,int nonlinear_depth_num, char nonlinear_depth_points[256]
#endif  
)
{
	int w, h;
	double inv_z = 1 / depth_near - 1 / depth_far;
#if POZNAN_NONLINEAR_DEPTH    
	double d;
	double _1depth_far = 1.0/depth_far;
	double _255_inv_z = 255.0/inv_z;
	imgpel LUT_nonlinear_forward[NONLINEAR_DEPTH_LUT_SIZE];
	nonlinear_depth_init_lut(LUT_nonlinear_forward, nonlinear_depth_num, nonlinear_depth_points, 1);
#endif

	for (w = 0; w < width; w++)
		for (h = 0; h < height; h++)
#if POZNAN_NONLINEAR_DEPTH                                    
		{
			d = (1.0 / depth[h][w] - _1depth_far) * _255_inv_z;
			depthmap[h][w] = (imgpel)float2int((float)nonlinear_depth_lerp(d, LUT_nonlinear_forward));
		}
#else
			 depthmap[h][w] = (imgpel)float2int((float)(255 * (1 / depth[h][w] - 1 / depth_far) / inv_z));
#endif
}


/*
 *  Project depth to left view, source depth is stored in double **depth, the projected depth is stored in double **projected_depth
 */
static	void project_depth_to_left_view(double **depth, double **projected_depth, int **map_pixels, double view_distance, VideoParameters*p_Vid)
{
	int h, w;
	int height =p_Vid->height;
	int width = p_Vid->width;

	double focal_x=p_Vid->rec_focal_length_x[0];
	double depth_factor = focal_x * view_distance;

	//printf("%f\n",focal_x);

	
	for (h = 0; h < height; h++)
	{
		int  max_x = 0;
		for (w = 0; w < width; w++)
		{
			int disparity = (int)float2int((float)(depth_factor / depth[h][w]));
			if((w+disparity>=width)||(w+disparity<0))
				continue;

			if(max_x<w+disparity)
				max_x=w+disparity;
			else
				continue;

			if ((projected_depth[h][w + disparity] > depth[h][w]) ||(fabs(projected_depth[h][w + disparity])<=1e-5))
			{
				projected_depth[h][w + disparity] = depth[h][w];
				map_pixels[h][w+disparity]+=1;
			}
		}
	}// ends depth map projection
}


/*
 *  Project depth to right view, source depth is stored in double **depth, the projected depth is stored in double **projected_depth
 */
static	void project_depth_to_right_view(double **depth, double **projected_depth, int **map_pixels, double view_distance, VideoParameters*p_Vid)
{
	int h, w;
	int height = p_Vid->height;
	int width = p_Vid->width;

	//!<focal_x  among all views  has the same  value. 
	//double focal_x=p_Vid->p_Inp->IntrinsicMatrix[0][0][0];

	double focal_x=p_Vid->rec_focal_length_x[0];

	//double max_disp, min_depth;
	double depth_factor = focal_x * view_distance;

	for (h = height - 1; h >= 0; h--)
	{
		int max_x = width-1;
		int disparity, projected_x;
		for (w = width - 1; w >= 0; w--)
		{
			disparity = (int)float2int((float)(depth_factor / depth[h][w]));
			projected_x = w + disparity;

			if((projected_x>=width)||(projected_x<0))
				continue;

			if(max_x>projected_x)
				max_x=projected_x;
			else
				continue;

			if ((projected_depth[h][projected_x] > depth[h][w]) ||(fabs(projected_depth[h][projected_x])<=1e-5))
			{
				projected_depth[h][projected_x] = depth[h][w];
				map_pixels[h][projected_x] += 1;
			}
		}
	}  // ends depth map projection
}


// Apply jvf over viewid = view
static	void non_linear_denoise_simple(double **org_depth, double ***projected_depth, int ***map_pixels, int view, double **denoised_depth, VideoParameters*p_Vid, double deviation)
{
	InputParameters*p_Inp=p_Vid->p_Inp;
	int width  = p_Vid->width;
	int height = p_Vid->height;
	int num_of_views=p_Inp->NumOfViews;
	int h, w, v;
	double denoised_pixel;
	int num_pixels;
	//double min_z=0.0;

	for (h = 0; h < height; h++)
	{
		for (w = 0; w < width; w++)
		{
			denoised_pixel = org_depth[h][w];
			num_pixels = 1;


			for (v = 0; v < num_of_views; v++)
			{
				if ( v != view && map_pixels[v][h][w] > 0)
				{
					double delta = fabs(org_depth[h][w] - projected_depth[v][h][w]) / org_depth[h][w];
					if (255*delta < deviation)
					{
						denoised_pixel += projected_depth[v][h][w];
						num_pixels++;
					}
				}
			}
			denoised_depth[h][w] = denoised_pixel/num_pixels;
		}
	}
}

static	double adaptive_calc_thresholding(imgpel ***input_depthmap, int num_of_views, int height, int width)
{
	int count[256];
	int i, v, h, w, max, max_index;

	for (i = 0; i < 256; i++)
		count[i] = 0;

	for (v = 0; v < num_of_views; v++)
		for (h = 0; h < height; h++)
			for (w = 0; w < width; w++)
				count[ input_depthmap[v][h][w] ]++;

	max = count[0];
	max_index = 0;
	for (i = 0; i < 256; i++)
	{
		if (max < count[i])
		{
			max = count[i];
			max_index = i;
		}
	}


	return 0.101 * max_index + 2.3401;
}



/*
 *  function joint_view_filter
 *  input parameters:
 *  1. imgpel ***input_depthmap:   n views of input depth map data.
 *  2. imgpel ***output_depthmap:  n views of output depth map data.
 *  3. JvfParams jvf_params:     depth map parameters include camera parameters.
 *  4. deviation:                deviation used in depth map fusing process.
 *  return 0 means joint view filter failed.
 *  return 1 means joint view filter succeed.
 */
static	int joint_view_filter_simple(imgpel ***input_depthmap, imgpel ***output_depthmap,VideoParameters*p_Vid, double deviation)
{

	InputParameters*p_Inp=p_Vid->p_Inp;

	int num_of_views=p_Inp->NumOfViews;
	int width=p_Vid->width;
	int height=p_Vid->height;

	int view, i, h, w;
	const int NumIter = 2; // Iteration numbers of JVF
	int iter;

	double***depth=NULL;
	double***projected_depth=NULL;
	double***denoised_depth=NULL;
	int***map_pixels=NULL;
//#if POZNAN_NONLINEAR_DEPTH
//	VideoParameters *p_VidDepth = p_Vid->is_depth ? p_Vid : p_Vid->p_DualVid;
//#endif
	if(deviation<1e-5)
		return 1;//!<skip

	get_mem3Ddouble(&depth,num_of_views,   height,width);

	get_mem3Ddouble(&projected_depth,num_of_views,height,width);
	get_mem3Ddouble(&denoised_depth,num_of_views,height,width);
	get_mem3Dint(&map_pixels,num_of_views,height,width);

	for(view=0;view<num_of_views;view++)
	{
		double depth_near=p_Vid->rec_depth_near[view];
		double depth_far=p_Vid->rec_depth_far[view];
		convert_depthmap_to_depth(input_depthmap[view],depth[view],fabs(depth_near),fabs(depth_far),width,height
#if POZNAN_NONLINEAR_DEPTH                                    
			,p_Vid->NonlinearDepthNum, p_Vid->NonlinearDepthPoints
#endif
		);
	}

	for (iter = 0; iter < NumIter; iter++)
	{
		memset(&(denoised_depth[0][0][0]),0x00,(p_Inp->NumOfViews)*height*width*sizeof(double));
		for (view = 0; view < num_of_views; view++)
		{
			double trans_curr_view=p_Vid->rec_translation[view];
			
			memset(&(projected_depth[0][0][0]),0x00,(p_Inp->NumOfViews)*height*width*sizeof(double));
			memset(&(map_pixels[0][0][0]),0x00,(p_Inp->NumOfViews)*height*width*sizeof(int));
			for(i=0;i<num_of_views;++i)
			{
				double trans_pro_view=p_Vid->rec_translation[i];
				int left_view=0;
				if(p_Vid->rec_depth_near[view]>=0)
				{
					if(trans_pro_view<trans_curr_view)
						left_view=1;
					else
						left_view=0;

				}
				else
				{
					if(trans_pro_view>trans_curr_view)
						left_view=1;
					else
						left_view=0;

				}
				if(i==view)
					continue;

				if(left_view)
				{
					//!<project left view to current view
					project_depth_to_right_view(depth[i], projected_depth[i], map_pixels[i], -fabs(trans_curr_view-trans_pro_view), p_Vid);
				}
				else
				{
					//!<project  right view to current view
					project_depth_to_left_view(depth[i], projected_depth[i], map_pixels[i], fabs(trans_curr_view-trans_pro_view), p_Vid );
				}
			}
			non_linear_denoise_simple(depth[view], projected_depth, map_pixels, view, denoised_depth[view], p_Vid, deviation);
		}
		for (view = 0; view < num_of_views; view++)
		{
			for (h = 0; h < height; h++)
				for (w = 0; w < width; w++)
					depth[view][h][w] = denoised_depth[view][h][w];
		}

	}// iter

	for (view = 0; view < num_of_views; view++)
	{
		double depth_near=p_Vid->rec_depth_near[view];
		double depth_far=p_Vid->rec_depth_far[view];
		convert_depth_to_depthmap(depth[view], output_depthmap[view], fabs(depth_near),fabs( depth_far), width, height
#if POZNAN_NONLINEAR_DEPTH                                    
			,p_Vid->NonlinearDepthNum, p_Vid->NonlinearDepthPoints
#endif                                                                          
		);
	}


	// Release memory
	free_mem3Ddouble(depth);
	free_mem3Ddouble(projected_depth);
	free_mem3Ddouble(denoised_depth);
	free_mem3Dint(map_pixels);

	return 1;
}

#if (PRE_JVF)
void pre_joint_view_filter(VideoParameters*p_Vid)
{
	InputParameters*p_Inp=p_Vid->p_Inp;
	int  i;
	double deviation;
	int frame_width=p_Vid->width;
	int frame_height=p_Vid->height;
	imgpel***input_depth=NULL;


	assert(p_Vid->is_depth);
	input_depth=(imgpel***)malloc(p_Inp->NumOfViews*sizeof(imgpel**));
	if(input_depth==NULL)
		no_mem_exit("JointViewFilterImage: input_depth");
	input_depth[0]=p_Vid->imgData.frm_data[0];
	for(i=1;i<p_Inp->NumOfViews;++i)
	{
		input_depth[i]=p_Vid->pre_jvf_imgdata[i].frm_data[0];
	}

	if(p_Inp->PreJVFMode==2)
		deviation=7*adaptive_calc_thresholding(input_depth,p_Inp->NumOfViews,frame_height,frame_width);
	else
		deviation=p_Inp->ThresholdForPreJVF;
	joint_view_filter_simple(input_depth,input_depth,p_Vid,deviation);


	free(input_depth);
}

#endif

#if INLOOP_JVF
int inloop_joint_view_filter(VideoParameters *p_Vid, StorablePicture **DepthReconPicBuf)
{
	InputParameters *p_Inp = p_Vid->p_Inp;
	double deviation;
	imgpel ***input_depth = calloc(p_Inp->NumOfViews, sizeof(imgpel**));
	int view;

	assert(p_Vid->is_depth);
	for (view = 0; view < p_Inp->NumOfViews; view++)
	{
		input_depth[view] = DepthReconPicBuf[view]->imgY;
	}

	deviation=p_Vid->inloop_jvf_thres;

	joint_view_filter_simple(input_depth, input_depth, p_Vid, deviation);
	free(input_depth);

	return 1;
}


#endif

#endif

