//! heavyhash extracted from optical bitcoin
//! 2022 barrystyle

#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <search.h>//qsort
#include<time.h>

#include "obtc.h"


#define M 64
#define N 64

bool Is4BitPrecision(const uint64_t matrix[64*64])
{
    for (int i = 0; i < 64; ++i) {
        for (int j = 0; j < 64; ++j) {
            if (matrix[ i*64 + j] > 0xF)
                return false;
        }
    }
    return true;
}




double DiagonalMatrix_operator(DiagonalMatrix_t *p, int i, int j) 
{
	assert(i >= 0 && i < 64);
	assert(j >= 0 && j < 64);
	if (i == j) {
		return p->pBlock[i];
	} else {
		return 0.0;
	}
}

void DiagonalMatrix_release(DiagonalMatrix_t *p)
{
	if (p->pBlock != NULL){
		free(p->pBlock);
		p->pBlock = NULL;
	}
}

void DiagonalMatrix_init(DiagonalMatrix_t *p, const double values[]) 
{
	p->pBlock = (double *)malloc(sizeof(double)*M);
	//memset(pBlock, 0.0, sizeof(double)*L(64,64));
	memcpy(p->pBlock, values, sizeof(double) * M);
	
	p->operator = DiagonalMatrix_operator;
	p->release = DiagonalMatrix_release;

}

void DiagonalMatrix_DiagonalMatrix(DiagonalMatrix_t *p) 
{
	p->operator = DiagonalMatrix_operator;
	p->release = DiagonalMatrix_release;
}

//-----------------------------vector-------------------------------//

void vector_move(Vector_t *p, ptrdiff_t delta) {
	p->ptr += delta;
}

Vector_t vector_slice(Vector_t v, size_t start) {
	//assert(start >= 0 && start <= p->len);
	Vector_t v_tmp;
	v_tmp.pBlock = v.pBlock + start * v.delta;
	v_tmp.len = v.len - start;
	v_tmp.delta = v.delta;
	return v_tmp;
}

double Vector_column_operator(Vector_t *p, size_t idx){
	return p->pBlock[idx * p->delta];
}

double Vector_row_operator(Vector_t *p, size_t idx){
	return p->pBlock[idx * p->delta];
}

void Vector_sync(Matrix_t *p, size_t idx, Vector_t vec, int offset){
	for(int i = 0; i < vec.len; i++){
		p->pBlock[idx+(offset+i)*N] = vec.pBlock[i];
	}
}

void Vector_row_sync(Matrix_t *p, size_t idx, Vector_t vec, int offset){
	for(int i = 0; i < vec.len; i++){
		p->pBlock[offset+idx*N+i] = vec.pBlock[i];
	}
}


//-----------------------------Martrix-------------------------------//
Matrix_t Matrix_clone(Matrix_t *p) 
{
	Matrix_t m;
	
	m.pBlock = (double *)malloc(sizeof(double)*L(64,64));
	memcpy(m.pBlock, p->pBlock, sizeof(double)*L(64,64));

	return m;
}

void Matrix_filledwith(Matrix_t *p, const double values[]) 
{
	//p->pBlock = (double *)malloc(sizeof(double)*L(64,64));
	//memset(pBlock, 0.0, sizeof(double)*L(64,64));
	memcpy(p->pBlock, values, sizeof(double) * L(64,64));
}

double Matrix_operator(Matrix_t *p, int i, int j) 
{
	assert(i >= 0 && i < N);
	assert(j >= 0 && j < N);

	return p->pBlock[i*N+j];

}



Vector_t Matrix_row(Matrix_t *p, int i) 
{
	Vector_t vec_tmp;
	vec_tmp.len = N;
	vec_tmp.delta = 1;
	vec_tmp.pBlock = p->pBlock + i*N;
	//return Vector< const double >(this->pBlock + i * N, N, 1);
	return vec_tmp;

}

