#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

#include <unistd.h>
#include <sys/time.h>

//#include <immintrin.h>
//#include <pmmintrin.h>

#include <boost/thread.hpp>

//#define AAA
#define BBB
namespace caffe {

	/*
void sblas_igemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const char alpha, const char* A, const char* B, const char beta, int* C) {
			struct timeval start, end, res;
	int* C_data = C;
	const char* A_data;
	const char* B_data;
	printf("%d %d %d\n", M, N, K);
			gettimeofday(&start, NULL);
	const char* A_d = A-K;
	const char* B_d;
	char* B_t;
	//B_Transpose
	B_data = B;
	for(int i = 0; i < K; i++) {
		B_t = &B_T[0]+i;
		for(int j = 0; j < N; j++) {
			*B_t = *B_data;
			B_t+=K;
			B_data++;
		}
	}
	for(int i = 0; i < M; i++){
		int j;
		A_d += K;

		B_data = B_T;
		for(j=0; j < N-4; j+=4){
			A_data = A_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K))2
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				A_data++;
				B_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}

		/*
		B_d = B;
		for(j = 0; j < N-4; j+=4){
			A_data = A_d;
			B_data = B_d;
			B_d+=4;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k!=0 ; k--){
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+1));
				acc2 += (short)(*A_data) * (short)(*(B_data+2));
				acc3 += (short)(*A_data) * (short)(*(B_data+3));

				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+N));
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+N+1));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+N+2));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+N+3));

				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2*N));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2*N+1));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2*N+2));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2*N+3));

				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3*N));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3*N+1));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3*N+2));
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3*N+3));

				A_data+=4;
				B_data += N*4;
			}

#ifndef FAST_APPX
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+1));
				acc2 += (short)(*A_data) * (short)(*(B_data+2));
				acc3 += (short)(*A_data) * (short)(*(B_data+3));
				A_data++;
				B_data += N;
			}
#endif
			(*C_data) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
		}/
		for(; j < N; j++){
			A_data = A_d;
			B_data = B_d;
			B_d++;
			int acc = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+N));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2*N));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3*N));
				A_data+=4;
				B_data += N*4;
			}
			for(;k<K;k++) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data += N;
			}
			(*C_data) = acc;
			C_data++;
		}*/
/*	}
}
*/
//#define FAST_APPX
//INT8 Edited
#ifdef AAA
/*char B_T[10000000];
void sblas_igemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const char alpha, const char* A, const char* B, const char beta, int* C) {
	int* C_data = C;
	const char* A_data;
	const char* B_data;
	const char* A_d = A;
	const char* B_d;
	char* B_t;
	//B_Transpose
	B_data = B;
	int i=0;
	for(; i < K; i++) {
		B_t = &B_T[0]+i;
		for(int j = 0; j < N; j++) {
			*B_t = *B_data;
			B_t+=K;
			B_data++;
		}
	}

	i=0;
*/
/*	for(; i < M-2; i+=2){
		int j;
		B_data = B_T;
		for(j=0; j < N-4; j+=4){
			A_data = A_d;
			int acc00 = 0;
		int acc01 = 0;
			int acc02 = 0;
			int acc03 = 0;
			int acc10 = 0;
			int acc11 = 0;
			int acc12 = 0;
			int acc13 = 0;

			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc01 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc01 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc01 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc02 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc02 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc02 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc03 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
				acc03 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
				acc03 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc11 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+K));
				acc11 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+K));
				acc11 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+K));

				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc12 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+2*K));
				acc12 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+2*K));
				acc12 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+2*K));

				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));
				acc13 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+3*K));
				acc13 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+3*K));
				acc13 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+3*K));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));

				A_data++;
				B_data++;
			}
			(*(C_data)) = acc00;
			(*(C_data+1)) = acc01;
			(*(C_data+2)) = acc02;
			(*(C_data+3)) = acc03;

			(*(C_data+N)) = acc10;
			(*(C_data+N+1)) = acc11;
			(*(C_data+N+2)) = acc12;
			(*(C_data+N+3)) = acc13;

			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc00 = 0;
			int acc10 = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*(A_data)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*(C_data)) = acc00;
			(*(C_data+N)) = acc10;
			C_data++;
		}
		C_data+=N;
		A_d += K*2;
	}*/ //original except
/*
	for(; i < M; i++){

		B_data = B_T;

		int j=0;
		for(; j < N-4; j+=4){
			A_data = A_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));
				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc0 += (short)(*A_data) * (short)(*B_data);

				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));

				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));

				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k != 0;k--) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				A_data++;
				B_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}
		A_d += K;
	}
}*/
#endif
#ifdef BBB
inline int _ceil(int a, int b) {
	return (a-1)/b + 1;
}
void sblas_igemm_general(const char* A, const char* B, int* C, int M, int N, int K) {
	int* C_data = C;
	const char* A_data;
	const char* B_data;
	const char* A_d = A;
	const char* B_d;
	char* B_t;
	//B_Transpose
	int i=0;

	i=0;

	for(; i < M-2; i+=2){
		int j;
		B_data = B;
		for(j=0; j < N-4; j+=4){
			A_data = A_d;
			int acc00 = 0;
		int acc01 = 0;
			int acc02 = 0;
			int acc03 = 0;
			int acc10 = 0;
			int acc11 = 0;
			int acc12 = 0;
			int acc13 = 0;

			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc01 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc01 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc01 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc02 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc02 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc02 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc03 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
				acc03 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
				acc03 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc11 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+K));
				acc11 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+K));
				acc11 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+K));

				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc12 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+2*K));
				acc12 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+2*K));
				acc12 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+2*K));

				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));
				acc13 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+3*K));
				acc13 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+3*K));
				acc13 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+3*K));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));

				A_data++;
				B_data++;
			}
			(*(C_data)) = acc00;
			(*(C_data+1)) = acc01;
			(*(C_data+2)) = acc02;
			(*(C_data+3)) = acc03;

			(*(C_data+N)) = acc10;
			(*(C_data+N+1)) = acc11;
			(*(C_data+N+2)) = acc12;
			(*(C_data+N+3)) = acc13;

			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc00 = 0;
			int acc10 = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*(A_data)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*(C_data)) = acc00;
			(*(C_data+N)) = acc10;
			C_data++;
		}
		C_data+=N;
		A_d += K*2;
	}

	for(; i < M; i++){

		B_data = B;

		int j=0;
		for(; j < N-4; j+=4){
			A_data = A_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));
				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc0 += (short)(*A_data) * (short)(*B_data);

				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));

				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));

				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k != 0;k--) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				A_data++;
				B_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}
		A_d += K;
	}
}
#define THREADN 8
void accum(const char* A, const char* B, int* C, int M, int N, int K, int m)
{
	const char* A_data;
	const char* A_d;
	const char* B_data;
	int* C_data;
	int i;
	int start = m*_ceil(M,THREADN), end = (m+1)*_ceil(M,THREADN);
	if(M<end) end = M;
	for(i=start;i<end; i++) {
		A_d = A + i*K;
		B_data = B;
		C_data = C + i*N;
		int j=N>>2;
		for(; j != 0; j--){
			A_data = A_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k != 0;k--) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				A_data++;
				B_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_data+=3*K;
		}
		j=N-((N>>2)<<2);
		for(; j != 0; j--){
			A_data = A_d;
			int acc = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}
	}
}
void accum_b(const char* A, const char* B, int* C, int M, int N, int K, int n)
{
	int Nt = (n+1)*_ceil(N,THREADN);
	int tmpj = n*_ceil(N,THREADN);
	if(N<Nt) Nt = N;

	for(int m=0;m<M;m++) {
	const char* A_data = A + m*K;
	const char* A_d = A_data;
	const char* B_data;
	int* C_data;
	int i;
	int j = tmpj;
	B_data = B + j*K;
	C_data = C + m*N + j;
	for(; j < Nt-3; j+=4){
		A_data = A_d;
		int acc0 = 0;
		int acc1 = 0;
		int acc2 = 0;
		int acc3 = 0;
		int k=K>>2;
		for(; k != 0; k--){
			acc0 += (short)(*A_data) * (short)(*B_data);
			acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
			acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
			acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

			acc1 += (short)(*A_data) * (short)(*(B_data+K));
			acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
			acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
			acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

			acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
			acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
			acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
			acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

			acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
			acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
			acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
			acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

			A_data+=4;
			B_data+=4;
		}
		k=K-((K>>2)<<2);
		for(;k != 0;k--) {
			acc0 += (short)(*A_data) * (short)(*B_data);
			acc1 += (short)(*A_data) * (short)(*(B_data+K));
			acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
			acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
			A_data++;
			B_data++;
		}
		(*(C_data)) = acc0;
		(*(C_data+1)) = acc1;
		(*(C_data+2)) = acc2;
		(*(C_data+3)) = acc3;
		C_data+=4;
		B_data+=3*K;
	}
	for(; j < Nt; j++){
		A_data = A_d;
		int acc = 0;
		int k=K>>2;
		for(; k != 0; k--){
			acc += (short)(*A_data) * (short)(*B_data);
			acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
			acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
			acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
			A_data+=4;
			B_data+=4;
		}
		k=K-((K>>2)<<2);
		for(;k!=0;k--) {
			acc += (short)(*A_data) * (short)(*B_data);
			A_data++;
			B_data++;
		}
		(*C_data) = acc;
		C_data++;
	}
	}
}

