// This software is Copyright (C) 2010 Lyndon While, Lucas Bradstreet. 

// This program is free software (software libre). 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 2 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. 

// Source: http://www.wfg.csse.uwa.edu.au/hypervolume/
#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#include "wfg.h"

#define BEATS(x,y)   (x > y) 
#define WORSE(x,y)   (BEATS(y,x) ? (x) : (y)) 

int n;     // the number of objectives 
POINT ref; // the reference point 

FRONT *fs;    // memory management stuff 
int fr = 0;   // current depth 
int maxm = 0; // identify the biggest fronts in the file 
int maxn = 0; 
int safe;     // the number of points that don't need sorting
int** nextp;
int** prevp;
int* firstp;
int* lastp;
int* psize;

double totaltime;

double hv(FRONT);

int greater(const void *v1, const void *v2)
// this sorts points worsening in the last objective
{
	POINT p = *(POINT*)v1;
	POINT q = *(POINT*)v2;
	for (int i = n - 1; i >= 0; i--) {
		if BEATS(p.objectives[i],q.objectives[i]) {
			return -1;
		}
		else if BEATS(q.objectives[i],p.objectives[i]) {
			return  1;
		}
	}
	return 0;
}

int greaterabbrev(const void *v1, const void *v2)
// this sorts points worsening in the penultimate objective
{
	POINT p = *(POINT*)v1;
	POINT q = *(POINT*)v2;
	for (int i = n - 2; i >= 0; i--) {
		if BEATS(p.objectives[i],q.objectives[i]) {
			return -1;
		}
		else if BEATS(q.objectives[i],p.objectives[i]) {
			return  1;
		}
	}
	return 0;
}

int dominates2way(POINT p, POINT q, int k)
// returns -1 if p dominates q, 1 if q dominates p, 2 if p == q, 0 otherwise 
// k is the highest index inspected 
{
	for (int i = k; i >= 0; i--) {
		if BEATS(p.objectives[i],q.objectives[i]) {
			for (int j = i - 1; j >= 0; j--) {
	 			if BEATS(q.objectives[j],p.objectives[j]) {
	 				return 0; 
	 			}
			}
			return -1;
		}
		else if BEATS(q.objectives[i],p.objectives[i]) {
			for (int j = i - 1; j >= 0; j--) {
				if BEATS(p.objectives[j],q.objectives[j]) {
					return 0;
				}
			}
			return  1;
		}
	}
	return 2;
}

bool dominates1way(POINT p, POINT q, int k)
// returns true if p dominates q or p == q, false otherwise 
// the assumption is that q doesn't dominate p 
// k is the highest index inspected 
{
	for (int i = k; i >= 0; i--) {
		if BEATS(q.objectives[i],p.objectives[i]) {
			return false;
		}
	}
	return true;
}