Vector_t Matrix_column(Matrix_t *p, int j) 
{
	Vector_t vec_tmp;
	vec_tmp.len = M;
	vec_tmp.delta = N;
	vec_tmp.pBlock = p->pBlock + j;

	return vec_tmp;
	//return Vector< double >(this->pBlock + j, M, N);

}

void Matrix_release(Matrix_t *p)
{
	if (p->pBlock != NULL){
		free(p->pBlock);
		p->pBlock = NULL;
	}
}

void Matrix_init(Matrix_t *p) 
{
	p->pBlock = (double *)malloc(sizeof(double)*L(64,64));
	memset(p->pBlock, 0.0, sizeof(double)*L(64,64));
	//memcpy(p->pBlock, values, sizeof(double) * L(64,64));
}

void Matrix_def(Matrix_t *p) 
{
	//p->clone = Matrix_clone;
	p->filledwith = Matrix_filledwith;
	p->operator = Matrix_operator;
	p->row = Matrix_row;
	p->column = Matrix_column;
	p->release = Matrix_release;
}






//-----------------------------Rotator-------------------------------//

double max(double a, double b)
{
	return a > b ? a : b;
}

double Rotator_operator(Rotator_t *p, int i, int j){
	assert(0 <= i && i < 2);
	assert(0 <= j && j < 2);
	return p->elements[i * 2 + j];
} 

void Rotator_init(Rotator_t *p, double x1, double x2)
{
	// normalizes by the maximum magnitude
	// to avoid harmful underflow and overflow
	double mx = max(fabs(x1), fabs(x2));
	
	x1 /= mx;
	x2 /= mx;
	double norm = sqrt(x1 * x1 + x2 * x2);
	double cs = x1 / norm;
	double sn = x2 / norm;
	p->elements[0] = cs;
	p->elements[1] = -sn;
	p->elements[2] = sn;
	p->elements[3] = cs;

	p->operator = Rotator_operator;
}

//-----------------------------Reflector-------------------------------//


void Reflector_transform(Reflector_t *p, double u0, size_t len){
	int i;
	for (i = 0; i < len; i++){
		p->u.pBlock[i] = p->u.pBlock[i] /u0;
	}
}

void Reflector_transform_left(Reflector_t *src1, Vector_t src2, Vector_t dst, double gUM, size_t len){
	int i;
	for (i = 0; i < len; i++){
		dst.pBlock[i] = src2.pBlock[i] - src1->u.pBlock[i] * gUM;
	}
}

void Reflector_transform_right(Reflector_t *src1, Vector_t src2, Vector_t dst, double gMU, size_t len){
	int i;
	for (i = 0; i < len; i++){
		dst.pBlock[i] = src2.pBlock[i] - gMU * src1->u.pBlock[i];
	}
}



void Reflector_init(Reflector_t *p, Vector_t v) {
	//assert(v.size() > 0 && v.size() <= L);
	//const size_t N = v.size();
	//const size_t p->L = sizeof(v)/sizeof(double);
	p->L = v.len;

	p->u.pBlock = (double *)malloc(sizeof(double)*v.len);
	memcpy(p->u.pBlock, v.pBlock, sizeof(double)*v.len);
	
	// normalizes elements by the maximum amplitude
	// to avoid harmful underflow and overflow

	double mx = 0.0;

	for (size_t i = 0; i < p->L; ++i) {
		mx = max(fabs(p->u.pBlock[i]), mx);
	}

	if (mx > 0.0) {
		// calculates the normalized norm
		double tau = 0.0;
		for (size_t i = 0; i < p->L; ++i) {
			double x = p->u.pBlock[i] / mx;
			p->u.pBlock[i] = x;
			tau += x * x;
		}
		tau = sqrt(tau);
		// tau's sign should be the same as the first element in `u`
		if (p->u.pBlock[0] < 0.0) {
			tau = -tau;
		}
		double u0 = p->u.pBlock[0] + tau;
		p->u.pBlock[0] = u0;
		Reflector_transform(p, u0, p->L);

		p->gamma = u0 / tau;
	} else {
		// v is a zero vector
		p->gamma = 0.0;
		memset(p->u.pBlock, 0.0, p->L);
	}
}

