/* @param i direction of incoming ray, unit vector
 * @param r direction of refraction ray, unit vector
 * @param normal unit vector
 * @param n1 refraction index
 * @param n2 refraction index
 *
 * reference: http://graphics.stanford.edu/courses/cs148-10-summer/docs/2006--degreve--reflection_refraction.pdf
 */
static double fresnel(const point3 r, const point3 l,
                      const point3 normal, double n1, double n2)
{
    /* TIR */
    if (length(l) < 0.99)
        return 1.0;
    double cos_theta_i = -dot_product(r, normal);
    double cos_theta_t = -dot_product(l, normal);
    double r_vertical_root = (n1 * cos_theta_i - n2 * cos_theta_t) /
                             (n1 * cos_theta_i + n2 * cos_theta_t);
    double r_parallel_root = (n2 * cos_theta_i - n1 * cos_theta_t) /
                             (n2 * cos_theta_i + n1 * cos_theta_t);
    return (r_vertical_root * r_vertical_root +
            r_parallel_root * r_parallel_root) / 2.0;
}

/* 
 *	Simple implementation: Prediction r[u,i] = Sigma U[u,k]* Vt[k,i] over f  
 */
void matrix_factorization(data_struct *training_set, REAL **U, REAL **Vt, int N, int M, int rank, int training_size){
	REAL lambda = 0.00002, regularizer=0.001;
	int epochs,i,f,user,item;
	REAL error,sq_err,prev_sqerr = 100.0;
	REAL pred, tmp, *cached_prods;

	cached_prods = safe_malloc(N*M*sizeof(*cached_prods));

	for(f=0; f<rank; f++){
		for(epochs=0; epochs < 800; epochs++){
			for(i=0; i<training_size; i++){
				user = training_set[i].u;
				item = training_set[i].i;
				if (f == 0)
					pred = dot_product(U,Vt,user,item,f,rank);
				else
					pred = cached_prods[user*M+item] + dot_product(U,Vt,user,item,f,rank);
				
				error = training_set[i].rate - pred;

				tmp = U[user][f];
				U[user][f] += lambda * (error * Vt[f][item] - regularizer * tmp);
				Vt[f][item] += lambda * (error * tmp - regularizer * Vt[f][item]);
			}
			sq_err = 0.0;
			for(i=0; i<training_size; i++){
				user = training_set[i].u;
				item = training_set[i].i;
				error = training_set[i].rate - dot_product(U,Vt,user,item,0,rank);
				sq_err += pow(error,2);
			}
			if(fabs(sq_err - prev_sqerr) < 1e-5){
				printf("Feat: %d Epochs: %d\n",f,epochs);
				break;
			}
			prev_sqerr = sq_err;
		}
		for(i=0; i<training_size; i++){
			user = training_set[i].u;
			item = training_set[i].i;
			if (f == 0)
				cached_prods[user*M+item] = U[user][f]*Vt[f][item];
			else
				cached_prods[user*M+item] += U[user][f]*Vt[f][item];
		}
	}
	free(cached_prods);
}

/**
 * put matrix vectors to list
 */
template <class ZT, class F> void GaussSieve<ZT, F>::add_mat_list(ZZ_mat<ZT> &B)
{
  Z_NR<ZT> t, current_norm;
  dot_product(best_sqr_norm, B[0], B[0]);
  ListPoint<ZT> *p;

  for (int i = 0; i < nr; ++i)
  {
    p = new_listpoint<ZT>(nc);
    matrix_row_to_list_point(B[i], p);

    // cout << "# [info] init: additing point ";
    // cout << p->v << endl;

    if (alg == 3)
      current_norm = update_p_3reduce(p);
    else if (alg == 2)
      current_norm = update_p_2reduce(p);
    else if (alg == 4)
      current_norm = update_p_4reduce(p);
    else
    {
      cout << " Error, only support 2-, 3- and 4-sieve" << endl;
      exit(1);
    }

    if ((current_norm < best_sqr_norm) && (current_norm > 0))
      // if ((current_norm < best_sqr_norm) )
      best_sqr_norm = current_norm;
  }
}

Int32 fxnTest1(UInt32 size, UInt32 *data)
{
    UInt32 start_indx, end_indx ;

#if CHATTER
    System_printf("fxnInit : Executing fxnTest1\n");
#endif

    FxnArgs *args = (FxnArgs *)((UInt32)data + sizeof(map_info_type));
    int* buffer1 = (int*)args->a ;
    int* buffer2 = (int*)args->b ;
    start_indx = args->start_indx;
    end_indx = args->end_indx;
    
    volatile int* result = (int*)BARRIER_CNTR_BASE + REDUCTION_OFFSET ;

    int i ;
    for(i=0 ; i<NUM_ITER ; i++) {
	    callBarrier(0, /*lock_id=*/4) ;
	    result[0] = 0 ;
	    callBarrier(1, /*lock_id=*/4) ;
	    dot_product(buffer1, buffer2, result, start_indx, end_indx) ;
	    callBarrier(2, /*lock_id=*/4) ;
    }

    return 1 ;
}