//char B_T[10000000];
boost::thread t[THREADN+1];
void sblas_igemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const char alpha, const char* A, const char* B, const char beta, int* C, char *B_T) {
	const char* B_data;
	char* B_t;
	//B_Transpose
	B_data = B;
	int i=0;
	for(; i < K; i++) {
		B_t = B_T+i;
		for(int j = 0; j < N; j++) {
			*B_t = *B_data;
			B_t+=K;
			B_data++;
		}
	}

//	i=0;

	if(M==1) {
		sblas_igemm_general(A, B_T, C, M, N, K);
	}
	else if(M>=THREADN) {
		for(i=0; i < THREADN; i++){
			t[i] = boost::thread{accum, A, B_T, C, M, N, K, i};
		}
		for(i=0; i < THREADN; i++){
			t[i].join();
		}
	}
	else if(N>=THREADN){
		int j;
		for(j=0; j < THREADN; j++){
			t[j] = boost::thread{accum_b, A, B_T, C, M, N, K, j};
		}
		for(j=0; j < THREADN; j++){
			t[j].join();
		}
	}
	else {
		sblas_igemm_general(A, B_T, C, M, N, K);
	}
}
#endif

void sblas_asymm_igemm_general(const unsigned char* A, const unsigned char* B, int* C, int M, int N, int K) {
	int* C_data = C;
	const unsigned char* A_data;
	const unsigned char* B_data;
	const unsigned char* A_d = A;
	const unsigned char* B_d;
	//B_Transpose
	int i=0;

	i=0;

	for(; i < M-2; i+=2){
		int j;
		B_data = B;
		for(j=0; j < N-4; j+=4){
			A_data = A_d;
			int acc00 = 0;
		int acc01 = 0;
			int acc02 = 0;
			int acc03 = 0;
			int acc10 = 0;
			int acc11 = 0;
			int acc12 = 0;
			int acc13 = 0;

			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc01 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc01 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc01 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));

				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc02 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc02 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc02 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));

				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc03 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));
				acc03 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));
				acc03 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc11 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+K));
				acc11 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+K));
				acc11 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+K));

				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc12 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+2*K));
				acc12 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+2*K));
				acc12 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+2*K));

				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));
				acc13 += (short)(*(A_data+K+1)) * (short)(*(B_data+1+3*K));
				acc13 += (short)(*(A_data+K+2)) * (short)(*(B_data+2+3*K));
				acc13 += (short)(*(A_data+K+3)) * (short)(*(B_data+3+3*K));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc01 += (short)(*A_data) * (short)(*(B_data+K));
				acc02 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc03 += (short)(*A_data) * (short)(*(B_data+3*K));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc11 += (short)(*(A_data+K)) * (short)(*(B_data+K));
				acc12 += (short)(*(A_data+K)) * (short)(*(B_data+2*K));
				acc13 += (short)(*(A_data+K)) * (short)(*(B_data+3*K));

				A_data++;
				B_data++;
			}
			(*(C_data)) = acc00;
			(*(C_data+1)) = acc01;
			(*(C_data+2)) = acc02;
			(*(C_data+3)) = acc03;

			(*(C_data+N)) = acc10;
			(*(C_data+N+1)) = acc11;
			(*(C_data+N+2)) = acc12;
			(*(C_data+N+3)) = acc13;

			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc00 = 0;
			int acc10 = 0;
			int k=0;
			for(; k < K-4; k+=4){
				acc00 += (short)(*A_data) * (short)(*B_data);
				acc00 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc00 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc00 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K+1)) * (short)(*(B_data+1));
				acc10 += (short)(*(A_data+K+2)) * (short)(*(B_data+2));
				acc10 += (short)(*(A_data+K+3)) * (short)(*(B_data+3));

				A_data+=4;
				B_data+=4;
			}
			for(;k<K;k++) {
				acc00 += (short)(*(A_data)) * (short)(*B_data);
				acc10 += (short)(*(A_data+K)) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*(C_data)) = acc00;
			(*(C_data+N)) = acc10;
			C_data++;
		}
		C_data+=N;
		A_d += K*2;
	}

	for(; i < M; i++){

		B_data = B;

		int j=0;
		for(; j < N-4; j+=4){
			A_data = A_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc3 += (short)(*(A_data+3)) * (short)(*(B_data+3+3*K));
				acc2 += (short)(*(A_data+3)) * (short)(*(B_data+3+2*K));
				acc1 += (short)(*(A_data+3)) * (short)(*(B_data+3+K));
				acc0 += (short)(*(A_data+3)) * (short)(*(B_data+3));

				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc0 += (short)(*A_data) * (short)(*B_data);

				acc0 += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc1 += (short)(*(A_data+1)) * (short)(*(B_data+1+K));
				acc2 += (short)(*(A_data+1)) * (short)(*(B_data+1+2*K));
				acc3 += (short)(*(A_data+1)) * (short)(*(B_data+1+3*K));

				acc0 += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc1 += (short)(*(A_data+2)) * (short)(*(B_data+2+K));
				acc2 += (short)(*(A_data+2)) * (short)(*(B_data+2+2*K));
				acc3 += (short)(*(A_data+2)) * (short)(*(B_data+2+3*K));

				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k != 0;k--) {
				acc0 += (short)(*A_data) * (short)(*B_data);
				acc1 += (short)(*A_data) * (short)(*(B_data+K));
				acc2 += (short)(*A_data) * (short)(*(B_data+2*K));
				acc3 += (short)(*A_data) * (short)(*(B_data+3*K));
				A_data++;
				B_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_data+=3*K;
		}
		for(; j < N; j++){
			A_data = A_d;
			int acc = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc += (short)(*A_data) * (short)(*B_data);
				acc += (short)(*(A_data+1)) * (short)(*(B_data+1));
				acc += (short)(*(A_data+2)) * (short)(*(B_data+2));
				acc += (short)(*(A_data+3)) * (short)(*(B_data+3));
				A_data+=4;
				B_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc += (short)(*A_data) * (short)(*B_data);
				A_data++;
				B_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}
		A_d += K;
	}
}

