/*
 *
 *
 *
 */

#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <cblas.h>
#include <string.h> // memcpy
#include <math.h>
#include <stdarg.h>

#include "cgraph.h"
#include "cg_operation.h"
#include "cg_types.h"
#include "cg_variables.h"
#include "cg_errors.h"
#include "cg_constants.h"
#include "cg_enums.h"
#include "cg_factory.h"
#include "cg_math.h"
#include "cg_ops.h"

void cg_assert(int cond, const char * rawcond, const char * fmt, ...)
{
	if(cond)
		return;

	char temp[1024];
	va_list vl;
	va_start(vl, fmt);
	vsprintf(temp, fmt, vl);
	va_end(vl);
	fprintf(stdout, "Fatal error, assertion failed: %s\n", rawcond);
	fprintf(stdout, temp);
	fprintf(stdout, "\n");

	exit(-1);
}

#define CHECK_RESULT(node) \
if(node->error != NULL){\
	return node;\
}

/*
 * Works only with constant types
 */
CGNode* copyNode(CGNode* node){
	CGNode* n = calloc(1, sizeof(CGNode));
	if(node->type != CGNT_CONSTANT){
		fprintf(stderr, "Call to copyNode with a non-constant node.\n... This should not happen, but who knows these days.\n");
		fprintf(stderr, "copy node with nodetype = %d\n", node->type);
		exit(-1);
	}

	n->type = CGNT_CONSTANT;
	n->constant = calloc(1, sizeof(CGPConstant));
	n->constant->type = node->constant->type;
	n->result = NULL;
	n->diff = NULL;

	switch(node->constant->type){
		case CGVT_DOUBLE: {
			CGDouble* d = calloc(1, sizeof(CGDouble));
			d->value = ((CGDouble*)node->constant->value)->value;

			n->constant->value = d;
			break;
		}

		case CGVT_VECTOR: {
			CGVector* V = calloc(1, sizeof(CGVector));
			CGVector* src = (CGVector*)node->constant->value;

			V->len = src->len;
			if(src->data != NULL) {
				V->data = calloc(V->len, sizeof(cg_float));
				memcpy(V->data, src->data, V->len * sizeof(cg_float));
			}

#ifdef CG_USE_OPENCL
	        V->buf = NULL;
	        V->loc = CG_DATALOC_HOST_MEM;
#endif
			n->constant->value = V;
			break;
		}

		case CGVT_MATRIX: {
			CGMatrix* M = calloc(1, sizeof(CGMatrix));
			CGMatrix* src = (CGMatrix*)node->constant->value;

			uint64_t size = src->cols * src->rows;

			M->rows = src->rows;
			M->cols = src->cols;

			if(src->data != NULL) {
				M->data = calloc(size, sizeof(cg_float));
				memcpy(M->data, src->data, size*sizeof(cg_float));
			}

#ifdef CG_USE_OPENCL
			M->buf = NULL;
			M->loc = CG_DATALOC_HOST_MEM;
#endif
			n->constant->value = M;
			break;
		}
	}

	return n;
}

// @Deprecated @NotUsed
void* copyNodeValue(CGNode* node){
	if(node->type != CGNT_CONSTANT){
		fprintf(stderr, "Call to copyNodeValue with a non-constant node.\n... This should not happen, but who knows these days.");
		exit(-1);
	}

	switch(node->constant->type){
		case CGVT_DOUBLE: {
			CGDouble* d = calloc(1, sizeof(CGDouble));
			d->value = ((CGDouble*)node->constant->value)->value;

			return d;
		}

		case CGVT_VECTOR: {
			CGVector* V = calloc(1, sizeof(CGVector));
			CGVector* src = (CGVector*)node->constant->value;

			V->len = src->len;
			V->data = calloc(V->len, sizeof(cg_float));

			memcpy(V->data, src->data, V->len*sizeof(cg_float));
			return V;
		}

		case CGVT_MATRIX: {
			CGMatrix* M = calloc(1, sizeof(CGMatrix));
			CGMatrix* src = (CGMatrix*)node->constant->value;

			uint64_t size = src->cols * src->rows;

			M->rows = src->rows;
			M->cols = src->cols;
			M->data = calloc(size, sizeof(cg_float));

			memcpy(M->data, src->data, size*sizeof(cg_float));
			return M;
		}
	}
}

/*
 * @Deprecated
 */
void* copyRNodeValue(CGResultNode* node){
	switch(node->type){
		case CGVT_DOUBLE: {
			CGDouble* d = calloc(1, sizeof(CGDouble));
			d->value = ((CGDouble*)node->value)->value;

			return d;
		}

		case CGVT_VECTOR: {
			CGVector* V = calloc(1, sizeof(CGVector));
			CGVector* src = (CGVector*)node->value;

			V->len = src->len;
			V->data = calloc(V->len, sizeof(cg_float));

			memcpy(V->data, src->data, V->len*sizeof(cg_float));
			return V;
		}

		case CGVT_MATRIX: {
			CGMatrix* M = calloc(1, sizeof(CGMatrix));
			CGMatrix* src = (CGMatrix*)node->value;

			uint64_t size = src->cols * src->rows;

			M->rows = src->rows;
			M->cols = src->cols;
			M->data = calloc(size, sizeof(cg_float));

			memcpy(M->data, src->data, size*sizeof(cg_float));
			return M;
		}
	}
}

CGResultNode* copyResultNode(CGResultNode* node){
	CGResultNode* res = calloc(1, sizeof(CGResultNode));

	switch(node->type){
		case CGVT_DOUBLE: {
			CGDouble* d = calloc(1, sizeof(CGDouble));
			CGDouble* d2 = (CGDouble*)node->value;
			d->value = d2->value;

			res->value = d;
			break;
		}

		case CGVT_VECTOR: {
			CGVector* V = calloc(1, sizeof(CGVector));
			CGVector* src = (CGVector*)node->value;

			V->len = src->len;
			V->data = calloc(V->len, sizeof(cg_float));

#ifdef CG_USE_OPENCL
			V->buf = src->buf;
#endif
			memcpy(V->data, src->data, V->len*sizeof(cg_float));

			res->value = V;
			break;
		}

		case CGVT_MATRIX: {
			CGMatrix* M = calloc(1, sizeof(CGMatrix));
			CGMatrix* src = (CGMatrix*)node->value;

			uint64_t size = src->cols * src->rows;

			M->rows = src->rows;
			M->cols = src->cols;
			M->data = calloc(size, sizeof(cg_float));
#ifdef CG_USE_OPENCL
            M->buf = src->buf;
#endif

			memcpy(M->data, src->data, size*sizeof(cg_float));

			res->value = M;
			break;
		}
	}
	res->type = node->type;

	return res;
}

CGMatrix* vectorToMatrix(CGVector* v){
	CGMatrix* m = calloc(1, sizeof(CGMatrix));
	m->rows = v->len;
	m->cols = 1;
	m->data = v->data;

	return m;
}

/*
 * Computational Graph traversing
 */