float CriminisiInpainting::ComputeDataTerm(const itk::Index<2>& queryPixel)
{
  try
  {
    FloatVector2Type isophote = this->IsophoteImage->GetPixel(queryPixel);
    FloatVector2Type boundaryNormal = this->BoundaryNormals->GetPixel(queryPixel);

    if(this->DebugMessages)
      {
      //std::cout << "Isophote: " << isophote << std::endl;
      //std::cout << "Boundary normal: " << boundaryNormal << std::endl;
      }
    // D(p) = |dot(isophote at p, normalized normal of the front at p)|/alpha

    vnl_double_2 vnlIsophote(isophote[0], isophote[1]);

    vnl_double_2 vnlNormal(boundaryNormal[0], boundaryNormal[1]);

    float dot = std::abs(dot_product(vnlIsophote,vnlNormal));

    float dataTerm = dot/this->Alpha;

    return dataTerm;
  }
  catch( itk::ExceptionObject & err )
  {
    std::cerr << "ExceptionObject caught in ComputeDataTerm!" << std::endl;
    std::cerr << err << std::endl;
    exit(-1);
  }
}

Quaternion
Quaternion::slerp(const Quaternion& o, float t) const
{
  /** Matze: I don't understand this code :-/ It's from
   * http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
   * Though the article recommends not to use slerp I see no code for the other
   * methods so I'll use slerp anyway
   */
  float dot = dot_product(o);

  const float DOT_THRESHOLD = 0.995f;
  if(dot > DOT_THRESHOLD) {
    // quaternions are too close, lineary interpolate them
    Quaternion result = *this + (o - *this)*t;
    result.normalize();
    return result;
  }
  
  dot = clamp(dot, -1 ,1); // robustness
  float theta_O = acosf(dot);
  float theta = theta_O * t;

  Quaternion v2 = o - (*this * dot);
  v2.normalize();

  return (*this * cosf(theta)) + (v2 * sinf(theta));
}

double hexbright::angle_change() {
  int* vec1 = vector(0);
  int* vec2 = vector(1);
  return angle_difference(dot_product(vec1, vec2),
                          magnitude(vec1),
                          magnitude(vec2));
}

//	Player::pre_collision
void Player::pre_collision (const Manifold& m) {
	const Vec2 HORIZ(1.0f, 0.0f);
	if ((dot_product (HORIZ, abs(m.normal))) > 0.5f) {
		player_body.static_friction = friction2;
		player_body.dynamic_friction = friction2;
	}
}

 /**
  * Returns the dot product of the specified arrays of doubles.
  * @param v1 First array.
  * @param v2 Second array.
  * @throw std::domain_error if the vectors are not the same size.
  */
 inline double dot_product(const std::vector<double>& v1,
                           const std::vector<double>& v2) {
   stan::math::check_matching_sizes("dot_product",
                                              "v1", v1,
                                              "v2", v2);
   return dot_product(&v1[0], &v2[0], v1.size());
 }

static bool find_third_point(int num_points, const double (*points)[3], int a, int b, int* p_c)
{
	const double* x1 = points[a];
	const double* x2 = points[b];

	double x2x1[3] = {x2[0] - x1[0], x2[1] - x1[1], x2[2] - x1[2]};
	double ns_x2x1 = norm_squared(x2x1);

	int bi = -1;
	double max_dist = 0.0;
	for (int i = 0;i<num_points;i++)
	{
		if (i == a || i == b)
			continue;

		const double* x0 = points[i];

		double x1x0[3] = {x1[0] - x0[0], x1[1] - x0[1], x1[2] - x0[2]};
		double dot = dot_product(x1x0, x2x1);
		double dist = (norm_squared(x1x0) * ns_x2x1 - dot*dot) / ns_x2x1;

		if (dist > max_dist)
		{
			max_dist = dist;
			bi = i;
		}
	}

	*p_c = bi;
	return max_dist > TOLERANCE;
}

vector<double> Network_t::feed_forward(vector<double> a)
{
	int i;
	vector<double> tmp;
	
	for (i = 0; i < this->biases.size(); ++i)
	{
		tmp.clear();
		int j;
		
		for (j = 0; j < this->biases[i].size(); ++j)
		{
			tmp.push_back( sigmoid( dot_product( a, weights[i][j] ) + biases[i][j] ) );
		}

		a = tmp;
	}

	// for (i = 0; i < a.size() ; ++i)
	// {
	// 	if(a.at(i) < 0.1)
	// 	{
	// 		a.at(i) = 0;
	// 	}
	// 	else if(a.at(i) > 0.9)
	// 	{
	// 		a.at(i) = 1;
	// 	}
	// }

	return a;
}