void makeDominatedBit(FRONT ps, int p)
// creates the front ps[0 .. p-1] in fs[fr], with each point bounded by ps[p] and dominated points removed 
{
	int l = 0;
	int u = psize[fr] - 1;
	int index = lastp[fr];
	int removed = 0;
	for (int i = psize[fr] - 1; i >= 0; i--) {
		if (BEATS(ps.points[p].objectives[n - 1],ps.points[index].objectives[n - 1])) {
			fs[fr].points[u].objectives[n - 1] = ps.points[index].objectives[n - 1]; 
			for (int j = 0; j < n - 1; j++) {
				fs[fr].points[u].objectives[j] = WORSE(ps.points[p].objectives[j],ps.points[index].objectives[j]); 
			}
			if (dominates1way(ps.points[p],ps.points[index],n-2)) {
				int left = index - prevp[fr][index];
				int right = index + nextp[fr][index];
				if (left < 0) {
					firstp[fr] = right;
				}
				else {
					nextp[fr][left] = right-left;
				}
				if (right >= maxm) {
					lastp[fr] = left;
				}
				else {
					prevp[fr][right] = right-left;
				}
				removed++;
			}
			u--;
		}
		else {
			fs[fr].points[l].objectives[n - 1] = ps.points[p].objectives[n - 1]; 
			for (int j = 0; j < n - 1; j++) {
				fs[fr].points[l].objectives[j] = WORSE(ps.points[p].objectives[j],ps.points[index].objectives[j]); 
			}
			l++;
		}
		index -= prevp[fr][index];
	}
	POINT t;
  // points below l are all equal in the last objective; points above l are all worse 
  // points below l can dominate each other, and we don't need to compare the last objective 
  // points above l cannot dominate points that start below l, and we don't need to compare the last objective 
	fs[fr].nPoints = 1;
	for (int i = 1; i < l; i++) {
		int j = 0;
		while (j < fs[fr].nPoints) {
			switch (dominates2way(fs[fr].points[i], fs[fr].points[j], n-2)) {
				case  0: 
					j++; 
					break;
				case -1: // AT THIS POINT WE KNOW THAT i CANNOT BE DOMINATED BY ANY OTHER PROMOTED POINT j 
					// SWAP i INTO j, AND 1-WAY DOM FOR THE REST OF THE js 
					t = fs[fr].points[j];
					fs[fr].points[j] = fs[fr].points[i]; 
					fs[fr].points[i] = t; 
					while(j < fs[fr].nPoints - 1 && dominates1way(fs[fr].points[j], fs[fr].points[fs[fr].nPoints - 1], n-1)) {
						fs[fr].nPoints--;
					}
					int k = j+1; 
					while (k < fs[fr].nPoints) {
						if(dominates1way(fs[fr].points[j], fs[fr].points[k], n-2)) {
							t = fs[fr].points[k];
							fs[fr].nPoints--;
							fs[fr].points[k] = fs[fr].points[fs[fr].nPoints]; 
							fs[fr].points[fs[fr].nPoints] = t; 
						}
						else {
							k++;
						}
					}
				default: 
					j = fs[fr].nPoints + 1;
			}
		}
		if (j == fs[fr].nPoints) {
			t = fs[fr].points[fs[fr].nPoints]; 
			fs[fr].points[fs[fr].nPoints] = fs[fr].points[i]; 
			fs[fr].points[i] = t; 
			fs[fr].nPoints++;
		}
	}
	safe = WORSE(l,fs[fr].nPoints);
	for (int i = l; i < psize[fr]; i++) {
		int j = 0;
		while (j < safe) {
			if(dominates1way(fs[fr].points[j], fs[fr].points[i], n-2)) {
				j = fs[fr].nPoints + 1;
			}
			else {
				j++;
			}
		}
		while (j < fs[fr].nPoints) {
			switch (dominates2way(fs[fr].points[i], fs[fr].points[j], n-1)) {
				case  0: 
					j++; 
					break;
				case -1: // AT THIS POINT WE KNOW THAT i CANNOT BE DOMINATED BY ANY OTHER PROMOTED POINT j 
					// SWAP i INTO j, AND 1-WAY DOM FOR THE REST OF THE js 
					t = fs[fr].points[j];
					fs[fr].points[j] = fs[fr].points[i]; 
					fs[fr].points[i] = t; 
					while(j < fs[fr].nPoints - 1 && dominates1way(fs[fr].points[j], fs[fr].points[fs[fr].nPoints - 1], n-1)) {
						fs[fr].nPoints--;
					}
					int k = j+1; 
					while (k < fs[fr].nPoints) {
						if(dominates1way(fs[fr].points[j], fs[fr].points[k], n-1)) {
							t = fs[fr].points[k];
							fs[fr].nPoints--;
							fs[fr].points[k] = fs[fr].points[fs[fr].nPoints]; 
							fs[fr].points[fs[fr].nPoints] = t; 
						}
						else {
							k++;
						}
					}
				default: 
					j = fs[fr].nPoints + 1;
			}
		}
		if (j == fs[fr].nPoints) {
			t = fs[fr].points[fs[fr].nPoints]; 
			fs[fr].points[fs[fr].nPoints] = fs[fr].points[i]; 
			fs[fr].points[i] = t; 
			fs[fr].nPoints++;
		}
	}
	int last = lastp[fr];
	if (last >= 0) {
		nextp[fr][last] = p-last;
	}
	prevp[fr][p] = p-last;
	nextp[fr][p] = maxm-p;
	lastp[fr] = p;
	psize[fr] += 1 - removed;
	fr++;
}

