// Copyright 2014 BVLC and contributors.

#include <algorithm>
#include <cmath>
#include <cfloat>
#include <vector>

#include "caffe/layer.hpp"
#include "caffe/vision_layers.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/io.hpp"
#include "caffe/filler.hpp"

using std::max;

namespace caffe {

template <typename Dtype>
void EuclideanThresholdLayer<Dtype>::LayerSetUp(
  const vector<Blob<Dtype>*>& bottom, vector<Blob<Dtype>*>* top) {
  CHECK_EQ(bottom[0]->channels(), bottom[1]->channels());
  CHECK_EQ(bottom[0]->height(), bottom[1]->height());
  CHECK_EQ(bottom[0]->width(), bottom[1]->width());
  M_ = bottom[0]->num();
  K_ = bottom[0]->count() / bottom[0]->num();
  diff_.Reshape(bottom[0]->num(), bottom[0]->channels(),
      bottom[0]->height(), bottom[0]->width());
  (*top)[0]->Reshape(bottom[0]->num(), 1, 1, 1);
  b1norm.Reshape(bottom[0]->num(), 1, 1, 1);
  b2norm.Reshape(bottom[0]->num(), 1, 1, 1);
  if (this->blobs_.size() > 0) {
      LOG(INFO) << "Skipping parameter initialization";
  } else {
      this->blobs_.resize(1);
    // Intialize the bias
      this->blobs_[0].reset(new Blob<Dtype>(1, 1, 1, 1));
      shared_ptr<Filler<Dtype> > bias_filler(GetFiller<Dtype>(
          this->layer_param_.euclidean_threshold_param().bias_filler()));
      bias_filler->Fill(this->blobs_[0].get());
      Dtype* bias_data = this->blobs_[0]->mutable_cpu_data();
      bias_data[0]=1e+6;
  }  // parameter initialization
}

template <typename Dtype>
void EuclideanThresholdLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      vector<Blob<Dtype>*>* top) {

}

template <typename Dtype>
void EuclideanThresholdLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    vector<Blob<Dtype>*>* top) {
	int count = bottom[0]->count();
	const Dtype* bottom_data1=bottom[0]->cpu_data();
	const Dtype* bottom_data2=bottom[1]->cpu_data();
	//const Dtype* label=bottom[2]->cpu_data();
	//LOG(INFO) << "bottom1(0):" << bottom_data1[0] << ", bottom1(1):" << bottom_data1[1] << ", bottom1(2):" << bottom_data1[2];
	//LOG(INFO) << "bottom2(0):" << bottom_data2[0] << ", bottom2(1):" << bottom_data2[1] << ", bottom2(2):" << bottom_data2[2];
	/*Blob<Dtype> b1normal, b2normal;
	b1normal.Reshape(bottom[0]->num(), bottom[0]->channels(),
		      bottom[0]->height(), bottom[0]->width());
	b2normal.Reshape(bottom[0]->num(), bottom[0]->channels(),
		      bottom[0]->height(), bottom[0]->width());
	Dtype* b1normdata=b1normal.mutable_cpu_data();
	Dtype* b2normdata=b2normal.mutable_cpu_data();
	Dtype* b1ndata=b1norm.mutable_cpu_data();
	Dtype* b2ndata=b2norm.mutable_cpu_data();
	for (int i=0;i<M_;i++)
	{
		b1ndata[i]= caffe_cpu_dot(K_, bottom_data1+(i*K_), bottom_data1+(i*K_));
		b2ndata[i]= caffe_cpu_dot(K_, bottom_data2+(i*K_), bottom_data2+(i*K_));
	}
	caffe_sqr(M_,b1ndata,b1ndata);
	caffe_sqr(M_,b2ndata,b2ndata);
	const Dtype* b1cdata=b1norm.cpu_data();
	const Dtype* b2cdata=b2norm.cpu_data();
	for (int i=0;i<M_;i++)
	{
		caffe_cpu_axpby(K_,Dtype(1.)/b1cdata[i],bottom_data1+(i*K_),Dtype(0),b1normdata+(i*K_));
		caffe_cpu_axpby(K_,Dtype(1.)/b2cdata[i],bottom_data2+(i*K_),Dtype(0),b2normdata+(i*K_));
	}*/
	/*caffe_sub(
	    count,
	    b1normal.cpu_data(),
	    b2normal.cpu_data(),
	    diff_.mutable_cpu_data());*/
	caffe_sub(
		count,
		bottom[0]->cpu_data(),
		bottom[1]->cpu_data(),
		diff_.mutable_cpu_data());
	const Dtype* bias_data = this->blobs_[0]->cpu_data();
	Dtype dot = caffe_cpu_dot(count, diff_.cpu_data(), diff_.cpu_data());
	Dtype* dist_data=(*top)[0]->mutable_cpu_data();
	Dtype* diff_data=diff_.mutable_cpu_data();
	//LOG(INFO) << "dimension:" << K_;
	for (int i=0;i<M_;i++){
		dist_data[i]=caffe_cpu_dot(K_,diff_data+(i*K_),diff_data+(i*K_));
		//LOG(INFO) << "distance:" << dist_data[i];
		//if (i%400==0)
		//{
			//LOG(INFO) << "distance:" << dist_data[i] << ", bias:" << bias_data[0];
		//}
		dist_data[i]=dist_data[i]-bias_data[0];
	}
	//LOG(INFO) << "bias:" << bias_data[0];
	//for (int i=0;i<5;i++)
	//{
	//	LOG(INFO) << "dist[" << i << "]=" << dist_data[i];
	//}
	//for (int i=0;i<M_;i++)
	//{
	//	if (label[i]==1)
	//	{
	//		caffe_cpu_gemm(CblasNoTrans,CblasTrans,1,1,K_,Dtype(1.),diff_data+(i*K_),diff_data+(i*K_));
	//	}
	//}

}

template <typename Dtype>
void EuclideanThresholdLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
		const vector<bool>& propagate_down, vector<Blob<Dtype>*>* bottom) {
	const Dtype* diff_data=diff_.cpu_data();
	const Dtype* top_diff = top[0]->cpu_diff();
	Dtype* bias_diff = this->blobs_[0]->mutable_cpu_diff();
	Dtype* bottom_diff0=(*bottom)[0]->mutable_cpu_diff();
	Dtype* bottom_diff1=(*bottom)[1]->mutable_cpu_diff();
	caffe_copy(diff_.count(),diff_data,bottom_diff0);
	caffe_copy(diff_.count(),diff_data,bottom_diff1);
	bias_diff[0]=0;
	const Dtype * b1ndata = b1norm.cpu_data();
	const Dtype * b2ndata = b2norm.cpu_data();
	for (int i=0;i<M_;i++){
		//caffe_scal(K_,2*top_diff[i]/b1ndata[i],bottom_diff0+(i*K_));
		//caffe_scal(K_,-2*top_diff[i]/b2ndata[i],bottom_diff1+(i*K_));
		caffe_scal(K_,2*top_diff[i],bottom_diff0+(i*K_));
		caffe_scal(K_,-2*top_diff[i],bottom_diff1+(i*K_));
		bias_diff[0]=bias_diff[0]-top_diff[i];
	}
	//bias_diff[0]=bias_diff[0]*Dtype(1e+21);
	//LOG(INFO) << "bias_diff:" << bias_diff[0];
}

#ifdef CPU_ONLY
STUB_GPU(EuclideanThresholdLayer);
#endif

INSTANTIATE_CLASS(EuclideanThresholdLayer);

}  // namespace caffe