void  scene_nodes_translation_data::choose_normal_and_reduction(vector3 const&  camera_origin)
{
    if (m_x_down && !m_y_down && !m_z_down)
    {
        m_normal = vector3_unit_z();
        m_reduction = vector3_unit_x();
    }
    else if (!m_x_down && m_y_down && !m_z_down)
    {
        m_normal = vector3_unit_z();
        m_reduction = vector3_unit_y();
    }
    else if (!m_x_down && !m_y_down && m_z_down)
    {
        m_normal = std::fabsf(dot_product(vector3_unit_x(), camera_origin)) > std::fabsf(dot_product(vector3_unit_y(), camera_origin)) ?
                        vector3_unit_x() :
                        vector3_unit_y();
        m_reduction = vector3_unit_z();
    }
    else if (m_x_down && !m_y_down && m_z_down)
    {
        m_normal = vector3_unit_y();
        m_reduction = vector3_unit_y();
    }
    else if (!m_x_down && m_y_down && m_z_down)
    {
        m_normal = vector3_unit_x();
        m_reduction = vector3_unit_x();
    }
    else
    {
        m_normal = vector3_unit_z();
        m_reduction = vector3_unit_z();
    }
}

int along (struct edge *edg, double axis[3])
{
	int orn, k, sgn;
	double dt;
	double vect[3];
	struct vertex *vtx1, *vtx2;
	struct arc *a;
    struct cept *ex;

	a = edg -> arcptr;
	vtx1 = a -> vtx[0];
	vtx2 = a -> vtx[1];
	if (vtx1 == NULL || vtx2 == NULL) {
		ex = new_cept (PARAMETER_ERROR,  NULL_VALUE,  FATAL_SEVERITY);
		add_object (ex, VERTEX, "vtx1 or vtx2");
		add_function (ex, "along");
		add_source (ex, "msrender.c");
		return(0);
	}
	orn = edg -> orn;
	sgn = 1 - 2 * orn;

	for (k = 0; k < 3; k++)
		vect[k] = vtx2 -> center[k] - vtx1 -> center[k];
	dt = dot_product (vect, axis) * sgn;
	return (dt > 0.0);
}

void player_class::move()
{
    //position+=velocity;
    //std::cout<<velocity.x<<","<<velocity.y<<"\n";
    position=dot_product(position,velocity);
    //std::cout<<velocity.x<<","<<velocity.y<<"\n";
}

t_double3			color_diffused(t_scene *scene, t_surface *surface, t_vector ray)
{
	t_double3		color_hit;
	t_light			*light;
	int				light_nb;
	double			dot_light;
	t_surface		*light_intersect;
	t_double3		reflected;

	color_hit = (t_double3){0, 0, 0};
	light = scene->light;
	light_nb = 0;
	while (light)
	{
		light_intersect = is_in_light(surface, scene, light, &dot_light);
		if (light_intersect->object == NULL || light_intersect->distance > 0)
		{
			color_hit = v_plus_v(color_hit, color_mix(scale_v(light->color,
				dot_light), surface->object->gloss,
				// scale_v(surface->object->color, dot_light)));
				scale_v(surface->color, dot_light)));
			reflected = reflect(scale_v(normalize(v_minus_v(light->pos, surface->point)), -1), surface->normal);
			color_hit = v_plus_v(color_hit, scale_v(light->color, pow(max_double(0, -dot_product(reflected, ray.dir) * surface->object->gloss), 2)));
		}
		free(light_intersect);
		light_nb++;
		light = light->next;
	}
	if (light_nb > 1)
		color_hit = scale_v(color_hit, (1.0 / (double)light_nb));
	return (color_hit);
}

vnl_vector_fixed<float,3> TrackingHandlerPeaks::ProposeDirection(const itk::Point<float, 3>& pos, std::deque<vnl_vector_fixed<float, 3> >& olddirs, itk::Index<3>& oldIndex)
{
  // CHECK: wann wird wo normalisiert
  vnl_vector_fixed<float,3> output_direction; output_direction.fill(0);

  itk::Index<3> index;
  m_DummyImage->TransformPhysicalPointToIndex(pos, index);

  vnl_vector_fixed<float,3> oldDir; oldDir.fill(0.0);
  if (!olddirs.empty())
    oldDir = olddirs.back();
  float old_mag = oldDir.magnitude();

  if (!m_Interpolate && oldIndex==index)
    return oldDir;

  output_direction = GetDirection(pos, m_Interpolate, oldDir);
  float mag = output_direction.magnitude();

  if (mag>=m_PeakThreshold)
  {
    output_direction.normalize();
    float a = 1;
    if (old_mag>0.5)
      a = dot_product(output_direction, oldDir);
    if (a>=m_AngularThreshold)
      output_direction *= mag;
    else
      output_direction.fill(0);
  }
  else
    output_direction.fill(0);

  return output_direction;
}