double hv2(FRONT ps, int k)
// returns the hypervolume of ps[0 .. k-1] in 2D 
// assumes that ps is sorted improving
{
	double volume = ps.points[0].objectives[0] * ps.points[0].objectives[1]; 
	for (int i = 1; i < k; i++) {
		volume += ps.points[i].objectives[1] * (ps.points[i].objectives[0] - ps.points[i - 1].objectives[0]);
	}
	return volume;
}

double inclhv(POINT p)
// returns the inclusive hypervolume of p
{
	double volume = 1;
	for (int i = 0; i < n; i++) {
		volume *= p.objectives[i];
	}
	return volume;
}

double inclhv2(POINT p, POINT q)
// returns the hypervolume of {p, q}
{
	double vp  = 1; 
	double vq  = 1;
	double vpq = 1;
	for (int i = 0; i < n; i++) {
		vp  *= p.objectives[i];
		vq  *= q.objectives[i];
		vpq *= WORSE(p.objectives[i],q.objectives[i]);
	}
	return vp + vq - vpq;
}

double inclhv3(POINT p, POINT q, POINT r)
// returns the hypervolume of {p, q, r}
{
	double vp = 1;
	double vq = 1; 
	double vr = 1;
	double vpq = 1; 
	double vpr = 1; 
	double vqr = 1;
	double vpqr = 1;
	for (int i = 0; i < n; i++) {
		vp *= p.objectives[i];
		vq *= q.objectives[i];
		vr *= r.objectives[i];
		if (BEATS(p.objectives[i],q.objectives[i])) {
			if (BEATS(q.objectives[i],r.objectives[i])) {
				vpq  *= q.objectives[i];
				vpr  *= r.objectives[i];
				vqr  *= r.objectives[i];
				vpqr *= r.objectives[i];
			}
			else {
				vpq  *= q.objectives[i];
				vpr  *= WORSE(p.objectives[i],r.objectives[i]);
				vqr  *= q.objectives[i];
				vpqr *= q.objectives[i];
			}
		}
		else if (BEATS(p.objectives[i],r.objectives[i])) {
			vpq  *= p.objectives[i];
			vpr  *= r.objectives[i];
			vqr  *= r.objectives[i];
			vpqr *= r.objectives[i];
		}
		else {
			vpq  *= p.objectives[i];
			vpr  *= p.objectives[i];
			vqr  *= WORSE(q.objectives[i],r.objectives[i]);
			vpqr *= p.objectives[i];
		}
		
	}
	return vp + vq + vr - vpq - vpr - vqr + vpqr;
}