void sblas_asymm_igemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const char alpha, const char* A, const char* B, const char beta, int* C, char *B_T) {
	const char* B_data;
	char* B_t;
	//B_Transpose
	B_data = B;
	int i=0;
	for(; i < K; i++) {
		B_t = B_T+i;
		for(int j = 0; j < N; j++) {
			*B_t = *B_data;
			B_t+=K;
			B_data++;
		}
	}

	sblas_asymm_igemm_general((unsigned char*)A, (unsigned char*)B_T, C, M, N, K);
}
/*
__attribute__ ((aligned(16))) int A_T[10000000];
__attribute__ ((aligned(16))) int B_T[10000000];
void sblas_igemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const char alpha, const char* A, const char* B, const char beta, int* C) {
			struct timeval start, end, res;
	int* C_data = C;
	const char* A_data;
	const char* B_data;
	printf("%d %d %d\n", M, N, K);
			gettimeofday(&start, NULL);
	const char* A_d = A;
	const char* B_d;
	int* A_t_data;
	int* B_t_data;
	int* A_t;
	int* B_t;
	int* A_t_d = A_T;
	int* B_t_d;
	//B_Transpose
	B_data = B;
	int i=0;
	for(; i < K; i++) {
		B_t = &B_T[0]+i;
		for(int j = 0; j < N; j++) {
			*B_t = *B_data;
			B_t+=K;
			B_data++;
		}
	}
	A_data = A;
	A_t = &A_T[0];
	i=0;
	for(; i < M; i++) {
		for(int j = 0; j < K; j++) {
			*A_t = *A_data;
			A_t++;
			A_data++;
		}
	}

	i=0;
	for(; i < M; i++){

		B_t_data = B_T;

		int j=N>>2;
		for(; j != 0; j--){
			A_t_data = A_t_d;
			int acc0 = 0;
			int acc1 = 0;
			int acc2 = 0;
			int acc3 = 0;
			int k=K>>2;
			for(; k != 0; k--){
				__m128i va, vb, vc;
				va = _mm_store_si128((__m128i *)A_t_data);
				vb = _mm_store_si128((__m128i *)B_t_data);
//				vc = _mm_multi_*/
/*				acc0 += (short)(*A_t_data) * (short)(*B_t_data);
				acc0 += (short)(*(A_t_data+1)) * (short)(*(B_t_data+1));
				acc0 += (short)(*(A_t_data+2)) * (short)(*(B_t_data+2));
				acc0 += (short)(*(A_t_data+3)) * (short)(*(B_t_data+3));

				acc1 += (short)(*A_t_data) * (short)(*(B_t_data+K));
				acc1 += (short)(*(A_t_data+1)) * (short)(*(B_t_data+1+K));
				acc1 += (short)(*(A_t_data+2)) * (short)(*(B_t_data+2+K));
				acc1 += (short)(*(A_t_data+3)) * (short)(*(B_t_data+3+K));

				acc2 += (short)(*A_t_data) * (short)(*(B_t_data+2*K));
				acc2 += (short)(*(A_t_data+1)) * (short)(*(B_t_data+1+2*K));
				acc2 += (short)(*(A_t_data+2)) * (short)(*(B_t_data+2+2*K));
				acc2 += (short)(*(A_t_data+3)) * (short)(*(B_t_data+3+2*K));

				acc3 += (short)(*A_t_data) * (short)(*(B_t_data+3*K));
				acc3 += (short)(*(A_t_data+1)) * (short)(*(B_t_data+1+3*K));
				acc3 += (short)(*(A_t_data+2)) * (short)(*(B_t_data+2+3*K));
				acc3 += (short)(*(A_t_data+3)) * (short)(*(B_t_data+3+3*K));
*/
/*				A_t_data+=4;
				B_t_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k != 0;k--) {
				acc0 += (short)(*A_t_data) * (short)(*B_t_data);
				acc1 += (short)(*A_t_data) * (short)(*(B_t_data+K));
				acc2 += (short)(*A_t_data) * (short)(*(B_t_data+2*K));
				acc3 += (short)(*A_t_data) * (short)(*(B_t_data+3*K));
				A_t_data++;
				B_t_data++;
			}
			(*(C_data)) = acc0;
			(*(C_data+1)) = acc1;
			(*(C_data+2)) = acc2;
			(*(C_data+3)) = acc3;
			C_data+=4;
			B_t_data+=3*K;
		}
		j=N-((N>>2)<<2);
		for(; j != 0; j--){
			A_t_data = A_t_d;
			int acc = 0;
			int k=K>>2;
			for(; k != 0; k--){
				acc += (short)(*A_t_data) * (short)(*B_t_data);
				acc += (short)(*(A_t_data+1)) * (short)(*(B_t_data+1));
				acc += (short)(*(A_t_data+2)) * (short)(*(B_t_data+2));
				acc += (short)(*(A_t_data+3)) * (short)(*(B_t_data+3));
				A_t_data+=4;
				B_t_data+=4;
			}
			k=K-((K>>2)<<2);
			for(;k!=0;k--) {
				acc += (short)(*A_t_data) * (short)(*B_t_data);
				A_t_data++;
				B_t_data ++;
			}
			(*C_data) = acc;
			C_data++;
		}
		A_t_d += K;
	}

			gettimeofday(&end, NULL);
			timersub(&end, &start, &res);
			printf("ingemm time: %lf\n", (float)res.tv_sec*1000.0 + (float)res.tv_usec/1000.0);
}
*/
//INT8 Edited
void sblas_intgemm(const CBLAS_TRANSPOSE TransA, const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const int alpha, const int* A, const float* B, const int beta, int* C) {
	int* C_data = C;
	for(int i = 0; i < M; i++){
		for(int j = 0; j < N; j++){
			const int* A_data = A + K*i;
			const float* B_data = B + j;
			int acc = 0;
			for(int k = 0; k < K; k++){
				acc += (*A_data) * (*B_data);
				A_data++;
				B_data += N;
			}
			(*C_data) = acc + (*C_data) * beta;
			C_data++;
		}
	}
}

void caffe_cpu_asymm_offset(const char* in_c, const char* w_c, int* out, const unsigned char in_zero_point, const unsigned char* w_zero_point, int M, int N, int K) {
	const unsigned char* in = (const unsigned char*) in_c;
	const unsigned char* w = (const unsigned char*) w_c;
	int* in_sum = (int*)malloc(sizeof(int)*N);
	int* w_offset = (int*)malloc(sizeof(int)*M);

	//in_offset
	for(int i=0;i<N;i++) {
		in_sum[i] = 0;
		for(int j=0;j<K;j++) {
			in_sum[i] += in[i + j*N];
		}
	}

	//if(in_zero_point) {
		//w_offset
		for(int i=0;i<M;i++) {
			w_offset[i] = 0;
			for(int j=0;j<K;j++) {
				w_offset[i] += w[i*K + j];
			}
			w_offset[i] *= in_zero_point;
		}
	//}

	for(int i=0;i<M;i++) {
		for(int j=0;j<N;j++) {
			out[i*N + j] -= in_sum[j] * w_zero_point[i];
			//if(in_zero_point) {
				out[i*N + j] -= w_offset[i];
				out[i*N + j] += K * in_zero_point * w_zero_point[i];
			//}
	//		printf("%d %d %d\n",in_sum[j] * w_zero_point[i],w_offset[i], in_zero_point * w_zero_point[i]);
		}
	}

	free(w_offset);
	free(in_sum);
/*
	printf("weight\n");
	for(int i=0;i<M;i++) {
		for(int j=0;j<K;j++) {
			printf("%d ", w[i*K + j]);
		}
	}
	printf("\n");
	printf("zero: %d\n", in_zero_point);
	for(int i=0;i<M;i++) {
		printf("%d ", w_zero_point[i]);
	}
	printf("\n");*/
}

//INT8 Edited
void caffe_cpu_cgemm(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const char alpha, const char* A, const char* B, const char beta,
    int* C, char* B_T) {
  // char gemm
  sblas_igemm(TransA, TransB, M, N, K, alpha, A, B, beta, C, B_T);
}
//INT8 Edited
void caffe_cpu_asymm_cgemm(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const char alpha, const char* A, const char* B, const char beta,
    int* C, char* B_T) {
  // char gemm
  sblas_asymm_igemm(TransA, TransB, M, N, K, alpha, A, B, beta, C, B_T);
}
//INT8 Edited
template<>
void caffe_cpu_igemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const int alpha, const int* A, const float* B, const int beta,
    int* C) {
  // char gemm
  sblas_intgemm(TransA, TransB, M, N, K, alpha, A, B, beta, C);
}
template<>
void caffe_cpu_igemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const int alpha, const int* A, const double* B, const int beta,
    int* C) {
  // char gemm
}

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