void Reflector_release(Reflector_t *p){
	if (p->u.pBlock != NULL){
		free(p->u.pBlock);
		p->u.pBlock = NULL;
	}
}


double inner_product(double *a,double *b,int n){
	int i;
	double sum = 0.0;
	
	for(i = 0; i < n; i++)
	{
		sum += (*(a+i))*(*(b+i));
	}
	return sum;
}

Matrix_t Reflector_applyFromLeftTo(Reflector_t *p, Matrix_t m){
	// H * m = m - gamma * u * u^T * m
	Matrix_t m2 = Matrix_clone(&m);//m->clone(m);
	Vector_t vec_m;
	Vector_t vec_m2;

	int offset = N - p->L;
	for (int i = 0; i < N; ++i) {
		// caches gamma * u^T * m
		vec_m = Matrix_column(&m, i);
	
		Vector_t srcColumn = vector_slice(vec_m, offset);
		double v_src_column[srcColumn.len];
	
		for(size_t i = 0; i < srcColumn.len; i++){
			v_src_column[i] = Vector_column_operator(&srcColumn, i);
		}
		srcColumn.pBlock = v_src_column;

		double gUM = inner_product(p->u.pBlock, srcColumn.pBlock, p->L);
		//Vector< const double > srcColumn = m->column(m, i).slice(offset);
		
		gUM *= p->gamma;
		// H * m = m - u * gUM
		vec_m2 = Matrix_column(&m2, i);
		Vector_t dstColumn = vector_slice(vec_m2, offset);
		double v_dstcolumn[dstColumn.len];
	
		for(size_t i = 0; i < dstColumn.len; i++){
			v_dstcolumn[i] = Vector_column_operator(&dstColumn, i);
		}
		dstColumn.pBlock = v_dstcolumn;

		Reflector_transform_left(p, srcColumn, dstColumn, gUM, p->L);
		Vector_sync(&m2, i, dstColumn, offset);
	}
	Matrix_release(&m);
	return m2;
}

Matrix_t Reflector_applyFromRightTo(Reflector_t *p,  Matrix_t m){
	// m * H = m - m * gamma * u * u^T
	Matrix_t m2 = Matrix_clone(&m);
	Vector_t vec_m;
	Vector_t vec_m2;

	int offset = 64 - p->L;

	for (int i = 0; i < M; ++i) {
		// caches gamma * m * u
		vec_m = Matrix_row(&m, i);
		Vector_t srcRow = vector_slice(vec_m, offset);

		double v_src_row[srcRow.len];
		for(size_t j = 0;  j< srcRow.len; j++){
			v_src_row[j] = Vector_row_operator(&srcRow, j);
		}
		srcRow.pBlock = v_src_row;

		double gMU = inner_product(p->u.pBlock, srcRow.pBlock, p->L);
		
		gMU *= p->gamma;
		// m * H = m - gMU * u^T
		vec_m2 = Matrix_row(&m2, i);
	
		Vector_t dstRow = vector_slice(vec_m2, offset);

		double v_dstrow[dstRow.len];
	
		for(size_t j = 0; j < dstRow.len; j++){
			v_dstrow[j] = Vector_row_operator(&dstRow, j);
		}
		dstRow.pBlock = v_dstrow;

		Reflector_transform_right(p ,srcRow, dstRow, gMU, p->L);
		Vector_row_sync(&m2, i, dstRow, offset);
	}
	Matrix_release(&m);
	return m2;
}


//-----------------------------Svd-------------------------------//
 
int cmp_double(const void* e1, const void* e2)
{
	if ((*(double*)e2 - *(double*)e1) > 0.00000)
		return 1;
	else if ((*(double*)e2 - *(double*)e1) == 0.000000)
		return 0;
	else
		return -1;
}