double inclhv4(POINT p, POINT q, POINT r, POINT s)
// returns the hypervolume of {p, q, r, s}
{
	double vp = 1; 
	double vq = 1; 
	double vr = 1; 
	double vs = 1;
	double vpq = 1; 
	double vpr = 1; 
	double vps = 1; 
	double vqr = 1; 
	double vqs = 1; 
	double vrs = 1; 
	double vpqr = 1; 
	double vpqs = 1; 
	double vprs = 1; 
	double vqrs = 1; 
	double vpqrs = 1; 
	for (int i = 0; i < n; i++) {
		vp *= p.objectives[i];
		vq *= q.objectives[i];
		vr *= r.objectives[i];
		vs *= s.objectives[i];
		if (BEATS(p.objectives[i],q.objectives[i])) {
			if (BEATS(q.objectives[i],r.objectives[i])) {
				if (BEATS(r.objectives[i],s.objectives[i])) {
					vpq *= q.objectives[i];
					vpr *= r.objectives[i];
					vps *= s.objectives[i];
					vqr *= r.objectives[i];
					vqs *= s.objectives[i];
					vrs *= s.objectives[i];
					vpqr *= r.objectives[i];
					vpqs *= s.objectives[i];
					vprs *= s.objectives[i];
					vqrs *= s.objectives[i];
					vpqrs *= s.objectives[i];
				}
				else {
					OBJECTIVE z1 = WORSE(q.objectives[i],s.objectives[i]);
					vpq *= q.objectives[i];
					vpr *= r.objectives[i];
					vps *= WORSE(p.objectives[i],s.objectives[i]);
					vqr *= r.objectives[i];
					vqs *= z1;
					vrs *= r.objectives[i];
					vpqr *= r.objectives[i];
					vpqs *= z1;
					vprs *= r.objectives[i];
					vqrs *= r.objectives[i];
					vpqrs *= r.objectives[i];
				}
			}
			else if (BEATS(q.objectives[i],s.objectives[i])) {
				vpq *= q.objectives[i];
				vpr *= WORSE(p.objectives[i],r.objectives[i]);
				vps *= s.objectives[i];
				vqr *= q.objectives[i];
				vqs *= s.objectives[i];
				vrs *= s.objectives[i];
				vpqr *= q.objectives[i];
				vpqs *= s.objectives[i];
				vprs *= s.objectives[i];
				vqrs *= s.objectives[i];
				vpqrs *= s.objectives[i];
			}
			else {
				OBJECTIVE z1 = WORSE(p.objectives[i],r.objectives[i]);
				vpq *= q.objectives[i];
				vpr *= z1;
				vps *= WORSE(p.objectives[i],s.objectives[i]);
				vqr *= q.objectives[i];
				vqs *= q.objectives[i];
				vrs *= WORSE(r.objectives[i],s.objectives[i]);
				vpqr *= q.objectives[i];
				vpqs *= q.objectives[i];
				vprs *= WORSE(z1,s.objectives[i]);
				vqrs *= q.objectives[i];
				vpqrs *= q.objectives[i];
			}
		}
		else if (BEATS(q.objectives[i],r.objectives[i])) {
			if (BEATS(p.objectives[i],s.objectives[i])) {
				OBJECTIVE z1 = WORSE(p.objectives[i],r.objectives[i]);
				OBJECTIVE z2 = WORSE(r.objectives[i],s.objectives[i]);
				vpq *= p.objectives[i];
				vpr *= z1;
				vps *= s.objectives[i];
				vqr *= r.objectives[i];
				vqs *= s.objectives[i];
				vrs *= z2;
				vpqr *= z1;
				vpqs *= s.objectives[i];
				vprs *= z2;
				vqrs *= z2;
				vpqrs *= z2;
			}
			else {
				OBJECTIVE z1 = WORSE(p.objectives[i],r.objectives[i]);
				OBJECTIVE z2 = WORSE(r.objectives[i],s.objectives[i]);
				vpq *= p.objectives[i];
				vpr *= z1;
				vps *= p.objectives[i];
				vqr *= r.objectives[i];
				vqs *= WORSE(q.objectives[i],s.objectives[i]);
				vrs *= z2;
				vpqr *= z1;
				vpqs *= p.objectives[i];
				vprs *= z1;
				vqrs *= z2;
				vpqrs *= z1;
			}
		}
		else if (BEATS(p.objectives[i],s.objectives[i])) {
				vpq *= p.objectives[i];
				vpr *= p.objectives[i];
				vps *= s.objectives[i];
				vqr *= q.objectives[i];
				vqs *= s.objectives[i];
				vrs *= s.objectives[i];
				vpqr *= p.objectives[i];
				vpqs *= s.objectives[i];
				vprs *= s.objectives[i];
				vqrs *= s.objectives[i];
				vpqrs *= s.objectives[i];
			}
		else {
			OBJECTIVE z1 = WORSE(q.objectives[i],s.objectives[i]);
			vpq *= p.objectives[i];
			vpr *= p.objectives[i];
			vps *= p.objectives[i];
			vqr *= q.objectives[i];
			vqs *= z1;
			vrs *= WORSE(r.objectives[i],s.objectives[i]);
			vpqr *= p.objectives[i];
			vpqs *= p.objectives[i];
			vprs *= p.objectives[i];
			vqrs *= z1;
			vpqrs *= p.objectives[i];
		}
	}
	return vp + vq + vr + vs - vpq - vpr - vps - vqr - vqs - vrs + vpqr + vpqs + vprs + vqrs - vpqrs;
}