//INT8 Edited
template <>
void caffe_axpy<char>(const int N, const char alpha, const char* X,
    char* Y) {
	LOG(FATAL)<<"no implemted INT8 caffe_axpy";
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

//INT8 Edited
template void caffe_set<char>(const int N, const char alpha, char* Y);
template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

//INT8 Edited
template void caffe_copy<char>(const int N, const char* X, char* Y);
template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

//INT8 Edited
template <>
void caffe_scal<char>(const int N, const char alpha, char *X) {
	LOG(FATAL)<<"no implemted INT8 caffe_scal";
	X = 0;
}

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

//INT8 Edited
//Spetialization for INT8
template<> void caffe_rng_uniform<char>(const int n, const char a, const char b,
                              char* r) {
	CHECK_GE(n, 0);
	CHECK(r);
	CHECK_LE(a, b);
	float a_ = (float)((int)a);
	float b_ = (float)((int)b);
	boost::uniform_real<float> random_distribution(a_, caffe_nextafter<float>(b_));
	boost::variate_generator<caffe::rng_t*, boost::uniform_real<float> >
		variate_generator(caffe_rng(), random_distribution);
	for (int i = 0; i < n; ++i) {
		int r_ = (int)(variate_generator());
		r[i] = (r_>127)?(127):((r_<-128)?(-128):(r_));
	}
}
template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

//INT8 Edited
//Spetialization for INT8
template<> void caffe_rng_gaussian<char>(const int n, const char a,
                               const char sigma, char* r) {
  	CHECK(r);
	char _sigma = 1;
  	CHECK_GT(_sigma, 0);
	float a_ = (float)((int)a);
	float sigma_ = (float)((int)_sigma);
  	boost::normal_distribution<float> random_distribution(a_, sigma_);
  	boost::variate_generator<caffe::rng_t*, boost::normal_distribution<float> >
  	    variate_generator(caffe_rng(), random_distribution);
  	for (int i = 0; i < n; ++i) {
		int r_ = (int)(variate_generator());
		r[i] = (r_>127)?(127):((r_<-128)?(-128):(r_));
  	}
}
template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

//INT8 Edited
//Spetialization for INT8
template<> void caffe_rng_bernoulli<char>(const int n, const char p, int* r){
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  float p_ = (float)((int)p);
  boost::bernoulli_distribution<float> random_distribution(p_);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<float> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

//INT8 Edited
template <>
char caffe_cpu_strided_dot<char>(const int n, const char* x, const int incx,
    const char* y, const int incy) {
  LOG(FATAL)<<"no implemted INT8 caffe_strided_dot";
  return 0;
}

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

//INT8 Edited
template
char caffe_cpu_dot<char>(const int n, const char* x, const char* y);

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

//INT8 Edited
template <>
char caffe_cpu_asum<char>(const int n, const char* x) {
  LOG(FATAL)<<"no implemted INT8 caffe_asum";
  return 0;
}

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


#ifndef PARTICLE_COMPONENT3_HPP
#define PARTICLE_COMPONENT3_HPP

#include "vec3.hpp"
#include "Component3.hpp"
#include "Actor.hpp"
#include "Random.hpp"
#include <boost/scoped_array.hpp>
#include "Camera3.hpp"
#include "Quaternion.hpp"
#include <cassert>

template <class T> // t should be a subclass of ParticleBase
struct ParticleBase {
	T* next;

	vec3 position;
	vec3 futurePosition;

	vec3 color;
	vec3 futureColor;

	float size;
	float futureSize;

	float alpha;
	float futureAlpha;

	ParticleBase() : next(0), position(0,0,0), futurePosition(0,0,0), color(0,0,0), futureColor(0,0,0) {}
};

struct Particle : public ParticleBase<Particle> {
};

enum FailedCreationAction {
	FCA_IGNORE,
	FCA_TAKE_OLDEST
};

template <class TParticle>
class PointEmitter {
public:
	PointEmitter(Actor* pActor, Random& pRandom) : mActor(pActor), mRand(pRandom) {
		mRand.randomize();
	}
	void emit(TParticle* pParticle, float pRange) {
		assert(pParticle);
		pParticle->position =
			pParticle->futurePosition =
			mActor->getPositionObject().estimatePosition( math::limitRange(0.0f, pRange + mRand.randomFloatWithSign() * 0.01f, 1.0f) );
	}
private:
	Actor* mActor;
	Random& mRand;
};

template <class TParticleSystem, class TParticle, class TEmitter>
class ParticleComponent3 : public Component3 {
public:
	ParticleComponent3(unsigned int pParticleCount, const TEmitter& pEmitter, Object3* pOwner) : Component3(pOwner), mEmitter(pEmitter), mParticles( new TParticle[pParticleCount] ), mAlive(0), mDead(0), mActiveParticles(0) {
		TParticle* lastParticle = 0;
		for(unsigned int particleIndex=0; particleIndex<pParticleCount; ++particleIndex) {
			TParticle* particle = &mParticles[particleIndex];
			if( lastParticle ) {
				lastParticle->next = particle;
			}
			else {
				mDead = particle;
			}
			lastParticle = particle;
		}
	}
	// returns true if should continue updating
	bool updateParticles(real pTime) {
		TParticle* previousParticle = 0;
		TParticle* currentParticle = mAlive;
		mActiveParticles = 0;
		while( currentParticle ) {
			TParticle* next = currentParticle->next;
			if( static_cast<TParticleSystem*>(this)->updateParticle(currentParticle, pTime) ) {
				++mActiveParticles;
				previousParticle = currentParticle;
			}
			else {
				if( previousParticle ) {
					previousParticle->next = currentParticle->next;
				}
				else {
					mAlive = currentParticle->next;
				}
				currentParticle->next = mDead;
				mDead = currentParticle;
			}
			currentParticle = next;
		}
		return mActiveParticles!=0;
	}
	void addParticles(unsigned int pParticleCount, FailedCreationAction pAction) {
		for(unsigned int particleAdded=0; particleAdded<pParticleCount; ++particleAdded) {
			// add a particle
			TParticle* particle = mDead;
			if( particle ) {
				mDead = particle->next;
			}
			else {
				// no dead particles to reanimate
				if( pAction == FCA_IGNORE ) {
					// we can't possible create anything more, so let's don't try
					return;
				}
				else if( FCA_TAKE_OLDEST) {
					TParticle* oldest = 0;
					TParticle* nextOldest = 0;
					for(TParticle* currentParticle=mAlive; currentParticle; currentParticle=currentParticle->next) {
						nextOldest = oldest;
						oldest = currentParticle;
					}
					assert(oldest);
					assert(nextOldest);
					nextOldest->next = 0;
					particle = oldest;

				}
				else {
					assert(0 && "bad value of pAction");
				}
			}

			// particle should be valid
			assert(particle); // let's make sure it is

			mEmitter.emit(particle, particleAdded/real(pParticleCount));
			static_cast<TParticleSystem*>(this)->setupParticle(particle);
			// add it to the list
			particle->next = mAlive;
			mAlive = particle;
		}
	}

	void renderParticles(const World3& pWorld, real pTime) const {
		setupRenderingForParticles();
		for(TParticle* currentParticle=mAlive; currentParticle; currentParticle=currentParticle->next) {
			static_cast<const TParticleSystem*>(this)->renderParticle(*currentParticle, pWorld, pTime);
		}
		restoreRenderingForParticles();
	}
	void renderParticlesAsBillboards(const World3& pWorld, real pTime) const {
		setupRenderingForParticles();
		const vec3 up = pWorld.getCamera().getRotationObject().getCurrentRotation().getUp();
		const vec3 right = pWorld.getCamera().getRotationObject().getCurrentRotation().getRight();
		for(TParticle* currentParticle=mAlive; currentParticle; currentParticle=currentParticle->next) {
			const TParticle& p = *currentParticle;
			::renderBillboard(up, right, pWorld,
				op::vec3::getLinearInterpolation(p.position, pTime, p.futurePosition),
				op::vec3::getLinearInterpolation(p.color, pTime, p.futureColor),
				math::interpolate(p.alpha, pTime, p.futureAlpha),
				math::interpolate(p.size, pTime, p.futureSize) );
		}
		restoreRenderingForParticles();
	}
private:
	Actor* mActor;
	boost::scoped_array<TParticle> mParticles;
	TParticle* mAlive;
	TParticle* mDead;
	TEmitter mEmitter;
	unsigned long mActiveParticles;
};

void setupRenderingForParticles();
void restoreRenderingForParticles();
void renderBillboard(const vec3& pUp, const vec3& pRight, const World3& pWorld, const vec3& pPosition, const vec3& pColor, real pAlpha, real pSize);

#endif


//
// Created by nvidia on 6/7/18.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include <algorithm> // for copy
#include <iterator> // for ostream_iterator
#include <boost/graph/graph_utility.hpp>
#include "../lib/SpVec.hpp"
#include "../lib/superpixel_seg.hpp"
#include "../lib/distance.hpp"

#include "../lib/FloodFillSsdSuperpixelAlgorithm.hpp"

using namespace cv;
using namespace std;
using namespace cv::ximgproc;

using namespace cove;

int main(int argc, char** argv) {
    int idx = 1;
#ifdef DEBUG
    namedWindow("params", 1);
#endif
    ALGORITHM_PARAM(blurKernel, 3, 10)
    ALGORITHM_PARAM(iterations, 10, 20)
    ALGORITHM_PARAM(blurDev, 0, 100)
    ALGORITHM_PARAM(regionSize, 7, 50)
    ALGORITHM_PARAM(ratio, 70, 200)
    ALGORITHM_PARAM(cannyLow, 70, 150)
    ALGORITHM_PARAM(ssdEpsilonSlider, 15, 250)
    ALGORITHM_PARAM(meanH, 10, 50)
    ALGORITHM_PARAM(meanL, 10, 50)
    ALGORITHM_PARAM(meanS, 10, 50)
    ALGORITHM_PARAM(stdH, 10, 50)
    ALGORITHM_PARAM(stdL, 10, 50)
    ALGORITHM_PARAM(stdS, 10, 50)
    ALGORITHM_PARAM(circ, 10, 50)

#ifdef CAMERA
    VideoCapture cap(2); // open the default camera
//    VideoCapture cap("sample.mp4"); // open the video
    if(!cap.isOpened()) {
        return -1;
    }
#endif
    int frameno=0;

//    Mat downsampled(60, 80, CV_8U, Scalar(0));
//    Mat final;
    Mat frame;
//    Mat cielab;
//    Mat mask;

    for (;;) {
        FloodFillSsdSuperpixelAlgorithm algorithm(80, 60, regionSize, static_cast<float>(ssdEpsilonSlider) / 10, blurKernel, blurDev, iterations,
                                                  static_cast<float>(ratio) / 1000, cannyLow, cannyLow*2.5);
        weights[0] = static_cast<double>(meanH) / 10;
        weights[1] = static_cast<double>(meanL) / 10;
        weights[2] = static_cast<double>(meanS) / 10;
        weights[3] = static_cast<double>(stdH) / 10;
        weights[4] = static_cast<double>(stdL) / 10;
        weights[5] = static_cast<double>(stdS) / 10;
        weights[6] = static_cast<double>(circ) / 10;

        clock_t start=CLOCK();

#ifdef DEBUG
        if (blurKernel % 2 == 0) {
            blurKernel++;
        }

#endif
#ifdef CAMERA
        cap >> frame; // get a new frame from camera
#else
        frame = imread(argv[idx]);
#endif
        if (frame.empty()) {
            cout << "frame empty" << endl;
            break;
        }

        Mat floorMask;
        Mat resized;
        algorithm.Run(frame, floorMask, resized);

        Mat zeros(floorMask.size(), CV_8UC3, Scalar(0));
        Mat red(floorMask.size(), CV_8UC3, Scalar(0, 0, 255));
        red.copyTo(zeros, floorMask);
        addWeighted(resized, 0.7, zeros, 0.3, 0.0, resized);

        SHOW_ALWAYS("output", resized);

        // =========== time fps and exit ===========
        double dur = CLOCK()-start;
        printf("avg time per frame %f ms. fps %f. frameno = %d\n",avgdur(dur),avgfps(),frameno++ );
        cout << argv[idx] << endl;
        switch(waitKey(1)) {
            case 27:
                exit(0);
            case 108:
                idx++;
                break;
            case 106:
                idx--;
                break;
        }
    }

    return 0;
}


#ifndef OBJECT_DETECT_NODE_H_
#define OBJECT_DETECT_NODE_H_

#include <thread>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <Eigen/Dense>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/recognition/hv/hv_go.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <rviz_visual_tools/rviz_visual_tools.h>

#include "kinova_unit_app/GetObject.h"

typedef pcl::PointXYZRGB PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;

struct CloudStyle
{
    double r;
    double g;
    double b;
    double size;

    CloudStyle (double r,
                double g,
                double b,
                double size) :
        r (r),
        g (g),
        b (b),
        size (size)
    {
    }
};

CloudStyle style_white (255.0, 255.0, 255.0, 4.0);
CloudStyle style_red (255.0, 0.0, 0.0, 3.0);
CloudStyle style_green (0.0, 255.0, 0.0, 5.0);
CloudStyle style_cyan (93.0, 200.0, 217.0, 4.0);
CloudStyle style_violet (255.0, 0.0, 255.0, 8.0);

std::string model_filename_;
std::string scene_filename_;

//Algorithm params
bool show_keypoints_ (false);
bool show_correspondences_ (false);
bool use_cloud_resolution_ (false);
bool use_hough_ (true);
float model_ss_ (0.005f);
float scene_ss_ (0.008f);
float rf_rad_ (0.015f);
// float descr_rad_ (0.024f);
// float cg_size_ (0.015f);
// float descr_rad_ (0.018f);
// float cg_size_ (0.015f);
float descr_rad_ (0.018f);
float cg_size_ (0.015f);
float cg_thresh_ (5.0f);
int icp_max_iter_ (5);
float icp_corr_distance_ (0.005f);
float hv_resolution_ (0.005f);
float hv_occupancy_grid_resolution_ (0.01f);
float hv_clutter_reg_ (5.0f);
float hv_inlier_th_ (0.005f);
float hv_occlusion_th_ (0.01f);
float hv_rad_clutter_ (0.03f);
float hv_regularizer_ (3.0f);
float hv_rad_normals_ (0.05);
bool hv_detect_clutter_ (true);
// bool use_cloud_resolution_(true);
// bool use_hough_(true);
// float model_ss_(7.5f); //0.01f
// float scene_ss_(20.0f);//0.03f-20
// float rf_rad_(10.0f);  //0.015f-10
// float descr_rad_(15.0f);//0.02f -19 -
// float cg_size_(10.0f);  //0.01f-10
// float cg_thresh_(5.0f);

class ObjectDetect {
  public:
    explicit ObjectDetect(ros::NodeHandle* nh, ros::NodeHandle* pnh);
    Eigen::Quaternionf ObjectRecognition(pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzCloudPtrRansacFiltered, int i);
    geometry_msgs::Pose pose_oritention;
  private:
    void PointCloudCB(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
    bool ObjectRequestCB(kinova_unit_app::GetObject::Request &req, kinova_unit_app::GetObject::Response &res);

    ros::NodeHandle *node_handle_;
    ros::NodeHandle *private_node_handle_;

    geometry_msgs::PoseStamped latest_object_pose_;

    std::string point_cloud_frame_;

    std::string point_cloud_topic_;
    std::string debug_output_topic_;
    std::string object_visual_markers_topic_;

    ros::Subscriber point_cloud_sub_;
    ros::Publisher debug_pointcloud_pub_;
    ros::ServiceServer object_request_srv_;

    ros::Publisher object_pub_;

    rviz_visual_tools::RvizVisualToolsPtr object_visual_markers_pub_;
};

#endif // OBJECT_DETECT_NODE_H_

ObjectDetect::ObjectDetect(ros::NodeHandle *nh, ros::NodeHandle *pnh) : node_handle_(nh), private_node_handle_(pnh) {
  private_node_handle_->param<std::string>("point_cloud_frame", point_cloud_frame_, "");
  private_node_handle_->param<std::string>("point_cloud_topic", point_cloud_topic_, "/depth/pointcloud");
  private_node_handle_->param<std::string>("debug_output_topic", debug_output_topic_, "/debug_pointcloud_output");
  private_node_handle_->param<std::string>("object_visual_markers_topic", object_visual_markers_topic_, "/object_visual_markers");

  point_cloud_sub_ = node_handle_->subscribe (point_cloud_topic_, 1, &ObjectDetect::PointCloudCB, this);
  debug_pointcloud_pub_ = node_handle_->advertise<sensor_msgs::PointCloud2> (debug_output_topic_, 1);

  object_pub_ = node_handle_->advertise<geometry_msgs::PoseArray> ("object_pose", 1);

  object_request_srv_ = private_node_handle_->advertiseService("request_first_object", &ObjectDetect::ObjectRequestCB, this);

  object_visual_markers_pub_.reset(new rviz_visual_tools::RvizVisualTools(point_cloud_frame_, object_visual_markers_topic_));
  object_visual_markers_pub_->loadMarkerPub();
  object_visual_markers_pub_->deleteAllMarkers();
  object_visual_markers_pub_->enableBatchPublishing();
}

bool ObjectDetect::ObjectRequestCB(kinova_unit_app::GetObject::Request &req, kinova_unit_app::GetObject::Response &res) {
  if(req.request_type == 0) {
    if(ros::Time::now() - latest_object_pose_.header.stamp < ros::Duration(0.5)) {
      res.object_pose = latest_object_pose_;
    } else {
      ROS_INFO("no update for object for %lf", (ros::Time::now()-latest_object_pose_.header.stamp).toSec());
      return false;
    }
  }

  return true;
}

void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*          Global Hypothese Verification Tutorial - Usage Guide          *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "***************************************************************************" << std::endl << std::endl;
  std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     -h:                          Show this help." << std::endl;
  std::cout << "     -k:                          Show keypoints." << std::endl;
  std::cout << "     --algorithm (Hough|GC):      Clustering algorithm used (default Hough)." << std::endl;
  std::cout << "     --model_ss val:              Model uniform sampling radius (default " << model_ss_ << ")" << std::endl;
  std::cout << "     --scene_ss val:              Scene uniform sampling radius (default " << scene_ss_ << ")" << std::endl;
  std::cout << "     --rf_rad val:                Reference frame radius (default " << rf_rad_ << ")" << std::endl;
  std::cout << "     --descr_rad val:             Descriptor radius (default " << descr_rad_ << ")" << std::endl;
  std::cout << "     --cg_size val:               Cluster size (default " << cg_size_ << ")" << std::endl;
  std::cout << "     --cg_thresh val:             Clustering threshold (default " << cg_thresh_ << ")" << std::endl << std::endl;
  std::cout << "     --icp_max_iter val:          ICP max iterations number (default " << icp_max_iter_ << ")" << std::endl;
  std::cout << "     --icp_corr_distance val:     ICP correspondence distance (default " << icp_corr_distance_ << ")" << std::endl << std::endl;
  std::cout << "     --hv_clutter_reg val:        Clutter Regularizer (default " << hv_clutter_reg_ << ")" << std::endl;
  std::cout << "     --hv_inlier_th val:          Inlier threshold (default " << hv_inlier_th_ << ")" << std::endl;
  std::cout << "     --hv_occlusion_th val:       Occlusion threshold (default " << hv_occlusion_th_ << ")" << std::endl;
  std::cout << "     --hv_rad_clutter val:        Clutter radius (default " << hv_rad_clutter_ << ")" << std::endl;
  std::cout << "     --hv_regularizer val:        Regularizer value (default " << hv_regularizer_ << ")" << std::endl;
  std::cout << "     --hv_rad_normals val:        Normals radius (default " << hv_rad_normals_ << ")" << std::endl;
  std::cout << "     --hv_detect_clutter val:     TRUE if clutter detect enabled (default " << hv_detect_clutter_ << ")" << std::endl << std::endl;
}

/**
 * Parses Command Line Arguments (Argc,Argv)
 * @param argc
 * @param argv
 */
void
parseCommandLine (int argc,
                  char *argv[])
{
  //Show help
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }

  //Model & scene filenames
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 2)
  {
    std::cout << "Filenames missing.\n";
    showHelp (argv[0]);
    exit (-1);
  }

  model_filename_ = argv[filenames[0]];
  scene_filename_ = argv[filenames[1]];

  //Program behavior
  if (pcl::console::find_switch (argc, argv, "-k"))
  {
    show_keypoints_ = true;
  }

  std::string used_algorithm;
  if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
  {
    if (used_algorithm.compare ("Hough") == 0)
    {
      use_hough_ = true;
    }
    else if (used_algorithm.compare ("GC") == 0)
    {
      use_hough_ = false;
    }
    else
    {
      std::cout << "Wrong algorithm name.\n";
      showHelp (argv[0]);
      exit (-1);
    }
  }

  //General parameters
  pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
  pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
  pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
  pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
  pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
  pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
  pcl::console::parse_argument (argc, argv, "--icp_max_iter", icp_max_iter_);
  pcl::console::parse_argument (argc, argv, "--icp_corr_distance", icp_corr_distance_);
  pcl::console::parse_argument (argc, argv, "--hv_clutter_reg", hv_clutter_reg_);
  pcl::console::parse_argument (argc, argv, "--hv_inlier_th", hv_inlier_th_);
  pcl::console::parse_argument (argc, argv, "--hv_occlusion_th", hv_occlusion_th_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_clutter", hv_rad_clutter_);
  pcl::console::parse_argument (argc, argv, "--hv_regularizer", hv_regularizer_);
  pcl::console::parse_argument (argc, argv, "--hv_rad_normals", hv_rad_normals_);
  pcl::console::parse_argument (argc, argv, "--hv_detect_clutter", hv_detect_clutter_);
}

double
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
  double res = 0.0;
  int n_points = 0;
  int nres;
  std::vector<int> indices (2);
  std::vector<float> sqr_distances (2);
  pcl::search::KdTree<PointType> tree;
  tree.setInputCloud (cloud);

  for (std::size_t i = 0; i < cloud->size (); ++i)
  {
    if (! std::isfinite ((*cloud)[i].x))
    {
      continue;
    }
    //Considering the second neighbor since the first is the point itself.
    nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
    if (nres == 2)
    {
      res += sqrt (sqr_distances[1]);
      ++n_points;
    }
  }
  if (n_points != 0)
  {
    res /= n_points;
  }
  return res;
}