DiagonalMatrix_t Svd_decomposeUSV(BidiagonalMatrix_t *p, Matrix_t *m) {
	const int MAX_ITERATIONS = N * 10;
	// allocates matrices
	Matrix_t m1 = Matrix_clone(m);
	Matrix_def(&m1);

	

	// bidiagonalizes a given matrix
	BidiagonalMatrix_t m2 = p->bidiagonalize(p, m1);
	// repeats Francis iteration


	int iteration = 0;
	int n = N;
 
	while (n >= 2) {
		// processes the n-1 x n-1 submatrix
		// if the current n x n submatrix has converged
		double bn = m2.operator(&m2, n - 1, n - 1);
	
		if (bn == 0.0 || fabs(m2.operator(&m2, n - 2, n - 1) / bn) < 1.0e-15) {
			--n;
		} else {
			// aborts if too many iterations
			++iteration;
			if (iteration > MAX_ITERATIONS) {
				break;
			}
			m2.doFrancis(&m2, n);
		}
	}

	// copies the diagonal elements
	// and makes all singular values positive
	double ss[N];
	for (int i = 0; i < N; ++i) {
		if (m2.operator(&m2, i, i) < 0) {
			ss[i] = -m2.operator(&m2, i, i);
			// inverts the sign of the right singular vector
			//Vector< double > vi = v.column(i);
			//std::transform(
			//	vi.begin(), vi.end(), vi.begin(),
			//	[](double x) {
			//		return -x;
			//	});
		} else {
			ss[i] = m2.operator(&m2, i, i);
		}
	}

	// sorts singular values in descending order if necessary
	int shuffle[M];  // M >= N
	bool sortNeeded = false;
	for (int i = 0; i < M; ++i) {
		shuffle[i] = i;
		sortNeeded = sortNeeded || (i < N - 1 && ss[i] < ss[i + 1]);
	}

	m1.release(&m1);
	BidiagonalMatrix_release(p);


	DiagonalMatrix_t dm;
	if (sortNeeded) {
		// shuffles the N (<= M) singular values
		qsort(ss, N,sizeof(double), cmp_double);
		
		double ss2[M];
	
		memcpy(ss2, ss, M*sizeof(double));
		DiagonalMatrix_init(&dm, ss2); 

		return dm;
	} else {
		DiagonalMatrix_init(&dm, ss); 
		return dm;
	}
}




bool Svd_isFullRank(DiagonalMatrix_t *p, const int size) {
	const double round_off = 1.000009e-12;
	for (int i = 0; i < size; ++i) {
		if (fabs( p->operator(p, i, i) ) < round_off){
			p->release(p);
			return false;
		}
	}
	p->release(p);
	return true;
}


//-----------------------------BidiagonalMatrix_t-------------------------------//
BidiagonalMatrix_t BidiagonalMatrix_bidiagonalize(BidiagonalMatrix_t *p, Matrix_t m)
{
	assert(M >= N);

	Vector_t vec_m;
	Vector_t vec_m2;

	for (int i = 0; i < N; ++i) {
		Reflector_t rU;
	
		vec_m = Matrix_column(&m, i);
		Vector_t column_slice = vector_slice(vec_m, i);
		// applies a householder transform to the column vector i
			
		double v_column[column_slice.len];
	
		for(size_t i = 0; i < column_slice.len; i++){
			v_column[i] = Vector_column_operator(&column_slice, i);
		}
		column_slice.pBlock = v_column;
		
		Reflector_init(&rU, column_slice);
	
		m = Reflector_applyFromLeftTo(&rU, m);

		Reflector_release(&rU);
		//u = rU.applyFromRightTo(u);  // U1^T*U0^T = U0*U1
		if (i < N - 1) {
			// applies a householder transform to the row vector i + 1
			//Reflector< N > rV(m.row(i).slice(i + 1));
			Reflector_t rV;
			vec_m2 = Matrix_row(&m, i);
			Vector_t row_slice = vector_slice(vec_m2, i+1);

			double v_row[row_slice.len];

			for(size_t i = 0; i < row_slice.len; i++){
				v_row[i] = Vector_row_operator(&row_slice, i);
			}
			row_slice.pBlock = v_row;
			Reflector_init(&rV, row_slice);
			
			m = Reflector_applyFromRightTo(&rV, m);
			//m = rV.applyFromRightTo(m);
			//v = rV.applyFromRightTo(v);
			
			Reflector_release(&rV);
			
		}
	}

	BidiagonalMatrix_init(p, &m); 
	return *p;
}