double exclhv(FRONT ps, int p)
// returns the exclusive hypervolume of ps[p] relative to ps[0 .. p-1] 
{
	makeDominatedBit(ps, p);
	double volume = inclhv(ps.points[p]) - hv(fs[fr - 1]);
	fr--;
	return volume;
}

double hv(FRONT ps)
// returns the hypervolume of ps[0 ..] 
{
	// process small fronts with the IEA 
	switch (ps.nPoints) {
		case 1: 
			return inclhv (ps.points[0]); 
		case 2: 
			return inclhv2(ps.points[0], ps.points[1]); 
		case 3: 
			return inclhv3(ps.points[0], ps.points[1], ps.points[2]); 
		case 4: 
			return inclhv4(ps.points[0], ps.points[1], ps.points[2], ps.points[3]); 
	}

	// these points need sorting 
	qsort(&ps.points[safe], ps.nPoints - safe, sizeof(POINT), greater); 
	// n = 2 implies that safe = 0 
	if (n == 2) {
		return hv2(ps, ps.nPoints); 
	}
	// these points don't NEED sorting, but it helps 
	qsort(ps.points, safe, sizeof(POINT), greaterabbrev); 

	if (n == 3 && safe > 0) {
		double volume = ps.points[0].objectives[2] * hv2(ps, safe); 
		n--;
		for (int i=0; i<safe; i++) {
			nextp[fr][i] = 1;
			prevp[fr][i] = 1;
		}
		psize[fr] = safe;
		firstp[fr] = 0;
		lastp[fr] = safe-1;
		nextp[fr][safe-1] = maxm-safe+1;
		for (int i = safe; i < ps.nPoints; i++) {
			// we can ditch dominated points here, but they will be ditched anyway in makeDominatedBit 
			volume += ps.points[i].objectives[n] * exclhv(ps, i);
		}
		n++; 
		return volume;
	}
	else {
		double volume = inclhv4(ps.points[0], ps.points[1], ps.points[2], ps.points[3]); 
		n--;
		for (int i=0; i<4; i++) {
			nextp[fr][i] = 1;
			prevp[fr][i] = 1;
		}
		psize[fr] = 4;
		firstp[fr] = 0;
		lastp[fr] = 3;
		nextp[fr][3] = maxm-3;
		for (int i = 4; i < ps.nPoints; i++) {
			// we can ditch dominated points here, but they will be ditched anyway in makeDominatedBit 
			volume += ps.points[i].objectives[n] * exclhv(ps, i);
		}
		n++; 
		return volume;
	}
}

void allocate_memory(int maxm_, int maxn_) {
	maxm = maxm_;
	maxn = maxn_;
	n = maxn;
	safe = 0;
	// allocate memory
	int maxdepth = maxn - 2;
	fs = malloc(sizeof(FRONT) * maxdepth);
	for (int i = 0; i < maxdepth; i++) {
		fs[i].points = malloc(sizeof(POINT) * maxm);
		for (int j = 0; j < maxm; j++) {
			fs[i].points[j].objectives = malloc(sizeof(OBJECTIVE) * (maxn - i - 1));
		}
	}
	nextp = malloc(sizeof(int*) * maxdepth);
	for (int i=0; i<maxdepth; i++) {
		nextp[i] = malloc(sizeof(int) * maxm);
	}
	prevp = malloc(sizeof(int*) * maxdepth);
	for (int i=0; i<maxdepth; i++) {
		prevp[i] = malloc(sizeof(int) * maxm);
	}
	firstp = malloc(sizeof(int) * maxdepth);
	lastp = malloc(sizeof(int) * maxdepth);
	psize = malloc(sizeof(int) * maxdepth);
}

void free_memory()
{
	int maxdepth = maxn - 2;
	
	for (int i = 0; i < maxdepth; i++) {
		for (int j = 0; j < maxm; j++) {
			free(fs[i].points[j].objectives);
		}
		free(fs[i].points);
	}
	free(fs);
	
	for (int i=0; i<maxdepth; i++) {
		free(nextp[i]);
	}
	free(nextp);
	
	for (int i=0; i<maxdepth; i++) {
		free(prevp[i]);
	}
	free(prevp);
	free(firstp);
	free(lastp);
	free(psize);
}