Eigen::Quaternionf ObjectDetect::ObjectRecognition(pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzCloudPtrFiltered, int i) {

  pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
  // pcl::PointCloud<PointType>::Ptr xyzCloudPtrFiltered1 (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
  pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
  pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
  pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());

  //
  //  Load clouds
  //

  // if (pcl::io::loadPCDFile ("/home/iqr/catkin_ws/src/kinova_unit_ros/kinova_unit_app/pcd/sense_watson.pcd", *xyzCloudPtrFiltered) < 0)
  // {
  //   std::cout << "Error loading scene cloud." << std::endl;
  //   // showHelp (argv[0]);
  //   return;
  // }

  // if (pcl::io::loadPCDFile ("/home/iqr/catkin_ws/src/kinova_unit_ros/kinova_unit_app/pcd/object_filter.pcd", *model) < 0)
  // {
  //   std::cout << "Error loading scene cloud." << std::endl;
  //   // showHelp (argv[0]);
  //   // return 0;
  // }
  std::string filename = "/home/iqr/catkin_ws/src/kinova_unit_ros/kinova_unit_app/pcd/object" + std::to_string(i) + ".pcd";
  if (pcl::io::loadPCDFile (filename, *model) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    // showHelp (argv[0]);
    // return 0;
  }

  /**
   * Compute Normals
   */
  pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
  norm_est.setKSearch (10);
  norm_est.setInputCloud (model);
  norm_est.compute (*model_normals);

  norm_est.setInputCloud (xyzCloudPtrFiltered);
  norm_est.compute (*scene_normals);

  /**
   *  Downsample Clouds to Extract keypoints
   */
  pcl::UniformSampling<PointType> uniform_sampling;
  uniform_sampling.setInputCloud (model);
  uniform_sampling.setRadiusSearch (model_ss_);
  uniform_sampling.filter (*model_keypoints);
  std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

  uniform_sampling.setInputCloud (xyzCloudPtrFiltered);
  uniform_sampling.setRadiusSearch (scene_ss_);
  uniform_sampling.filter (*scene_keypoints);
  std::cout << "Scene total points: " << xyzCloudPtrFiltered->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;

  /**
   *  Compute Descriptor for keypoints
   */
  pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
  descr_est.setRadiusSearch (descr_rad_);

  descr_est.setInputCloud (model_keypoints);
  descr_est.setInputNormals (model_normals);
  descr_est.setSearchSurface (model);
  descr_est.compute (*model_descriptors);

  descr_est.setInputCloud (scene_keypoints);
  descr_est.setInputNormals (scene_normals);
  descr_est.setSearchSurface (xyzCloudPtrFiltered);
  descr_est.compute (*scene_descriptors);

  /**
   *  Find Model-Scene Correspondences with KdTree
   */
  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
  pcl::KdTreeFLANN<DescriptorType> match_search;
  match_search.setInputCloud (model_descriptors);
  std::vector<int> model_good_keypoints_indices;
  std::vector<int> scene_good_keypoints_indices;

  for (std::size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);
    if (!std::isfinite (scene_descriptors->at (i).descriptor[0]))  //skipping NaNs
    {
      continue;
    }
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
    {
      pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
      model_scene_corrs->push_back (corr);
      model_good_keypoints_indices.push_back (corr.index_query);
      scene_good_keypoints_indices.push_back (corr.index_match);
    }
  }
  pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
  pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
  pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);

  std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

  /**
   *  Clustering
   */
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
  std::vector < pcl::Correspondences > clustered_corrs;

  if (use_hough_)
  {
    pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
    pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());

    pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
    rf_est.setFindHoles (true);
    rf_est.setRadiusSearch (rf_rad_);

    rf_est.setInputCloud (model_keypoints);
    rf_est.setInputNormals (model_normals);
    rf_est.setSearchSurface (model);
    rf_est.compute (*model_rf);

    rf_est.setInputCloud (scene_keypoints);
    rf_est.setInputNormals (scene_normals);
    rf_est.setSearchSurface (xyzCloudPtrFiltered);
    rf_est.compute (*scene_rf);

    //  Clustering
    pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
    clusterer.setHoughBinSize (cg_size_);
    clusterer.setHoughThreshold (cg_thresh_);
    clusterer.setUseInterpolation (true);
    clusterer.setUseDistanceWeight (false);

    clusterer.setInputCloud (model_keypoints);
    clusterer.setInputRf (model_rf);
    clusterer.setSceneCloud (scene_keypoints);
    clusterer.setSceneRf (scene_rf);
    clusterer.setModelSceneCorrespondences (model_scene_corrs);

    // clusterer.cluster (clustered_corrs);
    clusterer.recognize (rototranslations, clustered_corrs);
  }
  else
  {
    pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
    gc_clusterer.setGCSize (cg_size_);
    gc_clusterer.setGCThreshold (cg_thresh_);

    gc_clusterer.setInputCloud (model_keypoints);
    gc_clusterer.setSceneCloud (scene_keypoints);
    gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

    gc_clusterer.recognize (rototranslations, clustered_corrs);
  }

  /**
   * Stop if no instances
   */
  if (rototranslations.size () <= 0)
  {
    std::cout << "*** No instances found! ***" << std::endl;
  }
  else
  {
    std::cout << "Recognized Instances: " << rototranslations.size () << std::endl << std::endl;

    /**
     * Generates clouds for each instances found
     */
    std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;

    for (std::size_t i = 0; i < rototranslations.size (); ++i)
    {
      pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
      pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
      instances.push_back (rotated_model);
    }

    /**
     * ICP
     */
    std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
    if (true)
    {
      std::cout << "--- ICP ---------" << std::endl;

      for (std::size_t i = 0; i < rototranslations.size (); ++i)
      {
        pcl::IterativeClosestPoint<PointType, PointType> icp;
        icp.setMaximumIterations (icp_max_iter_);
        icp.setMaxCorrespondenceDistance (icp_corr_distance_);
        icp.setInputTarget (xyzCloudPtrFiltered);
        icp.setInputSource (instances[i]);
        pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
        icp.align (*registered);
        registered_instances.push_back (registered);
        std::cout << "Instance " << i << " ";
        if (icp.hasConverged ())
        {
          std::cout << "Aligned!" << std::endl;
        }
        else
        {
          std::cout << "Not Aligned!" << std::endl;
        }
      }

      std::cout << "-----------------" << std::endl << std::endl;
    }

    /**
     * Hypothesis Verification
     */
    std::cout << "--- Hypotheses Verification ---" << std::endl;
    std::vector<bool> hypotheses_mask;  // Mask Vector to identify positive hypotheses

    pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;

    GoHv.setSceneCloud (xyzCloudPtrFiltered);  // Scene Cloud
    GoHv.addModels (registered_instances, true);  //Models to verify
    GoHv.setResolution (hv_resolution_);
    // GoHv.setResolutionOccupancyGrid (hv_occupancy_grid_resolution_);
    GoHv.setInlierThreshold (hv_inlier_th_);
    GoHv.setOcclusionThreshold (hv_occlusion_th_);
    GoHv.setRegularizer (hv_regularizer_);
    GoHv.setRadiusClutter (hv_rad_clutter_);
    GoHv.setClutterRegularizer (hv_clutter_reg_);
    GoHv.setDetectClutter (hv_detect_clutter_);
    GoHv.setRadiusNormals (hv_rad_normals_);

    GoHv.verify ();
    GoHv.getMask (hypotheses_mask);  // i-element TRUE if hvModels[i] verifies hypotheses
    Eigen::Quaternionf q_bad;
    std::cout << "Model instances found: " << rototranslations.size () << std::endl;
    for (int i = 0; i < hypotheses_mask.size (); i++)
    {
      if (hypotheses_mask[i])
      {
        std::cout << "Instance " << i << " is GOOD! <---" << std::endl;

        std::cout << "\n    Instance " << i + 1 << ":" << std::endl;
        std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;

        // Print the rotation matrix and translation vector
        Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
        Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);

        printf ("\n");
        printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
        printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
        printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
        printf ("\n");
        printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));

        Eigen::Quaternionf q(rotation);
        // cout<<"q=\n"<<q.coeffs()<<endl<<endl;

        i = hypotheses_mask.size ();
        return q;
      }
      else
      {
        std::cout << "Instance " << i << " is bad!" << std::endl;
        // if (i == hypotheses_mask.size())
        // {
        //   q_bad.w() = 2.0;
        //   return q_bad;
        // }
      }
    }
    // return q_bad;
  }

  /**
   *  Visualization
   */
  // pcl::visualization::PCLVisualizer viewer ("Hypotheses Verification");
  // viewer.addPointCloud (xyzCloudPtrFiltered, "scene_cloud");

  // pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
  // pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());

  // pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
  // pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
  // pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
  // pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));

  // if (show_keypoints_)
  // {
  //   CloudStyle modelStyle = style_white;
  //   pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
  //   viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
  //   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
  // }

  // if (show_keypoints_)
  // {
  //   CloudStyle goodKeypointStyle = style_violet;
  //   pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
  //                                                                                goodKeypointStyle.b);
  //   viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
  //   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");

  //   pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
  //                                                                                goodKeypointStyle.b);
  //   viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
  //   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
  // }

  // for (std::size_t i = 0; i < instances.size (); ++i)
  // {
  //   std::stringstream ss_instance;
  //   ss_instance << "instance_" << i;

  //   CloudStyle clusterStyle = style_red;
  //   pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
  //   viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
  //   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());

  //   CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
  //   ss_instance << "_registered" << std::endl;
  //   pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
  //                                                                                registeredStyles.g, registeredStyles.b);
  //   viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
  //   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
  // }

  // while (!viewer.wasStopped ())
  // {
  //   viewer.spinOnce ();
  // }

}