void BidiagonalMatrix_release(BidiagonalMatrix_t *p)
{
	if (p->pBlock != NULL){
		free(p->pBlock);
		p->pBlock = NULL;
	}
}

double BidiagonalMatrix_operator(BidiagonalMatrix_t *p, int i, int j)
{
	assert(i >= 0 && i < M);
	assert(j >= 0 && j < N);
	if (i == j) {
		return p->pBlock[2 * i];
	} else if (i + 1 == j) {
		return p->pBlock[2 * i + 1];
	} else {
		return 0.0;
	}

}

double BidiagonalMatrix_applyFirstRotatorFromRight(BidiagonalMatrix_t *p, Rotator_t *r) 
{
	double b1 = p->pBlock[0];
	double g1 = p->pBlock[1];
	double b2 = p->pBlock[2];
	double r11 = Rotator_operator(r, 0, 0);//r->operator(r, 0, 0);
	double r12 = Rotator_operator(r, 0, 1);//r->operator(r, 0, 1);
	double r21 = Rotator_operator(r, 1, 0);//r->operator(r, 1, 0);
	double r22 = Rotator_operator(r, 1, 1);//r->operator(r, 1, 1);
	//Rotator_operator

	p->pBlock[0] = b1 * r11 + g1 * r21;
	p->pBlock[1] = b1 * r12 + g1 * r22;
	p->pBlock[2] = b2 * r22;
	return b2 * r21;
}

double BidiagonalMatrix_applyRotatorFromRight(BidiagonalMatrix_t *ptr, Rotator_t *r, int n, double bulge)
{
	double* p = ptr->pBlock + n * 2;
	double g0 = p[-1];
	double b1 = p[0];
	double g1 = p[1];
	double b2 = p[2];
	double r11 = r->operator(r, 0, 0);
	double r12 = r->operator(r, 0, 1);
	double r21 = r->operator(r, 1, 0);
	double r22 = r->operator(r, 1, 1);
	p[-1] = g0 * r11 + bulge * r21;
	p[0] = b1 * r11 + g1 * r21;
	p[1] = b1 * r12 + g1 * r22;
	p[2] = b2 * r22;
	return b2 * r21;
}

double BidiagonalMatrix_applyRotatorFromLeft(BidiagonalMatrix_t *ptr, Rotator_t *r, int n, double bulge) 
{
		double* p = ptr->pBlock + n * 2;
		double b1 = p[0];
		double g1 = p[1];
		double b2 = p[2];
		double r11 = r->operator(r, 0, 0);
		double r12 = r->operator(r, 0, 1);
		double r21 = r->operator(r, 1, 0);
		double r22 = r->operator(r, 1, 1);

		p[0] = r11 * b1 + r21 * bulge;
		p[1] = r11 * g1 + r21 * b2;
		p[2] = r12 * g1 + r22 * b2;
		double newBulge;
		if (n < N - 2) {
			double g2 = p[3];
			newBulge = r21 * g2;
			p[3] = r22 * g2;
		} else {
			newBulge = 0.0;
		}
		return newBulge;
}