CGResultNode* processUnaryOperation(CGraph* graph, CGUnaryOperationType type, CGNode* uhs, CGNode* parentNode){
	CGVarType uhsType = CGVT_DOUBLE;
	void* uhsValue = NULL;
	CGResultNode* newres = NULL;

	CGResultNode* lhsResult = computeCGNode(graph, uhs);
	CHECK_RESULT(lhsResult)
	uhsType = lhsResult->type;
	uhsValue = lhsResult->value;

	switch(type){
		case CGUOT_EXP:{
			if(uhsType == CGVT_DOUBLE){
				newres = expD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = expV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = expM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_LOG:{
			if(uhsType == CGVT_DOUBLE){
				newres = logD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = logV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = logM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_MINUS:{
			if(uhsType == CGVT_DOUBLE){
				CGDouble* rhs = calloc(1, sizeof(CGDouble));
				rhs->value = -1;

				CGResultNode* res = mulDD((CGDouble*)uhsValue, rhs, graph, parentNode);

				freeDoubleValue(rhs);

				parentNode->result = res;
				return res;
			}

			if(uhsType == CGVT_VECTOR){
				CGDouble* lhs = calloc(1, sizeof(CGDouble));
				lhs->value = -1;

				CGResultNode* res = mulDV(lhs, (CGVector*)uhsValue, graph, parentNode);

				free(lhs);
				parentNode->result = res;
				return res;
			}

			if(uhsType == CGVT_MATRIX){
				CGDouble* lhs = calloc(1, sizeof(CGDouble));
				lhs->value = -1;
				CGResultNode* res = mulDM(lhs, (CGMatrix*)uhsValue, graph, parentNode);

				free(lhs);

				parentNode->result = res;
				return res;
			}
			break;
		}

		case CGUOT_SIN:{
			if(uhsType == CGVT_DOUBLE){
				newres = sinD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = sinV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = sinM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_COS:{
			if(uhsType == CGVT_DOUBLE){
				newres = cosD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = cosV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = cosM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_TAN:{
			if(uhsType == CGVT_DOUBLE){
				newres = tanD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = tanV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = tanM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_TANH:{
			if(uhsType == CGVT_DOUBLE){
				newres = tanhD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = tanhV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = tanhM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGUOT_INV:{
			char msg[MAX_ERR_FMT_LEN];
			snprintf(msg, MAX_ERR_FMT_LEN, "Operation `%s` is not implemented/supported", getUnaryOperationTypeString(type));
			newres = returnResultError(graph, CGET_OPERATION_NOT_IMPLEMENTED, parentNode, msg);
		}

		case CGUOT_TRANSPOSE:{

			if(uhsType == CGVT_DOUBLE){
				newres = transposeD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = transposeV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = transposeM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			break;
		}

		case CGUOT_RELU:{
			if(uhsType == CGVT_DOUBLE){
				newres = reluD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = reluV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = reluM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			break;
		}

        case CGUOT_SOFTPLUS:{
			if(uhsType == CGVT_DOUBLE){
				newres = softplusD((CGDouble*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_VECTOR){
				newres = softplusV((CGVector*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if(uhsType == CGVT_MATRIX){
				newres = softplusM((CGMatrix*)uhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			break;
        }
	}
	char msg[MAX_ERR_FMT_LEN];
	snprintf(msg, MAX_ERR_FMT_LEN, "Operation [%s %s] cannot be applied", getVariableTypeString(uhsType), getUnaryOperationTypeString(type));
	newres = returnResultError(graph, CGET_INCOMPATIBLE_ARGS_EXCEPTION, parentNode, msg);
	return newres;
}

CGResultNode* processBinaryOperation(CGraph* graph, CGBinaryOperationType type, CGNode* lhs, CGNode* rhs, CGNode* parentNode){
	CGVarType lhsType = CGVT_DOUBLE;
	CGVarType rhsType = CGVT_DOUBLE;
	CGResultNode* newres = NULL;
	void* lhsValue = NULL;
	void* rhsValue = NULL;

	CGResultNode* lhsResult = computeCGNode(graph, lhs);
	CHECK_RESULT(lhsResult)
	lhsType = lhsResult->type;
	lhsValue = lhsResult->value;

	CGResultNode* rhsResult = computeCGNode(graph, rhs);
	CHECK_RESULT(rhsResult)
	rhsType = rhsResult->type;
	rhsValue = rhsResult->value;

	switch(type){
		case CGBOT_ADD:{
			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = addDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = addVD((CGVector*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_VECTOR)){
				newres = addVD((CGVector*)rhsValue, (CGDouble*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = addMD((CGMatrix*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_MATRIX)){
				newres = addMD((CGMatrix*)rhsValue, (CGDouble*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_VECTOR)){
				newres = addVV((CGVector*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_VECTOR)){
				newres = addMV((CGMatrix*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_MATRIX)){
				newres = addMV((CGMatrix*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_MATRIX)){
				newres = addMM((CGMatrix*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGBOT_SUB:{
			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = subDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = subVD((CGVector*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_VECTOR)){
				newres = subDV((CGDouble*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = subMD((CGMatrix*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_MATRIX)){
				newres = subDM((CGDouble*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_VECTOR)){
				newres = subVV((CGVector*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_MATRIX)){
				newres = subMM((CGMatrix*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_VECTOR)){
				newres = subMV((CGMatrix*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_MATRIX)){
				newres = subVM((CGVector*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGBOT_DIV:{
			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = divDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = divVD((CGVector*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_VECTOR)){
				newres = divDV((CGDouble*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_VECTOR)){
				newres = divVV((CGVector*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = divMD((CGMatrix*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_VECTOR)){
				newres = divMV((CGMatrix*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_MATRIX)){
				newres = divDM((CGDouble*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			break;
		}

		case CGBOT_MULT:{
			// TODO: update
			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_MATRIX)){
				newres = mulMM((CGMatrix*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			// TODO: update
			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_VECTOR)){
				newres = mulMV((CGMatrix*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			// TODO: update
			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_MATRIX)){
				newres = mulMV((CGMatrix*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = mulDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_VECTOR)){
				newres = mulDV((CGDouble*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = mulDV((CGDouble*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_MATRIX)){
				newres = mulDM((CGDouble*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = mulDM((CGDouble*)rhsValue, (CGMatrix*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_VECTOR)){
				newres = crossVV((CGVector*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGBOT_POW:{
			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = powDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = powVD((CGVector*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = powMD((CGMatrix*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGBOT_DOT: {
			/*
			 * The following are the same as MUL
			 */
			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_DOUBLE)){
				newres = mulDD((CGDouble*)lhsValue, (CGDouble*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_VECTOR)){
				newres = mulDV((CGDouble*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_DOUBLE)){
				newres = mulDV((CGDouble*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_DOUBLE) && (rhsType == CGVT_MATRIX)){
				newres = mulDM((CGDouble*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_DOUBLE)){
				newres = mulDM((CGDouble*)rhsValue, (CGMatrix*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			/* here starts dot specific impl */

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_MATRIX)){
				newres = dotMM((CGMatrix*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_MATRIX) && (rhsType == CGVT_VECTOR)){
				newres = dotMV((CGMatrix*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_MATRIX)){
				newres = dotVM((CGVector*)lhsValue, (CGMatrix*)rhsValue, graph, parentNode);
				//newres = dotMM(vectorToMatrix((CGVector*)lhsValue), (CGMatrix*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}

			/*
			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_MATRIX)){
				newres = mulMV((CGMatrix*)rhsValue, (CGVector*)lhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			*/

			if((lhsType == CGVT_VECTOR) && (rhsType == CGVT_VECTOR)){
				newres = dotVV((CGVector*)lhsValue, (CGVector*)rhsValue, graph, parentNode);
				parentNode->result = newres;
				return newres;
			}
			break;
		}

		case CGBOT_TMULT:{
			char msg[MAX_ERR_FMT_LEN];
			snprintf(msg, MAX_ERR_FMT_LEN, "Operation `%s` is not implemented/supported", getBinaryOperationTypeString(type));
			return returnResultError(graph, CGET_OPERATION_NOT_IMPLEMENTED, parentNode, msg);
		}
	}

	char msg[MAX_ERR_FMT_LEN];
	snprintf(msg, MAX_ERR_FMT_LEN, "Operation [%s %s %s] cannot be applied", getVariableTypeString(lhsType), getBinaryOperationTypeString(type), getVariableTypeString(rhsType));
	return returnResultError(graph, CGET_INCOMPATIBLE_ARGS_EXCEPTION, parentNode, msg);
}

CGResultNode* computeRawNode(CGNode* node){
	CGraph* tmp_G = makeGraph("temp_g");

	tmp_G->root = node;
	storeNodesInGraph(tmp_G, node);

	CGResultNode* res = computeGraph(tmp_G);
	res = copyResultNode(res);
	freeGraph(tmp_G);
	free(tmp_G);

	return res;
}

CGResultNode* computeCGNode(CGraph* graph, CGNode* node){
	CGResultNode* result = NULL;

	if(node->result != NULL){
		return node->result;
	}

	switch(node->type){
		case CGNT_CONSTANT:{
			result = constantNodeToResultNodeCopy(node);
			break;
		}

		case CGNT_VARIABLE:{
			if(graph == NULL)
			{
				char msg[MAX_ERR_FMT_LEN];
				snprintf(msg, MAX_ERR_FMT_LEN, "Cannot compute variable`%s` without the graph instance", node->var->name);
				return returnResultError(graph, CGET_NO_GRAPH_INSTANCE, node, msg);
			}

			CGNode* constantNode = *map_get(&graph->vars, node->var->name);
			if(constantNode == NULL)
			{
				char msg[MAX_ERR_FMT_LEN];
				snprintf(msg, MAX_ERR_FMT_LEN, "No variable `%s` was found in graph `%s`", node->var->name, graph!=NULL?graph->name:"[anonymous]");
				return returnResultError(graph, CGET_VARIABLE_DOES_NOT_EXIST, node, msg);
			}

			CGResultNode* rnode = computeCGNode(graph, constantNode);
			CHECK_RESULT(rnode)
			constantNode->result = rnode;
			node->result = copyResultNode(rnode);

			result = node->result;
			break;
		}
		case CGNT_BINARY_OPERATION:
			result = processBinaryOperation(graph, node->bop->type, node->bop->lhs, node->bop->rhs, node);
			break;
		case CGNT_UNARY_OPERATION:
			result = processUnaryOperation(graph, node->uop->type, node->uop->uhs, node);
			break;

		/*
		 * TODO: add this test to unittest
		 */
		case CGNT_AXIS_BOUND_OPERATION:
		{
            //CGResultNode* res = computeCGNode(graph, node->axop->uhs);

			switch(node->axop->type){
				case CGABOT_SUM:{
					CGResultNode* newres = computeCGNode(graph, node->axop->uhs);
					CHECK_RESULT(newres)

					if(newres->type == CGVT_DOUBLE){
						result = sumD((CGDouble*)newres->value, graph, node);
					}

					if(newres->type == CGVT_VECTOR){
						result = sumV((CGVector*)newres->value, graph, node);
					}

					if(newres->type == CGVT_MATRIX){
						result = sumM((CGMatrix*)newres->value, graph, node, node->axop->axis);
					}

					break;
				}

				case CGABOT_MAX:{
					result = max(node, graph);
					break;
				}

				case CGABOT_MIN:{
					result = min(node, graph);
					break;
				}

				case CGABOT_MEAN:{
					result = mean(node, graph);
					break;
				}

				case CGABOT_SOFTMAX:{
					char msg[MAX_ERR_FMT_LEN];
					snprintf(msg, MAX_ERR_FMT_LEN, "Operation [CGABOT_SOFTMAX] is not implemented/supported");
					return returnResultError(graph, CGET_OPERATION_NOT_IMPLEMENTED, node, msg);
					//CGResultNode* res = computeCGNode(graph, node->axop->uhs);
					//break;
				}

				case CGABOT_ARGMAX:{
				    result = argmax(node, graph);
                    break;
				}

                case CGABOT_ARGMIN:{
                    result = argmin(node, graph);
                    break;
                }

				default:
				    break;
			}

			break;
		}
		case CGNT_GRAPH:{
			result = computeGraph(node->graph);
			break;
			/*
			char msg[MAX_ERR_FMT_LEN];
			snprintf(msg, MAX_ERR_FMT_LEN, "Operation [GRAPH] is not implemented/supported");
			return returnResultError(graph, CGET_OPERATION_NOT_IMPLEMENTED, node, msg);
			*/
		}

		case CGNT_CROSS_ENTROPY_LOSS_FUNC:
		{
			CGResultNode* X_res = computeCGNode(graph, node->crossEntropyLoss->x);
			CGNode* X = resultNodeToConstantNodeCopy(X_res);

			CGNode* X_val = softmax_node(X);

			CGResultNode* x_res = computeRawNode(X_val);

			CGResultNode* y_res = computeCGNode(graph, node->crossEntropyLoss->y);

			result = crossEntropy(x_res, y_res, node->crossEntropyLoss->num_classes);

			freeResultNode(x_res);
			free(x_res);
			break;
		}
	}

	node->result = reduceDim(result);

	if(node->diff == NULL)
		switch(node->result->type){
			case CGVT_DOUBLE:
				node->diff = makeZeroDoubleConstantNode();
				break;
			case CGVT_VECTOR:{
				CGVector* v = (CGVector*)node->result ->value;
				node->diff = makeZeroVectorConstantNode(v->len);
				break;
			}

			case CGVT_MATRIX:{
				CGMatrix* v = (CGMatrix*)node->result ->value;
				node->diff = makeZeroMatrixConstantNode(v->rows, v->cols);
				break;
			}
		}

	return node->result;
}

CGResultNode* reduceDim(CGResultNode* result){
    //return result;

	switch(result->type){
		case CGVT_DOUBLE:{
			return result;
		}

		case CGVT_VECTOR:{
			CGVector* vec = (CGVector*)result->value;
			if (vec->len > 1)
				return result;

#ifdef CG_USE_OPENCL
			copyDataToHost(result);
#endif

			CGDouble* d = calloc(1, sizeof(CGDouble));
			d->value = vec->data[0];

			freeVectorValue(result->value);
			free(result->value);

			result->type = CGVT_DOUBLE;
			result->value = d;

			return result;
		}

		case CGVT_MATRIX:{
			CGMatrix* mat = (CGMatrix*)result->value;

			if((mat->rows>1) &&(mat->cols>1))
				return result;

			if((mat->rows == 1) && (mat->cols == 1)){
                copyDataToHost(result);

				CGDouble* d = calloc(1, sizeof(CGDouble));
				d->value = mat->data[0];

				freeMatrixValue(result->value);
				free(result->value);

				result->type = CGVT_DOUBLE;
				result->value = d;

				return result;
			}

			if(mat->rows == 1){
				CGVector* vec = calloc(1, sizeof(CGVector));
				vec->len = mat->cols;
				vec->data = mat->data;
				//TODO: Memory leak here
#ifdef CG_USE_OPENCL
				vec->buf = mat->buf;
#endif

				//freeMatrixValue(result->value);
				free(result->value);

				result->type = CGVT_VECTOR;
				result->value = vec;

				return result;
			}

			return result;
		}
	}
}

CGResultNode* computeGraph(CGraph* graph){
	resetGraphResultNodes(graph, graph->root);
	CGResultNode* res = computeCGNode(graph, graph->root);
	copyDataToHost(res);

	return res;
}

CGResultNode* computeGraphNode(CGraph* graph, CGNode* node){
	storeNodesInGraph(graph, node);
	resetGraphResultNodes(graph, node);
	return computeCGNode(graph, node);
}

void storeNodesInGraph(CGraph* graph, CGNode* node){
	int idx = -1;

	vec_find(&graph->nodes, node, idx);

	if(idx != -1)
		return;

	vec_push(&graph->nodes, node);

	switch(node->type){
		case CGNT_CONSTANT:
			break;
		case CGNT_VARIABLE:
			break;
		case CGNT_BINARY_OPERATION:
			storeNodesInGraph(graph, node->bop->lhs);
			storeNodesInGraph(graph, node->bop->rhs);
			break;
		case CGNT_UNARY_OPERATION:
			storeNodesInGraph(graph, node->uop->uhs);
			break;
		case CGNT_AXIS_BOUND_OPERATION:
			storeNodesInGraph(graph, node->axop->uhs);
			break;
		case CGNT_GRAPH:
			storeNodesInGraph(graph, node->graph->root);
			break;
		case CGNT_CROSS_ENTROPY_LOSS_FUNC:
			storeNodesInGraph(graph, node->crossEntropyLoss->x);
			storeNodesInGraph(graph, node->crossEntropyLoss->y);
			break;
	}
}

void resetGraphResultNodes(CGraph* graph, CGNode* node){
	if(node->result != NULL){
		freeResultNode(node->result);
		free(node->result);
		node->result = NULL;
	}

	if(node->diff != NULL){
		freeNode(graph, node->diff);
		free(node->diff);
		node->diff = NULL;
	}

	switch(node->type){
		case CGNT_CONSTANT:
			break;
		case CGNT_VARIABLE:{
			CGNode* var = graphGetVar(graph, node->var->name);
			if(var != NULL)
				resetGraphResultNodes(graph, var);
			break;
		}
		case CGNT_BINARY_OPERATION:
			resetGraphResultNodes(graph, node->bop->lhs);
			resetGraphResultNodes(graph, node->bop->rhs);
			break;
		case CGNT_UNARY_OPERATION:
			resetGraphResultNodes(graph, node->uop->uhs);
			break;
		case CGNT_AXIS_BOUND_OPERATION:
			resetGraphResultNodes(graph, node->axop->uhs);
			break;
		case CGNT_GRAPH:
			resetGraphResultNodes(graph, node->graph->root);
			break;
		case CGNT_CROSS_ENTROPY_LOSS_FUNC:
			resetGraphResultNodes(graph, node->crossEntropyLoss->x);
			resetGraphResultNodes(graph, node->crossEntropyLoss->y);
			break;
	}
}

void graphSetVar_lua(CGraph* graph, const char* name, CGNode* value){
	CGNode** old = map_get(&graph->vars, name);
	if(old != NULL){
		//freeNode(graph, *old);
		map_remove(&graph->vars, name);
	}

	int res = map_set(&graph->vars, name, value);
}


/* monte/gsl_monte_plain.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Michael Booth
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Plain Monte-Carlo. */

/* Author: MJB */

#ifndef __GSL_MONTE_PLAIN_H__
#define __GSL_MONTE_PLAIN_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdio.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct {
  size_t dim;
  double *x;
} gsl_monte_plain_state;

GSL_FUN int
gsl_monte_plain_integrate (const gsl_monte_function * f,
                           const double xl[], const double xu[],
                           const size_t dim,
                           const size_t calls,
                           gsl_rng * r,
                           gsl_monte_plain_state * state,
                           double *result, double *abserr);

GSL_FUN gsl_monte_plain_state* gsl_monte_plain_alloc(size_t dim);

GSL_FUN int gsl_monte_plain_init(gsl_monte_plain_state* state);

GSL_FUN void gsl_monte_plain_free (gsl_monte_plain_state* state);

__END_DECLS

#endif /* __GSL_MONTE_PLAIN_H__ */


///
/// @file
///
/// @author Mirko Myllykoski (mirkom@cs.umu.se), Umeå University
///
/// @internal LICENSE
///
/// Copyright (c) 2019-2020, Umeå Universitet
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice,
///    this list of conditions and the following disclaimer.
///
/// 2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation
///    and/or other materials provided with the distribution.
///
/// 3. Neither the name of the copyright holder nor the names of its
///    contributors may be used to endorse or promote products derived from this
///    software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
/// POSSIBILITY OF SUCH DAMAGE.
///

#include <starneig_config.h>
#include <starneig/configuration.h>
#include "cpu.h"
#include "../common/common.h"
#include "../common/sanity.h"
#include "../common/tiles.h"
#include "../common/trace.h"
#include <starpu.h>
#include <starpu_scheduler.h>
#include <hwloc.h>
#include <cblas.h>
#include <omp.h>

void starneig_hessenberg_cpu_prepare_column(
    void *buffers[], void *cl_args)
{
    // LAPACK subroutine that generates a real elementary reflector H
    extern void dlarfg_(int const *, double *, double *, int const *, double *);

    int i; // the index of the currect column inside the panel
    struct range_packing_info v_pi;
    starpu_codelet_unpack_args(cl_args, &i, &v_pi);

    int k = 0;

    double *Y = NULL; int ldY = 0;
    if (0 < i) {
        Y = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
        ldY = STARPU_MATRIX_GET_LD(buffers[k]);
        k++;
    }

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    int m = STARPU_MATRIX_GET_NX(buffers[k]);
    int nb = STARPU_MATRIX_GET_NY(buffers[k]);
    k++;

    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *P = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldP = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    // an intemediate vector interface for the trailing matrix operation
    struct starpu_vector_interface **v_i =
        (struct starpu_vector_interface **)buffers + k;
    k += v_pi.handles;

    // current column
    double *p = P+i*ldP;

    //
    // update the current column
    //

    if (0 < i) {

        // A <- A - Y * V' (update column from the right)
        cblas_dgemv(CblasColMajor, CblasNoTrans, m, i,
            -1.0, Y, ldY, V+i-1, ldV, 1.0, p, 1);

        //
        // update column from the left
        //

        // we use the last column of T as a work space
        double *w = T+(nb-1)*ldT;

        // w <- V1' * b1 (upper part)
        cblas_dcopy(i, p, 1, w, 1);
        cblas_dtrmv(
            CblasColMajor, CblasLower, CblasTrans, CblasUnit, i, V, ldV, w, 1);

        // w <- w + V2' * b2 (lower part)
        cblas_dgemv(CblasColMajor, CblasTrans, m-i, i,
            1.0, V+i, ldV, p+i, 1, 1.0, w, 1);

        // w <- T' * w
        cblas_dtrmv(
            CblasColMajor, CblasUpper, CblasTrans, CblasNonUnit, i,
            T, ldT, w, 1);

        // b2 <- b2 - V2 * w
        cblas_dgemv(CblasColMajor, CblasNoTrans, m-i, i,
            -1.0, V+i, ldV, w, 1, 1.0, p+i, 1);

        // b1 <- b1 - V1 * w
        cblas_dtrmv(
            CblasColMajor, CblasLower, CblasNoTrans, CblasUnit, i,
            V, ldV, w, 1);
        cblas_daxpy(i, -1.0, w, 1, p, 1);
    }

    //
    // compute the current unit vector
    //

    int height = m-i;
    double tau, *v = V+i*ldV+i;
    memcpy(v, p+i, height*sizeof(double));
    dlarfg_(&height, p+i, v+1, (const int[]){1}, &tau);
    v[0] = 1.0;

    //
    // copy the current unit vector to the intemediate vector interface
    //

    starneig_join_range(&v_pi, v_i, v, 1);

    //
    // set elements below the subdiagonal to zero
    //

    for (int j = i+1; j < m; j++)
        p[j] = 0.0;

    //
    // store tau for future use
    //

    T[i*ldT+i] = tau;
}

void starneig_hessenberg_cpu_compute_column(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi;
    struct range_packing_info v_pi, y_pi;
    starpu_codelet_unpack_args(cl_args, &A_pi, &v_pi, &y_pi);

    STARNEIG_EVENT_BEGIN(&A_pi, starneig_event_red);

    int k = 0;

    // involved trailing matrix tiles
    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += A_pi.handles;

    // intemediate vector interface for the trailing matrix operation
    struct starpu_vector_interface **v_i =
        (struct starpu_vector_interface **)buffers + k;
    k += v_pi.handles;

    // intemediate vector interface from the trailing matrix operation
    struct starpu_vector_interface **y_i =
        (struct starpu_vector_interface **)buffers + k;
    k += y_pi.handles;

    int t_rows = (A_pi.rend-1) / A_pi.bm + 1 - (A_pi.rbegin-1) / A_pi.bm;
    int t_cols = (A_pi.cend-1) / A_pi.bn + 1 - (A_pi.cbegin-1) / A_pi.bn;

    //
    // loop oper tile rows
    //

    for (int i = 0; i < t_rows; i++) {

        double *y = (double *) STARPU_VECTOR_GET_PTR(y_i[i]);

        int rbegin = MAX(     0,  A_pi.rbegin - i * A_pi.bm);
        int rend =   MIN(A_pi.bm, A_pi.rend   - i * A_pi.bm);

        //
        // loop over tile columns
        //

        for (int j = 0; j < t_cols; j++) {

            double *A = (double *) STARPU_MATRIX_GET_PTR(A_i[j*t_rows+i]);
            int ldA = STARPU_MATRIX_GET_LD(A_i[j*t_rows+i]);

            double *v = (double *) STARPU_VECTOR_GET_PTR(v_i[j]);

            int cbegin = MAX(      0, A_pi.cbegin - j * A_pi.bn);
            int cend =   MIN(A_pi.bn, A_pi.cend   - j * A_pi.bn);

            cblas_dgemv(
                CblasColMajor, CblasNoTrans, rend-rbegin, cend-cbegin,
                1.0, A+cbegin*ldA+rbegin, ldA, v+cbegin, 1, 1.0, y+rbegin, 1);
        }
    }

    STARNEIG_EVENT_END();
}

void starneig_hessenberg_cpu_finish_column(
    void *buffers[], void *cl_args)
{
    struct range_packing_info y_pi;
    int i;
    starpu_codelet_unpack_args(cl_args, &i, &y_pi);

    int k = 0;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int m = STARPU_MATRIX_GET_NX(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *Y = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldY = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    // intemediate vector interface from the trailing matrix operation
    struct starpu_vector_interface **y_i =
        (struct starpu_vector_interface **)buffers + k;
    k += y_pi.handles;

    double tau = T[i*ldT+i];
    double *v = V+i*ldV+i;

    //
    // finish Y update
    //

    starneig_join_range(&y_pi, y_i, Y+i*ldY, 0);

    // w <- V' * v (shared result)
    cblas_dgemv(CblasColMajor, CblasTrans, m-i, i,
        1.0, V+i, ldV, v, 1, 0.0, T+i*ldT, 1);

    // Y(:,i) <- Y(:,i) - Y * w
    cblas_dgemv(CblasColMajor, CblasNoTrans, m, i,
        -1.0, Y, ldY, T+i*ldT, 1, 1.0, Y+i*ldY, 1);

    cblas_dscal(m, tau, Y+i*ldY, 1);

    //
    // update T
    //

    // w <- tau * w
    cblas_dscal(i, -tau, T+i*ldT, 1);

    // T(0:i,i) = T * w
    cblas_dtrmv(
        CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit, i,
        T, ldT, T+i*ldT, 1);

    T[i*ldT+i] = tau;
}

void starneig_hessenberg_cpu_update_trail_right(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi;
    int nb, roffset, coffset;
    starpu_codelet_unpack_args(cl_args, &A_pi, &nb, &roffset, &coffset);

    STARNEIG_EVENT_BEGIN(&A_pi, starneig_event_blue);

    int m = A_pi.rend - A_pi.rbegin;
    int n = A_pi.cend - A_pi.cbegin;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[0]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[0]);

    double *Y = (double *) STARPU_MATRIX_GET_PTR(buffers[1]);
    int ldY = STARPU_MATRIX_GET_LD(buffers[1]);

    double *A = (double *) STARPU_MATRIX_GET_PTR(buffers[2]);
    int ldA = STARPU_MATRIX_GET_LD(buffers[2]);

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + 3;

    // join tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 0);

    // A <- Y V^T
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans,
        m, n, nb, -1.0, Y+roffset, ldY, V+coffset+nb-1, ldV, 1.0, A, ldA);

    // split tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 1);

    STARNEIG_EVENT_END();
}

void starneig_hessenberg_cpu_update_left_a(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi, W_pi;
    int nb, offset;
    starpu_codelet_unpack_args(cl_args, &A_pi, &W_pi, &nb, &offset);

    STARNEIG_EVENT_BEGIN(&A_pi, starneig_event_green);

    int m = A_pi.rend - A_pi.rbegin;
    int n = A_pi.cend - A_pi.cbegin;

    int k = 0;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *A = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldA = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *W = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldW = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *P = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldP = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += A_pi.handles;

    struct starpu_matrix_interface **W_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += W_pi.handles;

    // join A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 0);

    // join W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 0);

    // P <- A^T * V
    cblas_dgemm(
        CblasColMajor, CblasTrans, CblasNoTrans, n, nb, m,
        1.0, A, ldA, V+offset, ldV, 0.0, P, ldP);

    // P <- P * T
    cblas_dtrmm(
        CblasColMajor, CblasRight, CblasUpper, CblasNoTrans,
        CblasNonUnit, n, nb, 1.0, T, ldT, P, ldP);

    // W <- W + P
    for (int j = 0; j < nb; j++)
        cblas_daxpy(n, 1.0, P+j*ldP, 1, W+j*ldW, 1);

    // split W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 1);

    STARNEIG_EVENT_END();
}

void starneig_hessenberg_cpu_update_left_b(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi, W_pi;
    int nb, offset;
    starpu_codelet_unpack_args(cl_args, &A_pi, &W_pi, &nb, &offset);

    STARNEIG_EVENT_BEGIN(&packing_info, starneig_event_green);

    int m = A_pi.rend - A_pi.rbegin;
    int n = A_pi.cend - A_pi.cbegin;

    int k = 0;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *W = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldW = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *A = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldA = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    struct starpu_matrix_interface **W_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += W_pi.handles;

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += A_pi.handles;

    // join A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 0);

    // join W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 0);

    //  A <- A - V * W^T
    cblas_dgemm(
        CblasColMajor, CblasNoTrans, CblasTrans, m, n,
        nb, -1.0, V+offset, ldV, W, ldW, 1.0, A, ldA);

    // split A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 1);

    STARNEIG_EVENT_END();
}

void starneig_hessenberg_cpu_update_right_a(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi, W_pi;
    int nb, offset;
    starpu_codelet_unpack_args(cl_args, &A_pi, &W_pi, &nb, &offset);

    STARNEIG_EVENT_BEGIN(&packing_info, starneig_event_blue);

    int m = A_pi.rend - A_pi.rbegin;
    int n = A_pi.cend - A_pi.cbegin;

    int k = 0;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *T = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldT = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *A = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldA = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *W = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldW = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *P = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldP = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += A_pi.handles;

    struct starpu_matrix_interface **W_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += W_pi.handles;

    // join A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 0);

    // join W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 0);

    // P <- A * V
    cblas_dgemm(
        CblasColMajor, CblasNoTrans, CblasNoTrans, m, nb, n,
        1.0, A, ldA, V+offset, ldV, 0.0, P, ldP);

    // P <- P * T
    cblas_dtrmm(
        CblasColMajor, CblasRight, CblasUpper, CblasNoTrans,
        CblasNonUnit, m, nb, 1.0, T, ldT, P, ldP);

    // W <- W + P
    for (int j = 0; j < nb; j++)
        cblas_daxpy(m, 1.0, P+j*ldP, 1, W+j*ldW, 1);

    // split W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 1);

    STARNEIG_EVENT_END();
}

void starneig_hessenberg_cpu_update_right_b(
    void *buffers[], void *cl_args)
{
    struct packing_info A_pi, W_pi;
    int nb, offset;
    starpu_codelet_unpack_args(cl_args, &A_pi, &W_pi, &nb, &offset);

    STARNEIG_EVENT_BEGIN(&packing_info, starneig_event_blue);

    int m = A_pi.rend - A_pi.rbegin;
    int n = A_pi.cend - A_pi.cbegin;

    int k = 0;

    double *V = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldV = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *W = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldW = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    double *A = (double *) STARPU_MATRIX_GET_PTR(buffers[k]);
    int ldA = STARPU_MATRIX_GET_LD(buffers[k]);
    k++;

    struct starpu_matrix_interface **W_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += W_pi.handles;

    struct starpu_matrix_interface **A_i =
        (struct starpu_matrix_interface **)buffers + k;
    k += A_pi.handles;

    // join A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 0);

    // join W tiles
    starneig_join_window(&W_pi, ldW, W_i, W, 0);

    //  A <- A - W * V
    cblas_dgemm(
        CblasColMajor, CblasNoTrans, CblasTrans, m, n,
        nb, -1.0, W, ldW, V+offset, ldV, 1.0, A, ldA);

    // split A tiles
    starneig_join_window(&A_pi, ldA, A_i, A, 1);

    STARNEIG_EVENT_END();
}


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2018, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ndantam@miens.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

// uncomment to check that local allocs actually get freed
// #define AA_ALLOC_STACK_MAX 0

#include <stdlib.h>
#include <cblas.h>

#include "amino.h"
#include "amino/mat.h"
#include "amino/mat_internal.h"

#define VEC_LEN(X) ((int)(X->len))
#define MAT_ROWS(X) ((int)(X->rows))
#define MAT_COLS(X) ((int)(X->cols))

/******************/
/* Error Handling */
/******************/

static void
s_err_default( const char *message )
{
    fprintf(stderr, "AMINO ERROR: %s\n",message);
    abort();
    exit(EXIT_FAILURE);
}

static aa_la_err_fun  *s_err_fun = s_err_default;

void
aa_la_set_err( aa_la_err_fun *fun )
{
    s_err_fun = fun;
}

AA_API void
aa_la_err( const char *message ) {
    s_err_fun(message);
}

AA_API void
aa_la_fail_size( size_t a, size_t b )
{
        const size_t size = 256;
        char buf[size];
        snprintf(buf,size, "Mismatched sizes: %lu != %lu\n", a, b);
        aa_la_err(buf);
}

/****************/
/* Construction */
/****************/

AA_API void
aa_dvec_view( struct aa_dvec *vec, size_t len, double *data, size_t inc )
{
    *vec = AA_DVEC_INIT(len,data,inc);
}

AA_API void
aa_dvec_slice( const struct aa_dvec *src,
               size_t start,
               size_t stop,
               size_t step,
               struct aa_dvec *dst )
{
    if( stop > src->len || stop < start ) {
        aa_la_err("Slice out-of-bounds\n");
    }
    *dst = AA_DVEC_INIT( (stop - start) / step,
                         src->data + src->inc*start,
                         src->inc*step );
}

AA_API void
aa_dmat_row_vec( const struct aa_dmat *src, size_t row, struct aa_dvec *dst )
{
    if( row >= src->rows ) {
        aa_la_err("Row vector out-of-bounds\n");
    }
    aa_dvec_view(dst,src->cols, src->data + row, src->ld);
}

AA_API void
aa_dmat_col_vec( const struct aa_dmat *src, size_t col, struct aa_dvec *dst )
{
    if( col >= src->cols ) {
        aa_la_err("Row vector out-of-bounds\n");
    }
    aa_dvec_view(dst, src->rows, src->data + col*src->ld, 1);
}

AA_API void
aa_dmat_diag_vec( const struct aa_dmat *src, struct aa_dvec *dst )
{
    aa_dvec_view(dst, AA_MIN(src->rows, src->cols), src->data, src->ld + 1);
}

AA_API void
aa_dmat_view( struct aa_dmat *mat, size_t rows, size_t cols,
              double *data, size_t ld )
{
    *mat = AA_DMAT_INIT(rows,cols,data,ld);
}

AA_API void
aa_dmat_view_block(  struct aa_dmat *dst,
                     const struct aa_dmat *src,
                     size_t row_start, size_t col_start,
                     size_t rows, size_t cols )
{
    aa_dmat_block( src,
                   row_start, col_start,
                   row_start + rows, col_start + cols,
                   dst );
}

AA_API void
aa_dmat_block( const struct aa_dmat *src,
               size_t row_start, size_t col_start,
               size_t row_end, size_t col_end,
               struct aa_dmat *dst )
{
    size_t m = src->rows, n = src->cols;
    if( row_start >= row_end ||
        row_start >= m ||
        row_end > m ||
        col_start >= col_end ||
        col_start >= n ||
        col_end > n )
    {
        aa_la_err("Block out-of-bounds\n");
    }

    aa_dmat_view( dst,
                  row_end - row_start, col_end - col_start,
                  src->data + col_start*src->ld + row_start, src->ld );
}

AA_API struct aa_dvec *
aa_dvec_malloc( size_t len ) {
    struct aa_dvec *r;
    const size_t s_desc = sizeof(*r);
    const size_t s_elem = sizeof(r->data[0]);
    const size_t pad =  s_elem - (s_desc % s_elem);
    const size_t size = s_desc + pad + len * s_elem;

    const size_t off = s_desc + pad;

    char *ptr = (char*)malloc( size );

    r = (struct aa_dvec*)ptr;
    aa_dvec_view(r, len, (double*)(ptr+off), 1);
    return r;
}

AA_API struct aa_dvec *
aa_dvec_alloc( struct aa_mem_region *reg, size_t len ) {
    struct aa_dvec *r;
    const size_t s_desc = sizeof(*r);
    const size_t s_elem = sizeof(r->data[0]);
    const size_t pad =  s_elem - (s_desc % s_elem);
    char *ptr = (char*)aa_mem_region_alloc(reg, s_desc + pad + len * s_elem );

    r = (struct aa_dvec*)ptr;
    aa_dvec_view(r, len, (double*)(ptr+s_desc+pad), 1);
    return r;
}

AA_API struct aa_dvec *
aa_dvec_dup( struct aa_mem_region *reg, const struct aa_dvec *src)
{
    struct aa_dvec *dst = aa_dvec_alloc(reg,src->len);
    aa_dvec_copy(src,dst);
    return dst;
}

AA_API struct aa_dmat *
aa_dmat_dup( struct aa_mem_region *reg, const struct aa_dmat *src)
{
    struct aa_dmat *dst = aa_dmat_alloc(reg,src->rows,src->cols);
    aa_dmat_copy(src,dst);
    return dst;
}

AA_API struct aa_dmat *
aa_dmat_malloc( size_t rows, size_t cols )
{
    struct aa_dmat *r;
    const size_t s_desc = sizeof(*r);
    const size_t s_elem = sizeof(r->data[0]);
    const size_t pad =  s_elem - (s_desc % s_elem);
    char *ptr = (char*)malloc( s_desc + pad + rows*cols * s_elem );

    r = (struct aa_dmat*)ptr;
    aa_dmat_view(r, rows, cols, (double*)(ptr+s_desc+pad), rows);

    return r;
}

AA_API struct aa_dmat *
aa_dmat_alloc( struct aa_mem_region *reg, size_t rows, size_t cols )
{
    struct aa_dmat *r;
    const size_t s_desc = sizeof(*r);
    const size_t s_elem = sizeof(r->data[0]);
    const size_t pad =  s_elem - (s_desc % s_elem);
    size_t size = s_desc + pad + rows*cols * s_elem ;

    char *ptr = (char*)aa_mem_region_alloc(reg, size);

    r = (struct aa_dmat*)ptr;
    aa_dmat_view( r, rows, cols, (double*)(ptr+s_desc+pad), rows );

    return r;
}

AA_API void
aa_dmat_set( struct aa_dmat *A, double alpha, double beta )
{
    int mi   = (int)(A->rows);
    int ni   = (int)(A->cols);
    int ldai = (int)(A->ld);

    dlaset_( "G", &mi, &ni,
             &alpha, &beta,
             A->data, &ldai );

}

AA_API void
aa_dvec_set( struct aa_dvec *vec, double alpha )
{
    double *end = vec->data + vec->len*vec->inc;
    for( double *x = vec->data; x < end; x += vec->inc ) {
        *x = alpha;
    }
}

void
aa_dvec_zero( struct aa_dvec *vec )
{
    aa_dvec_set(vec,0);
}

AA_API void
aa_dmat_zero( struct aa_dmat *mat )
{
    aa_dmat_set(mat, 0, 0);
}

/* Level 1 BLAS */
AA_API void
aa_dvec_swap( struct aa_dvec *x, struct aa_dvec *y )
{
    aa_la_check_size(x->len, y->len);
    cblas_dswap( VEC_LEN(x), AA_VEC_ARGS(x), AA_VEC_ARGS(y) );
}

AA_API void
aa_dvec_scal( double a, struct aa_dvec *x )
{
    cblas_dscal( VEC_LEN(x), a, AA_VEC_ARGS(x) );
}

static void s_inc( size_t n, double alpha, double *x, size_t inc ) {
    for( double *end = x + n*inc; x < end; x += inc ) {
        *x += alpha;
    }
}

AA_API void
aa_dvec_inc( double alpha, struct aa_dvec *x )
{
    s_inc( x->len, alpha, x->data, x->inc );
}

AA_API void
aa_dvec_copy( const struct aa_dvec *x, struct aa_dvec *y )
{
    aa_la_check_size(x->len, y->len);
    cblas_dcopy( VEC_LEN(x), AA_VEC_ARGS(x), AA_VEC_ARGS(y) );
}

AA_API void
aa_dvec_axpy( double a, const struct aa_dvec *x, struct aa_dvec *y )
{
    aa_la_check_size(x->len, y->len);
    cblas_daxpy( VEC_LEN(x), a, AA_VEC_ARGS(x), AA_VEC_ARGS(y) );
}

AA_API double
aa_dvec_dot( const struct aa_dvec *x, struct aa_dvec *y )
{
    aa_la_check_size(x->len, y->len);
    return cblas_ddot( VEC_LEN(x), AA_VEC_ARGS(x), AA_VEC_ARGS(y) );
}

AA_API double
aa_dvec_nrm2( const struct aa_dvec *x )
{
    return cblas_dnrm2( VEC_LEN(x), AA_VEC_ARGS(x) );
}

/* Level 2 BLAS */
AA_API void
aa_dmat_gemv( CBLAS_TRANSPOSE trans,
              double alpha, const struct aa_dmat *A,
              const struct aa_dvec *x,
              double beta, struct aa_dvec *y )
{
    if( CblasTrans == trans ) {
        aa_la_check_size( A->rows, x->len );
        aa_la_check_size( A->cols, y->len );
    } else {
        aa_la_check_size( A->rows, y->len );
        aa_la_check_size( A->cols, x->len );
    }

    cblas_dgemv( CblasColMajor, trans,
                 MAT_ROWS(A), MAT_COLS(A),
                 alpha, AA_MAT_ARGS(A),
                 AA_VEC_ARGS(x),
                 beta, AA_VEC_ARGS(y) );

}

/* Level 3 BLAS */
AA_API void
aa_dmat_gemm( CBLAS_TRANSPOSE transA, CBLAS_TRANSPOSE transB,
              double alpha, const struct aa_dmat *A,
              const struct aa_dmat *B,
              double beta, struct aa_dmat *C )
{
    aa_la_check_size( A->rows, C->rows );
    aa_la_check_size( A->cols, B->rows );
    aa_la_check_size( B->cols, C->cols );

    assert( A->rows <= A->ld );
    assert( B->rows <= B->ld );
    assert( C->rows <= C->ld );

    cblas_dgemm( CblasColMajor,
                 transA, transB,
                 MAT_ROWS(A), MAT_COLS(B), MAT_COLS(A),
                 alpha, AA_MAT_ARGS(A),
                 AA_MAT_ARGS(B),
                 beta, AA_MAT_ARGS(C) );
}

/* LAPACK */

AA_API void
aa_dmat_lacpy( const char uplo[1],
               const struct aa_dmat *A,
               struct aa_dmat *B )
{

    aa_la_check_size(A->rows,B->rows);
    aa_la_check_size(A->cols,B->cols);
    int mi   = (int)(A->rows);
    int ni   = (int)(A->cols);
    int ldai = (int)(A->ld);
    int ldbi = (int)(B->ld);

    dlacpy_(uplo, &mi, &ni,
            A->data, &ldai,
            B->data, &ldbi);
}

AA_API void
aa_dmat_copy(  const struct aa_dmat *A, struct aa_dmat *B)
{
    aa_dmat_lacpy("G",A,B);
}

/* Matrix Functions */

static double s_ssd( size_t n,
                     double a,
                     double *x, size_t incx,
                     double *y, size_t incy )
{
    for( double *e = x + n*incx; x < e; x += incx, y+=incy ) {
        double d = *x - *y;
        a += d*d;
    }
    return a;
}

AA_API double
aa_dvec_ssd( const struct aa_dvec *x, const struct aa_dvec *y)
{
    aa_la_check_size( x->len, y->len );
    return s_ssd( x->len, 0,
                  x->data, x->inc,
                  y->data, y->inc );
}

AA_API double
aa_dmat_ssd( const struct aa_dmat *A, const struct aa_dmat *B)
{
    size_t m = A->rows, n = A->cols;
    aa_la_check_size( m, B->rows );
    aa_la_check_size( n, B->cols );
    double a = 0;
    for( double *Ac=A->data, *Bc=B->data, *ec=A->data + n*A->ld;
         Ac < ec;
         Ac+=A->ld, Bc+=B->ld )
    {
        a = s_ssd( m, a, Ac, 1, Bc, 1 );
    }
    return a;
}

AA_API double
aa_dmat_nrm2( const struct aa_dmat *A )
{
    int m=(int)A->rows, n=(int)A->cols, ld=(int)A->ld;
    return dlange_("F", &m, &n, A->data, &ld, NULL );
}

AA_API void
aa_dmat_scal( struct aa_dmat *x, double alpha )
{
    int m = (int)x->rows, n=(int)x->cols, ld=(int)x->ld;
    double cfrom=1;
    int info;
    dlascl_("G", NULL, NULL,
            &cfrom, &alpha,
            &m, &n, x->data, &ld,
            &info);
}

AA_API void
aa_dmat_inc( struct aa_dmat *A, double alpha )
{
    size_t m=A->rows, n=A->cols, ld=A->ld;
    double *x = A->data;

    if( m == n ) {
        s_inc(m*n, alpha, x, 1);
    } else {
        for( double *e = x + n*ld; x < e; x+=ld ) {
            s_inc(m, alpha, x, 1);
        }
    }

}

AA_API void
aa_dmat_axpy( double alpha, const struct aa_dmat *X, struct aa_dmat *Y)
{
    size_t m=X->rows, n=X->cols, ldX=X->ld, ldY=Y->ld;
    double *x=X->data, *y=Y->data;

    aa_la_check_size(m, Y->rows);
    aa_la_check_size(n, Y->cols);

    if( m == ldX && m == ldY ) {
        cblas_daxpy( (int)(m*n), alpha, x,1, y,1 );
    } else {
        int mi = (int)m;
        for( double *e = x + n*ldX; x < e; x+=ldX, y+=ldY ) {
            cblas_daxpy( mi, alpha, x,1, y,1 );
        }
    }
}

void
aa_dmat_trans( const struct aa_dmat *A, struct aa_dmat *B)
{
    const size_t m   = A->rows;
    const size_t n   = A->cols;
    const size_t lda = A->ld;
    const size_t ldb = B->ld;

    aa_la_check_size(m, B->cols);
    aa_la_check_size(n, B->rows);

    for ( double *Acol = A->data, *Brow=B->data, *Be=B->data + n;
          Brow < Be;
          Brow++, Acol+=lda )
    {
        cblas_dcopy( (int)m, Acol, 1, Brow, (int)ldb );
    }
}

AA_API int
aa_dmat_inv( struct aa_dmat *A )
{
    aa_la_check_size(A->rows,A->cols);
    int info;

    int *ipiv = (int*)
        aa_mem_region_local_alloc(sizeof(int)*A->rows);

    // LU-factor
    info = aa_cla_dgetrf( MAT_ROWS(A), MAT_COLS(A), AA_MAT_ARGS(A), ipiv );

    int lwork = -1;
    while(1) {
        double *work = (double*)
            aa_mem_region_local_tmpalloc( sizeof(double)*
                                      (size_t)(lwork < 0 ? 1 : lwork) );
        aa_cla_dgetri( MAT_ROWS(A), AA_MAT_ARGS(A), ipiv, work, lwork );
        if( lwork > 0 ) break;
        assert( -1 == lwork );
        lwork = (int)work[0];
    }

    aa_mem_region_local_pop(ipiv);

    return info;
}

int s_svd_helper ( const struct aa_dmat *A,
                   struct aa_mem_region *reg,
                   size_t *pkmin, size_t *pkmax,
                   double **pU, double **pVt, double **pS )
{

    size_t m = A->rows;
    size_t n = A->cols;

    // find  min/max dimensions
    if( m < n ) {
        *pkmin = m;
        *pkmax = n;
    } else {
        *pkmin = n;
        *pkmax = m;
    }

    // This method uses the SVD
    double *W  = (double*)aa_mem_region_alloc( reg, sizeof(double) * (m*m + n*n + *pkmin) );
    *pU  = W;        // size m*m
    *pVt = *pU + m*m; // size n*n
    *pS  = *pVt + n*n; // size min(m,n)

    // A = U S V^T
    aa_la_d_svd( m,n,
                 A->data, A->ld,
                 *pU, m, *pS, *pVt, n );

    return 0;
}

int
aa_dmat_pinv( const struct aa_dmat *A, double tol, struct aa_dmat *As )
{

    aa_la_check_size(A->rows, As->cols);
    aa_la_check_size(A->cols, As->rows);

    struct aa_mem_region *reg =  aa_mem_region_local_get();
    void *ptrtop = aa_mem_region_ptr(reg);

    size_t kmin, kmax;
    double *U, *Vt, *S;
    s_svd_helper( A, reg,
                  &kmin, &kmax, &U, &Vt, &S );

    size_t m = A->rows;
    const int mi = (int)(A->rows);
    const int ni = (int)(A->cols);

    if( tol < 0 ) {
        tol = (double)kmax * S[0] * DBL_EPSILON;
    }

    // \sum 1/s_i * v_i * u_i^T
    aa_dmat_zero(As);
    double *Asd = As->data;
    for( size_t i = 0; i < kmin && S[i] > tol; i ++ ) {
        cblas_dger( CblasColMajor, ni, mi, 1/S[i],
                    Vt + i, ni,
                    U + m*i, 1,
                    Asd, ni
            );
    }

    aa_mem_region_pop( reg, ptrtop );

    return 0;

    /* struct aa_mem_region *reg = aa_mem_region_local_get(); */
    /* struct aa_dmat *Ap = aa_dmat_alloc(reg,m,n); */

    /* // B = AA^T */
    /* double *B; */
    /* int ldb; */
    /* if( m <= n ) { */
    /*     /\* Use A_star as workspace when it's big enough *\/ */
    /*     B = As->data; */
    /*     ldb = (int)(As->ld); */
    /* } else { */
    /*     B = AA_MEM_REGION_NEW_N( reg, double, m*m ); */
    /*     ldb = (int)m; */
    /* } */

    /* aa_la_dlacpy( "A", A, Ap ); */

    /* // B is symmetric.  Only compute the upper half. */
    /* cblas_dsyrk( CblasColMajor, CblasUpper, CblasNoTrans, */
    /*              MAT_ROWS(Ap), MAT_COLS(Ap), */
    /*              1, AA_MAT_ARGS(Ap), */
    /*              0, B, ldb ); */

    /* // B += kI */
    /* /\* for( size_t i = 0; i < m*(size_t)ldb; i += ((size_t)ldb+1) ) *\/ */
    /* /\*     B[i] += k; *\/ */

    /* /\* Solve via Cholesky Decomp *\/ */
    /* /\* B^T (A^*)^T = A    and     B = B^T (Hermitian) *\/ */

    /* aa_cla_dposv( 'U', (int)m, (int)n, */
    /*               B, ldb, */
    /*               AA_MAT_ARGS(Ap) ); */

    /* aa_dmat_trans( Ap, As ); */

    /* aa_mem_region_pop( reg, Ap ); */

}

int
aa_dmat_dpinv( const struct aa_dmat *A, double k, struct aa_dmat *As)
{
    aa_la_check_size(A->rows, As->cols);
    aa_la_check_size(A->cols, As->rows);

    // TODO: Try DGESV for non-positive-definite matrices

    size_t m = A->rows;
    size_t n = A->cols;

    struct aa_mem_region *reg = aa_mem_region_local_get();
    void *ptrtop = aa_mem_region_ptr(reg);
    int r = -1;

    /* As = inv(A'*A - k*I) * A' A'*inv(A*A' - k*I) */
    if( m <= n ) {
        /*
         * (A^*) = A^T*inv(A*A^T - k*I)
         * (A^*) = A^T*inv(B)
         * (A^*)*B = A^T
         * B^T (A^*)^T = A    and     B = B^T (Hermitian)
         *
         */

        /* Use A_star as workspace for B */
        double *B = As->data;
        int ldb = (int)(As->ld);

        /* Ap = A */
        struct aa_dmat *Ap = aa_dmat_alloc(reg,m,n);
        aa_dmat_lacpy( "A", A, Ap );

        // B is symmetric.  Only compute the upper half.
        // B = A * A'
        cblas_dsyrk( CblasColMajor, CblasUpper, CblasNoTrans,
                     MAT_ROWS(Ap), MAT_COLS(Ap),
                     1, AA_MAT_ARGS(Ap),
                     0, B, ldb );

        /* B += kI */
        for( double *x = B, *e = B+(int)m*ldb; x < e; x+=ldb+1 )
            *x += k;

        /* B^T (A^*)^T = A    and     B = B^T (Hermitian) */

        /* Solve via Cholesky (positive definite) */
        r = aa_cla_dposv( 'U', (int)m, (int)n,
                           B, ldb,
                           AA_MAT_ARGS(Ap) );

        /* Solve via LU */
        /* int *ipiv = AA_MEM_REGION_NEW_N(reg,int,m); */
        /* r = aa_la_d_sysv( "U", m, n, */
        /*                   B, (size_t)ldb, */
        /*                   ipiv, */
        /*                   Ap->data, Ap->ld ); */

        aa_dmat_trans( Ap, As );

    } else {

        /*
         * (A^*) = inv(A^T*A - k*I) * A^T
         * (A^*) = inv(B) * A^T
         * B*(A^*) = A^T
         */

        /* Use A_star as workspace for B */
        double *B = AA_MEM_REGION_NEW_N(reg,double,n*n);
        int ldb = (int)(n);

        /* As = A^T */
        aa_dmat_trans( A, As );

        // B is symmetric.  Only compute the upper half.
        // B = A' * A = As * As'
        cblas_dsyrk( CblasColMajor, CblasUpper, CblasNoTrans,
                     MAT_ROWS(As), MAT_COLS(As),
                     1, AA_MAT_ARGS(As),
                     0, B, ldb );

        /* B += kI */
        //for( size_t i = 0; i < n*(size_t)ldb; i += ((size_t)ldb+1) )
        for( double *x = B, *e = B+n*n; x < e; x+=ldb+1 )
            *x += k;

        /* B * (A^*)^T = A    and     B = B^T (Hermitian) */

        /* Solve via Cholesky (positive definite) */
        r = aa_cla_dposv( 'U', (int)n, (int)m,
                           B, ldb,
                           AA_MAT_ARGS(As) );

        /* Solve via LU */
        /* int *ipiv = AA_MEM_REGION_NEW_N(reg,int,n); */
        /* r = aa_la_d_sysv( "U", n, m, */
        /*                   B, (size_t)ldb, */
        /*                   ipiv, */
        /*                   As->data, As->ld ); */

    }

    aa_mem_region_pop( reg, ptrtop );

    return r;
}

int
aa_dmat_dzdpinv(  const struct aa_dmat *A, double s_min, struct aa_dmat *As)
{

    aa_la_check_size(A->rows, As->cols);
    aa_la_check_size(A->cols, As->rows);

    struct aa_mem_region *reg =  aa_mem_region_local_get();
    void *ptrtop = aa_mem_region_ptr(reg);

    size_t kmin, kmax;
    double *U, *Vt, *S;
    s_svd_helper( A, reg,
                  &kmin, &kmax, &U, &Vt, &S );

    size_t m = A->rows;
    const int mi = (int)(A->rows);
    const int ni = (int)(A->cols);

    // \sum s_i/(s_i**2+k) * v_i * u_i^T
    aa_dmat_zero(As);
    double *Asd = As->data;
    size_t i = 0;

    // Undamped parts
    for( ; i < kmin && S[i] >= s_min; i ++ ) {
        cblas_dger( CblasColMajor, ni, mi, 1/S[i],
                    Vt + i, ni,
                    U + m*i, 1,
                    Asd, ni
            );
    }

    // Damped parts
    double s2 = s_min*s_min;
    for( ; i < kmin; i ++ ) {
        cblas_dger( CblasColMajor, ni, mi, S[i]/s2,
                    Vt + i, ni,
                    U + m*i, 1,
                    Asd, ni
            );
    }

    aa_mem_region_pop( reg, ptrtop );

    return 0;

}


/*
* -----------------------------------------------------------------
*  Binary Search Tree Library --- bst_lib.c
*  Version: 1.6180
*  Date: Mar 12, 2010
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  This program is free software: you can redistribute it and/or
*  modify it under the terms of the GNU General Public License as
*  published by the Free Software Foundation, either version 3 of
*  the License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the implementation file of a library
*  to work with binary search trees.
* -----------------------------------------------------------------
*/

#include <stdlib.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

#include "../include/bst_lib.h"

/*
*------------------------------------------------------------
*   bst_leaf_alloc
*
*   This function allocates a struct leaf.
*
*   Input:
*   void
*
*   Output:
*   leaf - pointer to a struct leaf
*
*   last update: May 10, 2009
*------------------------------------------------------------
*/

bst_leaf *bst_leaf_alloc()
{
    bst_leaf *leaf = NULL;

    /* memory allocation for bst_leaf */
    leaf = (bst_leaf *) malloc(sizeof(bst_leaf));
    if ( leaf == NULL )
        return NULL;

    /* setting bst_leaf elements equal to NULL */
    leaf->phi  = NULL;
    leaf->Rphi = NULL;
    leaf->A    = NULL;
    leaf->L    = NULL;

    return leaf;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_node_alloc
*
*   This function allocates a struct node.
*
*   Input:
*   void
*
*   Output:
*   node - pointer to a struct node
*
*   last update: May 10, 2009
*------------------------------------------------------------
*/

bst_node* bst_node_alloc()
{
    bst_node *node = NULL;

    /* memory allocation for bst_node */
    node = (bst_node *) malloc(sizeof(bst_node));
    if ( node == NULL )
        return NULL;

    /* setting bst_node elements equal to NULL */
    node->v      = NULL;
    node->r_node = NULL;
    node->l_node = NULL;
    node->r_leaf = NULL;
    node->l_leaf = NULL;

    return node;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_leaf_free
*
*   This function frees the memory used by a struct leaf.
*
*   Input:
*   leaf - pointer to a struct leaf
*
*   Output:
*   void
*
*   last update: May 9, 2009
*------------------------------------------------------------
*/

void bst_leaf_free(void **leaf_bl)
{
    bst_leaf *leaf = NULL;

    /* checking if leaf_bl is NULL */
    if ( *leaf_bl == NULL )
        return;

    leaf = (bst_leaf *) (*leaf_bl);

    /* releasing bst_leaf elements */
    if( leaf->phi != NULL )
    {
        gsl_vector_free(leaf->phi);
        leaf->phi = NULL;
    }

    if( leaf->Rphi != NULL )
    {
        gsl_vector_free(leaf->Rphi);
        leaf->Rphi = NULL;
    }

    if( leaf->A != NULL )
    {
        gsl_matrix_free(leaf->A);
        leaf->A = NULL;
    }

    if( leaf->L != NULL )
    {
        gsl_matrix_free(leaf->L);
        leaf->L = NULL;
    }

    /* releasing allocated memory by bst_leaf */
    free(*leaf_bl);
    *leaf_bl = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_node_free
*
*   This function frees the memory used by a struct node.
*
*   Input:
*   node - pointer to a struct node
*
*   Output:
*   void
*
*   last update: Feb 19, 2010
*------------------------------------------------------------
*/

void bst_node_free(void **node_bl)
{
    bst_node *node = NULL;

    /* checking if node_bl is NULL */
    if ( *node_bl == NULL )
        return;

    node = (bst_node *) (*node_bl);

    /* releasing allocated memory by bst_node elements */
    if( node->l_leaf != NULL )
    {
        bst_leaf_free((void **)&(node->l_leaf));
        node->l_leaf = NULL;
    }

    if( node->r_leaf != NULL )
    {
        bst_leaf_free((void **)&(node->r_leaf));
        node->r_leaf = NULL;
    }

    if( node->l_node != NULL )
    {
        bst_node_free((void **)&(node->l_node));
        node->l_node = NULL;
    }

    if( node->r_node != NULL )
    {
        bst_node_free((void **)&(node->r_node));
        node->r_node = NULL;
    }

    if( node->v != NULL )
    {
        gsl_vector_free(node->v);
        node->v = NULL;
    }

    /* releasing allocated memory by bst_node */
    free(*node_bl);
    *node_bl = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_leaf_set
*
*   This function sets the bst_leaf elements equal to
*   the input elements and returns GSL_SUCCESS if
*   there is no problem during its execution.
*
*   Input:
*   phi  - composition
*   Rphi - reaction mapping
*   A    - mapping gradient matrix
*   L    - ellipsoid Cholesky matrix
*
*   Output:
*   leaf - pointer to a struct leaf
*   success or error
*
*   last update: Feb 22, 2009
*------------------------------------------------------------
*/

int bst_leaf_set(gsl_vector *phi,gsl_vector *Rphi,
                    gsl_matrix *A,gsl_matrix *L,bst_leaf *leaf)
{
    /* checking if leaf is NULL */
    if ( leaf == NULL )
        return GSL_EFAILED;

    /* setting phi equal to the input vector phi */
    if ( leaf->phi == NULL )
        leaf->phi = gsl_vector_calloc(phi->size);

    gsl_vector_memcpy(leaf->phi,phi);

    /* setting Rphi equal to the input vector Rphi */
    if ( leaf->Rphi == NULL )
        leaf->Rphi = gsl_vector_calloc(Rphi->size);

    gsl_vector_memcpy(leaf->Rphi,Rphi);

    /* setting A equal to the input matrix A */
    if ( leaf->A == NULL )
        leaf->A = gsl_matrix_calloc(A->size1,A->size2);

    gsl_matrix_memcpy(leaf->A,A);

    /* setting L equal to the input matrix L */
    if ( leaf->L == NULL )
        leaf->L = gsl_matrix_calloc(L->size1,L->size2);

    gsl_matrix_memcpy(leaf->L,L);

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_cutplane
*
*   This function compute the vector ( v = phi_r - phi_l )
*   and the scalar ( a = (|phi_r|^2-|phi_l|^2)/2 )
*   which define a cutting plane used to search any
*   information in the binary search tree.
*
*   Input:
*   phi_l - left  composition
*   phi_r - right compositon
*
*   Output:
*   v    - cutting plane normal vector
*   a    - cutting plane scalar
*
*   last update: Feb 27, 2009
*------------------------------------------------------------
*/

double bst_cutplane(gsl_vector *phi_l,gsl_vector *phi_r,gsl_vector *v)
{
    double a1, a2;

    /* v := phi_r */
    gsl_vector_memcpy(v,phi_r);

    /* v := phi_r - phi_l */
    gsl_vector_sub(v,phi_l);

    /* a1 := |phi_r|^2 */
    gsl_blas_ddot(phi_r,phi_r,&a1);

    /* a2 := |phi_l|^2 */
    gsl_blas_ddot(phi_l,phi_l,&a2);

    return 0.5*(a1-a2);
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_node_set
*
*   This function set the bst_node elements equal to
*   the input elements and define the vector and
*   scalar to do the search for a information.
*
*   Input:
*   l_leaf - left  leaf
*   r_leaf - right leaf
*
*   Output:
*   node - pointer to a struct node
*   success or error
*
*   last update: Feb 27, 2009
*------------------------------------------------------------
*/

int bst_node_set(bst_leaf *l_leaf,bst_leaf *r_leaf,bst_node *node)
{
    /* checking if node is NULL */
    if ( node == NULL )
        return GSL_EFAILED;

    /* memory allocation for v */
    if ( node->v == NULL )
    	node->v = gsl_vector_calloc(r_leaf->phi->size);

    /* setting v and a */
    node->a = bst_cutplane(l_leaf->phi,r_leaf->phi,node->v);

    /* setting leaf elements equal to the input leaves */
    node->l_leaf = l_leaf;
    node->r_leaf = r_leaf;

    /* setting node elements equal to NULL */
    node->l_node = NULL;
    node->r_node = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_node_add
*
*   This function adds a new node to a given side
*   of a old node and returns GSL_SUCCESS if
*   there is no problem during its execution.
*
*   Input:
*   side     - new node side
*   old_node - pointer to the old node
*   new_node - pointer to the new node
*
*   Output:
*   success or error
*
*   last update: Feb 22, 2009
*------------------------------------------------------------
*/

int bst_node_add(int side,bst_node *old_node,bst_node *new_node)
{
    if( side == RIGHT )
    {
        if( old_node->r_node != NULL )
            return GSL_EFAILED;
        else
        {
            /* adding a new right node */
            old_node->r_node = new_node;
            old_node->r_leaf = NULL;

            return GSL_SUCCESS;
        }
    }
    else if( side == LEFT )
    {
        if( old_node->l_node != NULL )
            return GSL_EFAILED;
        else
        {
            /* adding a new left node */
            old_node->l_node = new_node;
            old_node->l_leaf = NULL;

            return GSL_SUCCESS;
        }
    }
    else
        return GSL_EFAILED;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_search
*
*   This function searchs for a near composition phi0 in the
*   binary search tree.
*
*   Input:
*   root     - binary tree root
*   phi      - composition
*
*   Output:
*   end_leaf - leaf with the near composition
*   end_node - node with the near composition
*   side     - near compositon leaf side
*
*   last update: Feb 3, 2009
*------------------------------------------------------------
*/

int bst_search(bst_node *root,gsl_vector *phi,
                        bst_node **end_node,bst_leaf **end_leaf)
{
    double dot;

    if( root == NULL )
    {
        *end_leaf = NULL;
        *end_node = NULL;

        return GSL_EFAILED;
    }

    /* dot := v^T*phi */
    gsl_blas_ddot(root->v,phi,&dot);

    /* if dot > a then right node else left node */
    if( dot > root->a )
    {
        /* extern node */
        if( root->r_node == NULL )
        {
            *end_leaf = root->r_leaf;
            *end_node = root;

            return RIGHT;
        }
        else
            return bst_search(root->r_node,phi,end_node,end_leaf);
    }
    else
    {
        /* extern node */
        if( root->l_node == NULL )
        {
            *end_leaf = root->l_leaf;
            *end_node = root;

            return LEFT;
        }
        else
            return bst_search(root->l_node,phi,end_node,end_leaf);
    }
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   bst_height
*
*   This function computes the binary search tree height.
*
*   Input:
*   root - binary tree root
*
*   Output:
*   height - binary search tree height
*
*   last update: Nov 2, 2009
*------------------------------------------------------------
*/

int bst_height(bst_node *root)
{
    int l_tree_height = 0;
    int r_tree_height = 0;

    if ( root != NULL )
    {
        l_tree_height = bst_height(root->l_node);
        r_tree_height = bst_height(root->r_node);

        if ( l_tree_height > r_tree_height )
            return l_tree_height + 1;
        else
            return r_tree_height + 1;
    }
    else
        return 0;

}
/*------------------------------------------------------------*/


#include <stdlib.h>
#include <stddef.h>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include <cblas.h>
#include "LCP_Solver.h"
#include "debug.h"

void numericsError(char * functionName, char* message)
{
  char output[200] = "Numerics error - ";
  strcat(output, functionName);
  strcat(output, message);
  strcat(output, ".\n");
  fprintf(stderr, "%s", output);
  exit(EXIT_FAILURE);
}

void prodNumericsMatrix(int sizeX, int sizeY, double alpha, const NumericsMatrix* const A, const double* const x, double beta, double* y)
{
  assert(A);
  assert(x);
  assert(y);
  assert(A->size0 == sizeY);
  assert(A->size1 == sizeX);
  cblas_dgemv(CblasColMajor, CblasNoTrans, sizeY, sizeX, alpha, A->matrix0, sizeY, x, 1, beta, y, 1);
}

void deleteSolverOptions(SolverOptions* op)
{
  if(op)
  {
    if (op->iparam != NULL)
      free(op->iparam);
    op->iparam = NULL;
    if (op->dparam != NULL)
      free(op->dparam);
    op->dparam = NULL;
  }
}

void lcp_lexicolemke(LinearComplementarityProblem* problem, double *zlem , double *wlem , int *info , SolverOptions* options)
{
  /* matrix M of the lcp */
  double * M = problem->M->matrix0;
  assert(M);
  /* size of the LCP */
  int dim = problem->size;
  assert(dim>0);
  int dim2 = 2 * (dim + 1);

  int i, drive, block, Ifound;
  int ic, jc;
  int ITER;
  int nobasis;
  int itermax = options->iparam[0];

  i=0;
  int n = problem->size;
  double *q = problem->q;

  while ((i < (n - 1)) && (q[i] >= 0.))
    i++;

  if ((i == (n - 1)) && (q[n - 1] >= 0.))
  {
    /* TRIVIAL CASE : q >= 0
     * z = 0 and w = q is solution of LCP(q,M)
     */
    for (int j = 0 ; j < n; j++)
    {
      zlem[j] = 0.0;
      wlem[j] = q[j];
    }
    *info = 0;
    options->iparam[1] = 0;   /* Number of iterations done */
    options->dparam[1] = 0.0; /* Error */
    if (options->verboseMode > 0)
      printf("lcp_lexicolemke: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
    return ;
  }

  double z0, zb, dblock;
  double pivot, tovip;
  double tmp;
  int *basis;
  double** A;

  /*output*/
  options->iparam[1] = 0;

  /* Allocation */
  basis = (int *)malloc(dim * sizeof(int));
  A = (double **)malloc(dim * sizeof(double*));

  for (ic = 0 ; ic < dim; ++ic)
    A[ic] = (double *)malloc(dim2 * sizeof(double));

  /* construction of A matrix such that
   * A = [ q | Id | -d | -M ] with d = (1,...1)
   */
  /* We need to init only the part corresponding to Id */
  for (ic = 0 ; ic < dim; ++ic)
    for (jc = 1 ; jc <= dim; ++jc)
      A[ic][jc] = 0.0;

  for (ic = 0 ; ic < dim; ++ic)
    for (jc = 0 ; jc < dim; ++jc)
      A[ic][jc + dim + 2] = -M[dim * jc + ic];

  assert(problem->q);

  for (ic = 0 ; ic < dim; ++ic) A[ic][0] = problem->q[ic];

  for (ic = 0 ; ic < dim; ++ic) A[ic][ic + 1 ] =  1.0;
  for (ic = 0 ; ic < dim; ++ic) A[ic][dim + 1] = -1.0;

  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});
  /* End of construction of A */

  Ifound = 0;

  for (ic = 0 ; ic < dim  ; ++ic) basis[ic] = ic + 1;

  drive = dim + 1;
  block = 0;
  z0 = A[block][0];
  ITER = 0;

  /* Start research of argmin lexico */
  /* With this first step the covering vector enter in the basis */
  for (ic = 1 ; ic < dim ; ++ic)
  {
    zb = A[ic][0];
    if (zb < z0)
    {
      z0    = zb;
      block = ic;
    }
    else if (zb == z0)
    {
      for (jc = 0 ; jc < dim ; ++jc)
      {
        dblock = A[block][1 + jc] - A[ic][1 + jc];
        if (dblock < 0)
        {
          break;
        }
        else if (dblock > 0)
        {
          block = ic;
          break;
        }
      }
    }
  }

  /* Stop research of argmin lexico */
  DEBUG_PRINTF("Pivoting %i and %i\n", block, drive);

  pivot = A[block][drive];
  tovip = 1.0 / pivot;

  /* Pivot < block , drive > */
  A[block][drive] = 1;
  for (ic = 0       ; ic < drive ; ++ic) A[block][ic] = A[block][ic] * tovip;
  for (ic = drive + 1 ; ic < dim2  ; ++ic) A[block][ic] = A[block][ic] * tovip;

  /* */

  for (ic = 0 ; ic < block ; ++ic)
  {
    tmp = A[ic][drive];
    for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
  }
  for (ic = block + 1 ; ic < dim ; ++ic)
  {
    tmp = A[ic][drive];
    for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
  }

   nobasis = basis[block];
  basis[block] = drive;

  DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));
  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  while (ITER < itermax && !Ifound)
  {

    ++ITER;

    if (nobasis < dim + 1)      drive = nobasis + (dim + 1);
    else if (nobasis > dim + 1) drive = nobasis - (dim + 1);

    DEBUG_PRINTF("driving variable %i \n", drive);

    /* Start research of argmin lexico for minimum ratio test */
    pivot = 1e20;
    block = -1;

    for (ic = 0 ; ic < dim ; ++ic)
    {
      zb = A[ic][drive];
      if (zb > 0.0)
      {
        z0 = A[ic][0] / zb;
        if (z0 > pivot) continue;
        if (z0 < pivot)
        {
          pivot = z0;
          block = ic;
        }
        else
        {
          for (jc = 1 ; jc < dim + 1 ; ++jc)
          {
            assert(block >=0 && "lcp_lexicolemke: block <0");
            dblock = A[block][jc] / pivot - A[ic][jc] / zb;
            if (dblock < 0.0) break;
            else if (dblock > 0.0)
            {
              block = ic;
              break;
            }
          }
        }
      }
    }
    if (block == -1)
    {
      Ifound = 1;
      DEBUG_PRINT("The pivot column is nonpositive !\n"
          "It either means that the algorithm failed or that the LCP is infeasible\n"
          "Check the class of the M matrix to find out the meaning of this\n");
      break;
    }

    if (basis[block] == dim + 1) Ifound = 1;

    /* Pivot < block , drive > */
    pivot = A[block][drive];
    tovip = 1.0 / pivot;
    A[block][drive] = 1;

    for (ic = 0       ; ic < drive ; ++ic) A[block][ic] = A[block][ic] * tovip;
    for (ic = drive + 1 ; ic < dim2  ; ++ic) A[block][ic] = A[block][ic] * tovip;

    /* */

    for (ic = 0 ; ic < block ; ++ic)
    {
      tmp = A[ic][drive];
      for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
    }
    for (ic = block + 1 ; ic < dim ; ++ic)
    {
      tmp = A[ic][drive];
      for (jc = 0 ; jc < dim2 ; ++jc) A[ic][jc] -=  tmp * A[block][jc];
    }

    nobasis = basis[block];
    basis[block] = drive;

    DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));

    DEBUG_PRINT("total matrix\n");
    DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  } /* end while*/

  DEBUG_EXPR_WE( DEBUG_PRINT("new basis: ")
      for (unsigned int i = 0; i < dim; ++i)
      { DEBUG_PRINTF("%i ", basis[i])}
      DEBUG_PRINT("\n"));

  DEBUG_PRINT("total matrix\n");
  DEBUG_EXPR_WE(for (unsigned int i = 0; i < dim; ++i)
      { for(unsigned int j = 0 ; j < dim2; ++j)
      { DEBUG_PRINTF("%1.2e ", A[i][j]) }
      DEBUG_PRINT("\n")});

  for (ic = 0 ; ic < dim; ++ic)
  {
    drive = basis[ic];
    if (drive < dim + 1)
    {
      zlem[drive - 1] = 0.0;
      wlem[drive - 1] = A[ic][0];
    }
    else if (drive > dim + 1)
    {
      zlem[drive - dim - 2] = A[ic][0];
      wlem[drive - dim - 2] = 0.0;
    }
  }

  options->iparam[1] = ITER;

  if (Ifound) *info = 0;
  else *info = 1;

  free(basis);

  for (i = 0 ; i < dim ; ++i) free(A[i]);
  free(A);
}

int linearComplementarity_driver(LinearComplementarityProblem* problem, double *z , double *w, SolverOptions* options)
{
  /********************
   * 0 - Check inputs *
   ********************/

  if (options == NULL)
    numericsError("lcp_driver", "null input for solver options");
  if (problem == NULL || z == NULL || w == NULL)
    numericsError("lcp_driver", "null input for LinearComplementarityProblem and/or unknowns (z,w)");

  int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */

  if (NoDefaultOptions == 0)
  {
    numericsError("lcp_driver_DenseMatrix", "options for solver have not been set");
  }

  if (options->verboseMode > 0)
    printSolverOptions(options);

  /* Output info. : 0: ok -  >0: problem (depends on solver) */
  int info = -1;

  /******************************************
   *  1 - Check for trivial solution
   ******************************************/

  int i = 0;
  int n = problem->size;
  double *q = problem->q;
/*  if (!((options->solverId == SICONOS_LCP_ENUM) && (options->iparam[0] == 1 )))*/
    {
      while ((i < (n - 1)) && (q[i] >= 0.)) i++;
      if ((i == (n - 1)) && (q[n - 1] >= 0.))
      {
        /* TRIVIAL CASE : q >= 0
         * z = 0 and w = q is solution of LCP(q,M)
         */
        for (int j = 0 ; j < n; j++)
        {
          z[j] = 0.0;
          w[j] = q[j];
        }
        info = 0;
        options->dparam[1] = 0.0; /* Error */
        if (options->verboseMode > 0)
          printf("LCP_driver_DenseMatrix: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
        return info;
      }
    }

  /*************************************************
   *  2 - Call Lemke solver (if no trivial sol.)
   *************************************************/

  if (options->verboseMode == 1)
    printf(" ========================== Call Lemke solver for Linear Complementarity problem ==========================\n");

  /****** Lemke algorithm ******/
  /* IN: itermax
     OUT: iter */
   lcp_lexicolemke(problem, z , w , &info , options);

  /*************************************************
   *  3 - Computes w = Mz + q and checks validity
   *************************************************/
  if (options->filterOn > 0)
  {
    int info_ = lcp_compute_error(problem, z, w, options->dparam[0], &(options->dparam[1]),
				  options->verboseMode);
    if (info <= 0) /* info was not setor the solver was happy */
      info = info_;
  }

  return info;
}

void lcp_compute_error_only(unsigned int n, double *z , double *w, double * error)
{
  /* Checks complementarity */

  *error = 0.;
  double zi, wi;
  for (unsigned int i = 0 ; i < n ; i++)
  {
    zi = z[i];
    wi = w[i];
    if (zi < 0.0)
    {
      *error += -zi;
      if (wi < 0.0) *error += zi * wi;
    }
    if (wi < 0.0) *error += -wi;
    if ((zi > 0.0) && (wi > 0.0)) *error += zi * wi;
  }
}

int lcp_compute_error(LinearComplementarityProblem* problem, double *z , double *w, double tolerance,
		      double * error, int verbose)
{
  /* Checks inputs */
  if (problem == NULL || z == NULL || w == NULL)
    numericsError("lcp_compute_error", "null input for problem and/or z and/or w");

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  unsigned int n = problem->size;
  cblas_dcopy(n , problem->q , incx , w , incy);  // w <-q
  prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
  double normq = cblas_dnrm2(n , problem->q , incx);
  lcp_compute_error_only(n, z, w, error);
  *error = *error / (normq + 1.0); /* Need some comments on why this is needed */
  if (*error > tolerance)
  {
    if (verbose > 0)
	printf(" Numerics - lcp_compute_error : error = %g > tolerance = %g.\n", *error, tolerance);
    return 1;
  }
  else
    return 0;
}

int linearComplementarity_lexicolemke_setDefaultSolverOptions(SolverOptions* options)
{
  /* if (options->verboseMode > 0) */
  /* { */
  /*   printf("Set the Default SolverOptions for the Lemke Solver\n"); */
  /* } */

  options->isSet = 1;
  options->filterOn = 1;
  options->iSize = 5;
  options->dSize = 5;
  options->iparam = (int *)calloc(options->iSize, sizeof(int));
  options->dparam = (double *)calloc(options->dSize, sizeof(double));
  options->verboseMode = 0;
  options->dparam[0] = 1e-6;
  options->iparam[0] = 10000;
  return 0;
}

void printSolverOptions(SolverOptions* options)
{
  printf("\n ========== Numerics Non Smooth Solver parameters: \n");
  if (options->isSet == 0)
    printf("The solver parameters have not been set. \t options->isSet = %i \n", options->isSet);
  else
  {
    printf("The solver parameters below have  been set \t options->isSet = %i\n", options->isSet);
    printf("Name of the solver\t\t\t\t Lemke \n");
    if (options->iparam != NULL)
    {
      printf("int parameters \t\t\t\t\t options->iparam\n");
      printf("size of the int parameters\t\t\t options->iSize = %i\n", options->iSize);
      for (int i = 0; i < options->iSize; ++i)
        printf("\t\t\t\t\t\t options->iparam[%i] = %d\n", i, options->iparam[i]);
    }
    if (options->dparam != NULL)
    {
      printf("double parameters \t\t\t\t options->dparam\n");
      printf("size of the double parameters\t\t\t options->dSize = %i\n", options->dSize);
      for (int i = 0; i < options->iSize; ++i)
        printf("\t\t\t\t\t\t options->dparam[%i] = %.6le\n", i, options->dparam[i]);
    }
  }

  printf("See Lemke documentation for parameters definition)\n");
  printf("\n");
}