void ObjectDetect::PointCloudCB(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
  pcl::PCLPointCloud2 debugPCL;
  sensor_msgs::PointCloud2 debug_output;

  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2* cloud_filtered = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2Ptr cloudFilteredPtr (cloud_filtered);

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform voxel grid downsampling filtering
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  // sor.setInputCloud (cloudPtr);
  // // sor.setLeafSize (0.005, 0.005, 0.005);
  // sor.setLeafSize (0.01, 0.01, 0.01);
  // sor.filter (*cloudFilteredPtr);

  pcl::PointCloud<pcl::PointXYZRGB> *xyz_cloud = new pcl::PointCloud<pcl::PointXYZRGB>;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzCloudPtr (xyz_cloud); // need a boost shared pointer for pcl function inputs

  // convert the pcl::PointCloud2 tpye to pcl::PointCloud<pcl::PointXYZRGB>
  pcl::fromPCLPointCloud2(*cloudPtr, *xyzCloudPtr);

  //perform passthrough filtering to remove table leg

  // create a pcl object to hold the passthrough filtered results
  pcl::PointCloud<pcl::PointXYZRGB> *xyz_cloud_filtered = new pcl::PointCloud<pcl::PointXYZRGB>;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzCloudPtrFiltered (xyz_cloud_filtered);

  // Create the filtering object
  pcl::PassThrough<pcl::PointXYZRGB> pass_x, pass_y, pass_z;
  pass_x.setInputCloud (xyzCloudPtr);
  pass_x.setFilterFieldName ("x");
  pass_x.setFilterLimits (-0.2, 0.4);
  pass_x.filter (*xyzCloudPtrFiltered);

  pass_y.setInputCloud (xyzCloudPtrFiltered);
  pass_y.setFilterFieldName ("y");
  pass_y.setFilterLimits (-0.33, 0);
  pass_y.filter (*xyzCloudPtrFiltered);

  pass_z.setInputCloud (xyzCloudPtrFiltered);
  pass_z.setFilterFieldName ("z");
  pass_z.setFilterLimits (0.8, 1.015);
  pass_z.filter (*xyzCloudPtrFiltered);

  // object recognition
  // ObjectDetect obj;
  // std::thread recognition(&ObjectDetect::ObjectRecognition, &obj,*pose_oritention);

  pcl::PointCloud<pcl::PointXYZRGB> *xyz_cloud_ransac_filtered = new pcl::PointCloud<pcl::PointXYZRGB>;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyzCloudPtrRansacFiltered (xyz_cloud_ransac_filtered);

  // perform ransac planar filtration to remove table top
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg1;
  // Optional
  seg1.setOptimizeCoefficients (true);
  // Mandatory
  seg1.setModelType (pcl::SACMODEL_PLANE);
  seg1.setMethodType (pcl::SAC_RANSAC);
  seg1.setDistanceThreshold (0.022);

  seg1.setInputCloud (xyzCloudPtrFiltered);
  seg1.segment (*inliers, *coefficients);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;

  //extract.setInputCloud (xyzCloudPtrFiltered);
  extract.setInputCloud (xyzCloudPtrFiltered);
  extract.setIndices (inliers);
  extract.setNegative (true);
  extract.filter (*xyzCloudPtrRansacFiltered);

  // ObjectRecognition(xyzCloudPtrRansacFiltered);

  pcl::toPCLPointCloud2( *xyzCloudPtrRansacFiltered ,debugPCL);
  pcl_conversions::fromPCL(debugPCL, debug_output);
  debug_pointcloud_pub_.publish(debug_output);

  // perform euclidean cluster segmentation to seporate individual objects

  if(xyzCloudPtrFiltered->empty() == true) {
    return;
  }

  // Create the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (xyzCloudPtrRansacFiltered);

  // create the extraction object for the clusters
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
  // specify euclidean cluster parameters
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (50);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (xyzCloudPtrRansacFiltered);
  // exctract the indices pertaining to each cluster and store in a vector of pcl::PointIndices
  ec.extract (cluster_indices);

  // declare the output variable instances
  sensor_msgs::PointCloud2 output;
  pcl::PCLPointCloud2 outputPCL;

  // ROS_INFO_STREAM(cluster_indices.size());
  Eigen::Isometry3d top_pose = Eigen::Isometry3d::Identity();
  Eigen::Isometry3d centroid_pose = Eigen::Isometry3d::Identity();
  object_visual_markers_pub_->deleteAllMarkers();
  geometry_msgs::PoseArray  pose_array;
  // here, cluster_indices is a vector of indices for each cluster. iterate through each indices object to work with them seporately
  ROS_INFO("%d", cluster_indices.size());
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {

    // create a pcl object to hold the extracted cluster
    pcl::PointCloud<pcl::PointXYZRGB> *cluster = new pcl::PointCloud<pcl::PointXYZRGB>;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr clusterPtr (cluster);

    // now we are in a vector of indices pertaining to a single cluster.
    // Assign each point corresponding to this cluster in xyzCloudPtrPassthroughFiltered a specific color for identification purposes
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
      clusterPtr->points.push_back(xyzCloudPtrRansacFiltered->points[*pit]);
    }

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*clusterPtr, centroid);

    Eigen::Vector4f minPoint, maxPoint;
    pcl::getMinMax3D(*clusterPtr, minPoint, maxPoint);

    top_pose.translation() << (maxPoint[0]+minPoint[0])/2.0, (maxPoint[1]+minPoint[1])/2.0, minPoint[2];
    centroid_pose.translation() << (maxPoint[0]+minPoint[0])/2.0, (maxPoint[1]+minPoint[1])/2.0, (maxPoint[2]+minPoint[2])/2.0;
    ROS_INFO("%f %f %f", maxPoint[0]-minPoint[0], maxPoint[1]-minPoint[1], maxPoint[2]-minPoint[2]);

    object_visual_markers_pub_->publishAxis(top_pose);
    object_visual_markers_pub_->publishWireframeCuboid(centroid_pose, maxPoint[0]-minPoint[0], maxPoint[1]-minPoint[1], maxPoint[2]-minPoint[2], rviz_visual_tools::GREEN);

    int x,y,z = -1;
    bool finded = false;
    Eigen::Quaternionf q;
    // q = ObjectRecognition(clusterPtr, 0);
    ROS_INFO("start");
    // while(!finded) {
    for (int i = 0; i < 3; ++i)
    {
      // if (i != x && i != y)
      // {
      //   /* code */
      // }
      q = ObjectRecognition(clusterPtr, i);
      // if (q.w() != 2)
      if (q.w() != 0)
      {
        // finded = true;
        i = 3;
      }
    }
    // }
    ROS_INFO("find");
    geometry_msgs::Pose pose;
    pose.position.x = top_pose.translation()[0];
    pose.position.y = top_pose.translation()[1];
    pose.position.z = top_pose.translation()[2];

    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();
    if (q.w() != 0)
    {
      pose_array.poses.push_back(pose);
    }
    // convert to pcl::PCLPointCloud2
    pcl::toPCLPointCloud2( *clusterPtr ,outputPCL);

    // Convert to ROS data type
    pcl_conversions::fromPCL(outputPCL, output);

  }
  // for (int i = 0; i < sizeof(pose_array); ++i)
  // {
    // pose_array[i].poses.orientation.x = ;
    // pose_array[i].poses.orientation.y = ;
    // pose_array[i].poses.orientation.z = ;
    // pose_array[i].poses.orientation.w = ;
  // }

  // pcl::toPCLPointCloud2(*cluster_indices ,debugPCL);
  // pcl_conversions::fromPCL(debugPCL, debug_output);
  // debug_pointcloud_pub_.publish(debug_output);

  pose_array.header.stamp = ros::Time::now();
  pose_array.header.frame_id = point_cloud_frame_;

  if(pose_array.poses.size() > 0) {
    latest_object_pose_.header.stamp = pose_array.header.stamp;
    latest_object_pose_.header.frame_id = pose_array.header.frame_id;
    latest_object_pose_.pose = pose_array.poses[0];
    object_pub_.publish(pose_array);
  }

  // object_visual_markers_pub_->trigger();
}