double BidiagonalMatrix_calculateShift(BidiagonalMatrix_t *m, int n) 
{
	assert(M >= N);
	assert(n >= 2);
	double b1 = m->operator(m, n - 2, n - 2);
	double b2 = m->operator(m, n - 1, n - 1);
	double g1 = m->operator(m, n - 2, n - 1);

	// solves lambda^4 - d*lambda^2 + e = 0
	// where
	//  d = b1^2 + b2^2 + g1^2
	//  e = b1^2 * b2^2
	// chooses lambda (rho) closest to b2
	double rho;
	double d = b1 * b1 + b2 * b2 + g1 * g1;
	double e = b1 * b1 * b2 * b2;
	// lambda^2 = (d +- sqrt(d^2 - 4e)) / 2
	// so, f = d^2 - 4e must be positive
	double f = d * d - 4 * e;
	
	if (f >= 0) {
		f = sqrt(f);
		// lambda = +-sqrt(d +- f)  (d >= 0, f >= 0)
		// if d > f, both d+f and d-f have real square roots
		// otherwise considers only d+f
		if (d > f) {
			// lets l1 > l2
			double l1 = sqrt((d + f) * 0.5);
			double l2 = sqrt((d - f) * 0.5);
			// if b2 >= 0, chooses a positive shift
			// otherwise chooses a negative shift
			if (b2 >= 0) {
				if (fabs(b2 - l1) < fabs(b2 - l2)) {
					rho = l1;
				} else {
					rho = l2;
				}
			} else {
				if (fabs(b2 + l1) < fabs(b2 + l2)) {
					rho = -l1;
				} else {
					rho = -l2;
				}
			}
		} else {
			double l1 = sqrt((d + f) * 0.5);
			if (fabs(b2 - l1) <= fabs(b2 + l1)) {
				rho = l1;
			} else {
				rho = -l1;
			}
		}
	} else {
		// no solution. chooses b2 as the shift
		rho = b2;
	}
	
	return rho;
}



void BidiagonalMatrix_doFrancis(BidiagonalMatrix_t *m, int n)
{
	assert(M >= N);
	assert(n >= 2);
	// calculates the shift
	double rho = m->calculateShift(m, n);

	// applies the first right rotator
	double b1 = m->operator(m, 0, 0);
	double g1 = m->operator(m, 0, 1);
	double mx = max(fabs(rho), max(fabs(b1), fabs(g1)));
	rho /= mx;
	b1 /= mx;
	g1 /= mx;
	//Rotator_t r0(b1 * b1 - rho * rho, b1 * g1);
	
	Rotator_t r0;
	Rotator_init(&r0, b1 * b1 - rho * rho, b1 * g1);

	double bulge = m->applyFirstRotatorFromRight(m, &r0);
	//v = r0.applyFromRightTo(&r0, v, 0);
	// applies the first left rotator

	Rotator_t r1;
	Rotator_init(&r1, m->operator(m, 0, 0), bulge);
	//Rotator_t r1(m(0, 0), bulge);
	bulge = m->applyRotatorFromLeft(m, &r1, 0, bulge);
	//u = r1.applyFromRightTo(&r1, u, 0);	// U1^T*U0^T = U0*U1

	for (int i = 1; i + 1 < n; ++i) {
		// calculates (i+1)-th right rotator
		//Rotator rV(m(i - 1, i), bulge);
		Rotator_t rV;
		Rotator_init(&rV, m->operator(m, i - 1, i), bulge);
	
		bulge = m->applyRotatorFromRight(m, &rV, i, bulge);
		//v = rV.applyFromRightTo(&rV, v, i);
		// calculates (i+1)-th left rotator
		//Rotator rU(m(i, i), bulge);
		Rotator_t rU;
		Rotator_init(&rU, m->operator(m, i, i), bulge);
		
		bulge = m->applyRotatorFromLeft(m, &rU, i, bulge);
		//u = rU.applyFromRightTo(rU, u, i);	// U1^T*U0^T = U0*U1
	}
}

