#include <stdlib.h>
#include <stdio.h>
#include "polytope.h"

void display_polytope(Polytope* p)
{
	int i;
	printf("Number of inequalities: %d, Dimension of the polytope: %d\n",p->k,p->l);
	printf("A matrix: ");
	for(i=0;i<((p->k)*(p->l));i++)
	{
		printf("%5.2f, ",*(p->A+i));
	}
	printf("\nb vector: ");
	for(i=0;i<p->k;i++)
	{
		printf("%5.2f, ",*(p->b+i));
	}
	printf("\ncenter: ");
	for(i=0;i<p->l;i++)
	{
		printf("%5.2f, ",*(p->center+i));
	}
	printf("\n");
}

int isInside(const Polytope* p, const pfloat x[])
{
	idxint k = p->k;
	idxint l = p->l;
	pfloat *A = p->A;
	int i,j;
	pfloat rowSum;
	for(j=0;j<k;j++)
	{
		rowSum = 0;
		for(i=0;i<l;i++)
		{
			rowSum+= *(A+i*k+j) * x[i];
		}
		if(rowSum < p->b[i])
		{
			return 0;
		}
	}
	return 1;
}

Polytope* from_box(idxint l,pfloat* upper,pfloat* lower)
{
	Polytope *p = (Polytope*)malloc(sizeof(Polytope));
	int i;
	idxint k = l*2;
	p->k = k;
	p->l = l;
	p->A = (pfloat*)malloc(sizeof(pfloat)*l*k);
	for(i=0;i< l*k;i++)
	{
		*(p->A+i) = 0;
	}
	p->b = (pfloat*)malloc(sizeof(pfloat)*k);
	p->center = (pfloat*)malloc(sizeof(pfloat)*l);
	for(i=0;i<l;i++)
	{
		*(p->A+i*k+2*i) = 1;
		*(p->A+i*k+2*i+1) = -1;
		*(p->b+2*i) = *(lower+i);
		*(p->b+2*i+1) =-1*(*(upper+i));
		*(p->center+i) = (*(upper+i) + *(lower+i))/2;
	}
	return p;
}

Polytope* create_poly(idxint k,idxint l,pfloat* A,pfloat* b,pfloat* center)
{
	Polytope *p = (Polytope*)malloc(sizeof(Polytope));
	p->k = k;
	p->l = l;
	p->A = A;
	p->b = b;
	p->center = center;
	return p;
}

Polytope* cartesian_prod(Polytope* p1, Polytope* p2)
{
	idxint k1 = p1->k;
	idxint k2 = p2->k;
	idxint l1 = p1->l;
	idxint l2 = p2->l;
	idxint k = k1+k2;
	idxint l = l1+l2;
	pfloat* A = (pfloat*) malloc(sizeof(pfloat)*k*l);
	pfloat* b = (pfloat*) malloc(sizeof(pfloat)*k);
	pfloat* center = (pfloat*) malloc(sizeof(pfloat)*l);
	int i,j;
	Polytope *p;
	for(i=0;i<k;i++)
	{
		for(j=0;j<l;j++)
		{
			if(i<k1 && j<l1)
			{
				*(A+i+j*k) = *(p1->A+i+j*k1);
			}
			else if (i>=k1 && j>=l1)
			{
				*(A+i+j*k) = *(p2->A+(i-k1)+(j-l1)*k2);
			}
			else
			{
				*(A+i+j*k) = 0;
			}
		}
	}
	for(i=0;i<k;i++)
	{
		if (i < k1)
		{
			*(b+i) = *(p1->b+i);
		}
		else
		{
			*(b+i) = *(p2->b+i-k1);
		}
	}
	for(j=0;j<l;j++)
	{
		if (j < l1)
		{
			*(center+j) = *(p1->center+j);
		}
		else
		{
			*(center+j) = *(p2->center+j-l1);
		}

	}
	p = create_poly(k,l,A,b,center);
	return p;
}

void free_polytope(Polytope* p)
{
	free(p->A);
	free(p->b);
	free(p->center);
	free(p);
}