int main (int argc, char** argv) {
  ros::init (argc, argv, "object_detect_node");

  ros::NodeHandle nh, pnh("~");
  ObjectDetect object_detect(&nh, &pnh);
  // parseCommandLine (argc, argv);
  ros::spin();
}


/* *******************************************************************************
TODO: Add services to support its publishing behaviours.
********************************************************************************/

//#include <boost/date_time.hpp>
#include <trac_ik/trac_ik.hpp>
#include <ros/ros.h>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include "geometry_msgs/Pose.h"
#include "sensor_msgs/JointState.h"
#include <kdl_conversions/kdl_msg.h>
#include <vector>
#include <math.h>

geometry_msgs::Pose pose_command;
sensor_msgs::JointState robot_joint_states;
bool new_pose = true;

void pose_callback(const geometry_msgs::Pose::ConstPtr& msg){
    new_pose = true;
    pose_command=*msg;
}

void joint_state_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
  robot_joint_states=*msg;
}

double normalize_angel(double angel){
  while (angel < -M_PI)
    angel += 2*M_PI;
  while (angel > M_PI)
    angel -= 2*M_PI;
  return angel;
}
int main(int argc, char** argv)
{

  bool sim = true;
  if (strcmp(argv[1], "false") == 0)
    sim = false;
  ros::init(argc, argv, "ik_node");
  ros::NodeHandle nh("~");

  std::string chain_start, chain_end, urdf_param;
  std::vector<std::string> joint_names;
  double timeout;

  nh.param("chain_start", chain_start, std::string(""));
  nh.param("chain_end", chain_end, std::string(""));

  if (chain_start=="" || chain_end=="") {
    ROS_FATAL("Missing chain_start or chain_end parameter info in launch file.");
    exit (-1);
  }

  nh.param("timeout", timeout, 0.005);
  nh.param("urdf_param", urdf_param, std::string("/robot_description"));
  double eps = 0.02;

  TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);

  KDL::Chain chain;
  KDL::JntArray ll, ul; //lower joint limits, upper joint limits

  if (!tracik_solver.getKDLChain(chain)) {
    ROS_ERROR("There was no valid KDL chain found");
    return -1;
  }
  if (!tracik_solver.getKDLLimits(ll,ul)) {
    ROS_ERROR("There were no valid KDL joint limits found");
    return -1;
  }

  assert(chain.getNrOfJoints() == ll.data.size());
  assert(chain.getNrOfJoints() == ul.data.size());

  ROS_INFO ("Using %d joints",chain.getNrOfJoints());

  // Set up KDL IK and I  K
  KDL::ChainFkSolverPos_recursive fk_solver(chain); // Forward kin. solver
  //KDL::ChainIkSolverVel_pinv vik_solver(chain); // PseudoInverse vel solver
  //KDL::ChainIkSolverPos_NR_JL kdl_solver(chain,ll,ul,fk_solver, vik_solver, 1, eps); // Joint Limit Solver

  KDL::JntArray result(chain.getNrOfJoints());
  KDL::Frame end_effector_pose;

  // Initialize result with nominal configuration(midway between all joint limits)
  for (uint j=0; j<result.data.size(); j++) {
    result(j) = (ll(j)+ul(j))/2.0;
  }
  //fk_solver.JntToCart(result,end_effector_pose);

  while(!nh.hasParam("/joint_names")){
    ROS_INFO("Waiting for parameter '/joint_names'");
    sleep(1);
  }
  ROS_INFO("Get parameter '/joint_names'");
  nh.getParam("/joint_names",joint_names);

  sensor_msgs::JointState msg;
  msg.name = joint_names;
  msg.position.resize(joint_names.size());
  msg.velocity.resize(joint_names.size());
  robot_joint_states.position.resize(joint_names.size());
  robot_joint_states.velocity.resize(joint_names.size());
  // for(int i=0;i<joint_names.size();++i)
  //   msg.velocity[i] = 0.04;
  ros::Rate pub_rate(10);
  ros::Publisher pub = nh.advertise<sensor_msgs::JointState>("/joint_command",2);
  ros::Subscriber sub = nh.subscribe("/marker_pose", 2, pose_callback);
  ros::topic::waitForMessage<geometry_msgs::Pose>("/marker_pose");
  ros::Subscriber joint_state_sub = nh.subscribe("/joint_states",2,joint_state_callback);

  std::vector<double> angel_diff;
  angel_diff.resize(joint_names.size());
  double max_angel_diff;
  double max_vel=0.08;
  double run_time;

  int rc;
  while(ros::ok()){
    tf::poseMsgToKDL(pose_command,end_effector_pose);
    rc = tracik_solver.CartToJnt(result,end_effector_pose,result);
    if (rc>=0){//ik success
       ROS_INFO_STREAM("*** TRAC-IK successed. rc: "<<rc<<". Result: ");
       for(uint j=0; j<result.data.size(); j++)
       {
          ROS_INFO_STREAM(" "<<result(j));
          //ROS_INFO_STREAM("ik solution found.");
          msg.position[j] = result(j);
          if (sim == false)
          {
            angel_diff[j] = abs(msg.position[j]-robot_joint_states.position[j]);
          }
       }
       if (sim == false){
          max_angel_diff = *max_element(angel_diff.begin(),angel_diff.end());
          run_time = max_angel_diff / max_vel;
          if (run_time < 0.01)
            run_time = 0.01;
          for(uint j=0; j<result.data.size(); j++){
            msg.velocity[j] = angel_diff[j]/run_time;
            if (msg.velocity[j]<0.02)
              msg.velocity[j]=0.02;
          }
       }
       msg.header.stamp = ros::Time::now();
       new_pose = false;
       while(!new_pose){ //wait for new pose
         pub.publish(msg);
         ros::spinOnce();
         pub_rate.sleep();
       }
    }
    ros::spinOnce();
  }
  // Useful when you make a script that loops over multiple launch files that test different robot chains
  // std::vector<char *> commandVector;
  // commandVector.push_back((char*)"killall");
  // commandVector.push_back((char*)"-9");
  // commandVector.push_back((char*)"roslaunch");
  // commandVector.push_back(NULL);

  // char **command = &commandVector[0];
  // execvp(command[0],command);

  return 0;
}