void BidiagonalMatrix_def(BidiagonalMatrix_t *p)
{
	p->applyFirstRotatorFromRight = BidiagonalMatrix_applyFirstRotatorFromRight;
	p->applyRotatorFromLeft = BidiagonalMatrix_applyRotatorFromLeft;
	p->applyRotatorFromRight = BidiagonalMatrix_applyRotatorFromRight;
	p->bidiagonalize = BidiagonalMatrix_bidiagonalize;
	p->calculateShift = BidiagonalMatrix_calculateShift;
	p->doFrancis = BidiagonalMatrix_doFrancis;
	p->operator = BidiagonalMatrix_operator;
	p->releases = BidiagonalMatrix_release;	

}

void BidiagonalMatrix_init(BidiagonalMatrix_t *p, Matrix_t *m) 
{
	assert(M >= N);
	int len;
	len = 2 * N - 1;

	p->pBlock = (double *)malloc(sizeof(double)*len);
	memset(p->pBlock, 0.0,sizeof(double)*len);
	
	for (int i = 0; i < N; ++i) {
		p->pBlock[i * 2] = Matrix_operator(m, i, i);//m->operator(m, i, i);
		if (i < N - 1) {
			p->pBlock[i * 2 + 1] = Matrix_operator(m, i, i + 1);//m->operator(m, i, i + 1);
		}
	}
}


bool IsFullRank(const uint64_t matrix_[64*64])
{
    double matrix__ [64*64];
  //  Matrix<64, 64> matrix;
	
  	
    for (int i = 0; i < 64; ++i) {
        for (int j = 0; j < 64; ++j) {
            matrix__[64*i + j] = (double) matrix_[64*i + j];
        }
    }

	DiagonalMatrix_t dm;
    Matrix_t mt;
	BidiagonalMatrix_t bt;

	DiagonalMatrix_init(&dm, matrix__);
    //matrix.fill(matrix__);

	Matrix_init(&mt);
	Matrix_def(&mt);
    mt.filledwith(&mt, matrix__); 

	BidiagonalMatrix_def(&bt);
	DiagonalMatrix_t usv = Svd_decomposeUSV(&bt, &mt);
	DiagonalMatrix_t singularValues = usv;
	mt.release(&mt);
	dm.release(&dm);
	//DiagonalMatrix_release(&dm);
	return Svd_isFullRank(&usv,64);

	

}


uint64_t GetUint64_t(uint8_t *data, int pos)
{
	const uint8_t* ptr = data + pos * 8;
	return ((uint64_t)ptr[0]) | \
		   ((uint64_t)ptr[1]) << 8 | \
		   ((uint64_t)ptr[2]) << 16 | \
		   ((uint64_t)ptr[3]) << 24 | \
		   ((uint64_t)ptr[4]) << 32 | \
		   ((uint64_t)ptr[5]) << 40 | \
		   ((uint64_t)ptr[6]) << 48 | \
		   ((uint64_t)ptr[7]) << 56;
}

void XoShiRo256PlusPlus_init(Obtc_t *Obtc, uint64_t *s, uint256 seed) {
	for (int i = 0; i < 4; ++i) {
		//p->s[i] = seed.GetUint64(i);
		s[i] = GetUint64_t(Obtc->data_r,i);
	}
}

uint64_t RotateLeft64(const uint64_t x, int k) {
	return (x << k) | (x >> (64 - k));
}


uint64_t XoShiRo256PlusPlus_operator(uint64_t *s){
	const uint64_t result = RotateLeft64(s[0] + s[3], 23) + s[0];

	const uint64_t t = s[1] << 17;

	s[2] ^= s[0];
	s[3] ^= s[1];
	s[1] ^= s[2];
	s[0] ^= s[3];

	s[2] ^= t;

	s[3] = RotateLeft64(s[3], 45);

	return result;
}