void dot_product_v(vector_t * vec1, vector_t * vec2, double * results, int count)
{
    for (int i = 0; i < count; ++i)
    {
        results[i] = dot_product(&(vec1[i]), &(vec2[i]));
    }
}

/* Compute how many planes of the hull of the given set of points
   the given witness point lies outside of.
   */
static int num_outside( int npts, double errin, double * errout,
	      REAL (*pts)[DIM], struct transf * t, REAL witness[DIM])
{
   double val, worst;
   int f, nf, nv, nbad;
   double trans_pts[MAX_POINTS][3];
   double faces[2*MAX_POINTS][4];

   if ( t!=0 )
     transform_hull( npts, pts, trans_pts, t);

   /* construct the hull */
   
   nv = sac_qhull( npts, ( t==0 ) ? pts : trans_pts,
		   (double (*)[3]) 0, (int *) 0, (int *) 0,
		   &nf, faces);

   nbad = 0;
   for ( f=0 ; f<nf ; f++ )
     {
       val = dot_product( witness, faces[f]) + faces[f][3];
       if ( val > errin + ZETA ) {
	 if ( ( nbad==0 ) || ( val > worst ) )
	   worst = val;
	 nbad++;
       }
     }
   *errout = ( nbad>0 ) ? worst : 0.0;

   return nbad;
   }

t_ray	find_refract_vect(t_ray *start_ray, t_hit drawn_pixel, double c_r, int test)
{
	double	ref_refract;
	double	new_ref_index;
	t_ray	res;	
	t_vec	new_norm;
	t_vec	inv_dir;

	res.pos = vec_add(start_ray->pos, scalar_product(start_ray->dir, drawn_pixel.t));
	inv_dir = scalar_product(start_ray->dir, -1);
	drawn_pixel.point_norm = scalar_product(drawn_pixel.point_norm, test);
	ref_refract = dot_product(drawn_pixel.point_norm, inv_dir);
	ref_refract /= (get_length(drawn_pixel.point_norm) * get_length(inv_dir));
	new_ref_index =	1 - c_r * c_r * (1 - ref_refract * ref_refract);
	if (new_ref_index > 0)
	{
		new_ref_index = sqrt(new_ref_index);
		new_ref_index = c_r * ref_refract - new_ref_index;
		new_norm = scalar_product(drawn_pixel.point_norm, new_ref_index);
		res.dir = scalar_product(inv_dir, c_r);
		res.dir = vec_sub(res.dir, new_norm); 
	}
	else
		res.dir = init_vector(0, 0, 0);
	return (res);
}

  /*
   * The viscosity is computed using the Wilke mixture rule.
   * \f[
   * \mu = \sum_k \frac{\mu_k X_k}{\sum_j \Phi_{k,j} X_j}.
   * \f]
   * Here \f$ \mu_k \f$ is the viscosity of pure species \e k,
   * and 
   * \f[
   * \Phi_{k,j} = \frac{\left[1 
   * + \sqrt{\left(\frac{\mu_k}{\mu_j}\sqrt{\frac{M_j}{M_k}}\right)}\right]^2}
   * {\sqrt{8}\sqrt{1 + M_k/M_j}}
   * \f] 
   * @see updateViscosity_T();
   */ 
  doublereal LiquidTransport::viscosity() {
        
    update_temp();
    update_conc();

    if (m_visc_mix_ok) return m_viscmix;
  
    // update viscSpecies_[] and m_phi[] if necessary
    if (!m_visc_temp_ok) {
      updateViscosity_temp();
    }

    if (!m_visc_conc_ok) {
      updateViscosities_conc();
    }

    if (viscosityModel_ == LVISC_CONSTANT) {
      return m_viscmix;
    } else if (viscosityModel_ == LVISC_MIXTUREAVG) {
      m_viscmix = dot_product(viscSpecies_, m_molefracs);
    } else if (viscosityModel_ == LVISC_WILKES) {
      multiply(m_phi, DATA_PTR(m_molefracs), DATA_PTR(m_spwork));
      m_viscmix = 0.0;
      for (int k = 0; k < m_nsp; k++) {
	m_viscmix += m_molefracs[k] * viscSpecies_[k]/m_spwork[k]; 
      }
    }
    
    return m_viscmix;
  }