void GenerateHeavyHashMatrix_t(Obtc_t *Obtc, uint256 matrix_seed, uint64_t matrix[64*64])
{
	XoShiRo256PlusPlus_init(Obtc, Obtc->ss, matrix_seed);

    do {
        for (int i = 0; i < 64; ++i) {
            for (int j = 0; j < 64; j += 16) {
                uint64_t value = XoShiRo256PlusPlus_operator(Obtc->ss);//generator();
                for (int shift = 0; shift < 16; ++shift) {
                    matrix[64*i + j + shift] =  (value >> (4 * shift)) & 0xF;
                }
            }
        }
    //} while (!Is4BitPrecision(matrix) || !IsFullRank(matrix)); 
    }while(!Is4BitPrecision(matrix));
}




void serialize_heavyhash(Obtc_t *Obtc, uint64_t matrix[64*64], const char* in, char* out, int len)
{
    uint8_t temp[200]={
        0x02,0xb9,0x7c,0x78,0x6f,0x82,0x43,0x83,0x5d,0x11,0x29,0xcf,0x82,0xaf,0xa5,0xbc,0xb1,0xfc,0xce,0x9c,
        0xe7,0x8b,0x52,0x72,0x48,0xb0,0x94,0x27,0xa8,0x74,0x2e,0xdb,0x89,0xca,0x4e,0x84,0x9b,0xce,0xcf,0x4a,
        0xd1,0x02,0x57,0x41,0x05,0x09,0x5f,0x8d,0xba,0x1d,0xe5,0xe4,0x45,0x16,0x68,0xe4,0xc1,0xa2,0x02,0x1d,
        0x56,0x3b,0xb1,0x42,0x8f,0x06,0xdd,0x1c,0x7a,0x2f,0x85,0x1a,0x34,0x85,0x54,0x90,0x64,0xa3,0x6a,0x46,
        0xb2,0x1a,0x60,0x1f,0x85,0xb4,0xb2,0x23,0xe6,0xc8,0x5d,0x8f,0x82,0xe9,0xda,0x89,0xec,0x70,0xf1,0xa4,
        0x25,0xb1,0x37,0x15,0x44,0xe3,0x67,0x87,0x5b,0x29,0x91,0x52,0x0f,0x96,0x07,0x05,0x40,0xf1,0x4a,0x0e,
        0x2e,0x65,0x1c,0x3c,0x43,0x28,0x5f,0xf0,0xf8,0xeb,0xf1,0x33,0x88,0x66,0x31,0x40,0x77,0x6b,0xf6,0x0c,
        0x78,0x9b,0xc2,0x9c,0x18,0x3a,0x98,0x1e,0xad,0x41,0x5b,0x10,0x4a,0xef,0x61,0xd6,0x29,0xdc,0xe2,0x46,
        0x7b,0x2f,0xaf,0xca,0x87,0x5e,0x2d,0x65,0x1b,0xa5,0xa4,0xa3,0xf5,0x98,0x69,0xa0,0x1e,0x5f,0x2e,0x72,
        0x0e,0xfb,0x44,0xd2,0x29,0xbf,0x88,0x55,0xb7,0x02,0x7e,0x3c,0x11,0x3c,0xff,0x0d,0xa1,0xf6,0xd8,0x3d
    };
    for(int i = 0 ;i< 200 ;i++)Obtc->const_data[i] = temp[i];

	CHeavyHash_init(Obtc, &Obtc->CHeavyHash_p, matrix);
	CHeavyHash_Write(&Obtc->CHeavyHash_p, (const unsigned char*)in, len);
	CHeavyHash_Finalize(Obtc, &Obtc->CHeavyHash_p, (unsigned char*)out);
}



void opticalbtc_hash(const char* in, char* out, int len)
{
	uint8_t *ptr = (uint8_t*) in;
	uint256 seed, hashprev;
	uint64_t matrix[64*64];

	Obtc_t Obtc;

	CSHA3_256_init(&Obtc, &Obtc.CSHA3_256_p);
	memcpy(Obtc.data_r,ptr, 32);
	GenerateHeavyHashMatrix_t(&Obtc, seed, matrix);
    serialize_heavyhash(&Obtc, matrix, in, out, len);

}