/*==================================================================

  Program:   View Generation Toolkit
  Module:    viewpoints.cxx
  Language:  C++
  Date:      2002/05/27
  Version:   1.0

===================================================================*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "viewpoints.h"

const double Pi = 3.1415927;
const double radtodeg = 180.0/Pi;
const double degtorad = Pi/180.0;


typedef struct {
    double  x, y, z;
} point;

typedef struct {
    point     pt[3];	/* Vertices of triangle */
    double    area;	/* Unused; might be used for adaptive subdivision */
} triangle;

typedef struct {
    int       npoly;	/* # of triangles in object */
    triangle *poly;	/* Triangles */
} object;

typedef struct {
  int    index;
  double value;
} sortstruct;

/* Six equidistant points lying on the unit sphere */
#define XPLUS {  1,  0,  0 }	/*  X */
#define XMIN  { -1,  0,  0 }	/* -X */
#define YPLUS {  0,  1,  0 }	/*  Y */
#define YMIN  {  0, -1,  0 }	/* -Y */
#define ZPLUS {  0,  0,  1 }	/*  Z */
#define ZMIN  {  0,  0, -1 }	/* -Z */

/* Vertices of a unit octahedron */
triangle octahedron[] = {
  { { YPLUS, XPLUS, ZPLUS }, 0.0 },//1
  { { YPLUS, ZPLUS, XMIN  }, 0.0 },//2
  { { YPLUS, XMIN , ZMIN  }, 0.0 },//3
  { { YPLUS, ZMIN , XPLUS }, 0.0 },//4
  { { YMIN , ZPLUS, XPLUS }, 0.0 },//5
  { { YMIN , XMIN , ZPLUS }, 0.0 },//6
  { { YMIN , ZMIN , XMIN  }, 0.0 },//7
  { { YMIN , XPLUS, ZMIN  }, 0.0 } //8
};

/* A unit octahedron */
object oct = {
    sizeof(octahedron) / sizeof(octahedron[0]),
    &octahedron[0]
};

int compare_sortstruct(const void* c, const void* d)
{
  sortstruct* a = (sortstruct*)c;
  sortstruct* b = (sortstruct*)d;

  if(a->value < b->value) 
    return -1;
  else if(a->value == b->value)
    return 0;
  else 
    return 1;
}

/* Return the midpoint on the line between two points */
point *midpoint(point* a, point* b)
{
    static point r;

    r.x = (a->x + b->x) * 0.5;
    r.y = (a->y + b->y) * 0.5;
    r.z = (a->z + b->z) * 0.5;

    return &r;
}

double dist(double phi1, double theta1, double phi2, double theta2)
{
  double cos_alpha;
  phi1 = phi1*degtorad;
  phi2 = phi2*degtorad;
  theta1 = theta1*degtorad;
  theta2 = theta2*degtorad;

  cos_alpha = sin(phi1)*sin(phi2)*cos(theta1-theta2)+cos(phi1)*cos(phi2);
  return acos(cos_alpha)*radtodeg;
}

point getmidpoint(triangle* t)
{
	point p;

	p.x = (t->pt[0].x + t->pt[1].x)/2;
	p.y = (t->pt[0].y + t->pt[1].y)/2;
	p.z = (t->pt[0].z + t->pt[1].z)/2;

	p.x = (t->pt[2].x - p.x)/3 + p.x;
	p.y = (t->pt[2].y - p.y)/3 + p.y;
	p.z = (t->pt[2].z - p.z)/3 + p.z;

	return p;
}

/* Normalize a point p */
point *normalize(point* p)
{
    static point r;
    double mag;

    r = *p;
    mag = r.x * r.x + r.y * r.y + r.z * r.z;
    if (mag != 0.0) {
	mag = 1.0 / sqrt(mag);
	r.x *= mag;
	r.y *= mag;
	r.z *= mag;
    }

    return &r;
}

// Computes r to the power p
int pow(int r, int p)
{
  int i = r;
  for(;p > 1;p--) i *= r;

  return i;
}


CViewPoints::CViewPoints(int n) 
{
  num = n;
  AllocatePoints(num);
}

CViewPoints::CViewPoints() 
{
    phi = NULL; theta = NULL;
}

CViewPoints::~CViewPoints() {
  // Cleans up memory allocated in ComputePoints
  //
  if(phi != NULL) free(phi);
  if(theta != NULL) free(theta);
}

int* CViewPoints::GetNeighbours(int ind, double range, int& n)
{
	int i;
	int* index;
	int* retarray = NULL;

	n = 0;
	if( ( index = (int*)malloc((num-1)*sizeof(int)) ) == NULL) return NULL;

	for(i=0; i<num; i++) {
		if(i!=ind && dist(phi[i], theta[i], phi[ind], theta[ind]) <= range) {
			index[n] = i;	
			n++;
		}
	}
   
	if(n>0 && ( retarray = (int*)malloc(n*sizeof(int)) )!=NULL)
		for(i=0; i<n; i++) retarray[i] = index[i];

	free(index);

	return retarray;
}

int* CViewPoints::GetNeighbours(int ind, int n)
{
   int i, q;
   sortstruct* ss = NULL;
   int* retarray = NULL;

   if( (ss = (sortstruct*)malloc((num-1)*sizeof(sortstruct)) ) == NULL)
	   return NULL;

   for(i=0, q=0; i<num; i++) {
     if(i!=ind){
       ss[q].index = i;
       ss[q].value = dist(phi[i], theta[i], phi[ind], theta[ind]);
       
       q++;
     }
   }

   qsort((void *)ss, (size_t)(num-1), sizeof(sortstruct), compare_sortstruct);
   
   if( (num-1) < n) n = num-1;
   if( ( retarray = (int*)malloc(n*sizeof(int)) ) == NULL) return NULL;
   
   for(i=0; i < n; i++){
     retarray[i] = ss[i].index;
   }

   free(ss);

   return retarray;
}

void CViewPoints::AllocatePoints(int n)
{
  if( (phi = (float*)malloc(sizeof(float)*n)) == NULL) Abort();
  if( (theta = (float*)malloc(sizeof(float)*n)) == NULL){
    free(phi);
    Abort();
  }
}

void CViewPoints::Abort()
{
  // Cleans up memory allocated in ComputePoints
  //
  if(phi != NULL) free(phi);
  if(theta != NULL) free(theta);
  
  printf("Abort has occured\n");

  abort();
}

float CViewPoints::Phi(int ind) {
  if((ind > -1) && (ind < num)) 
	return phi[ind];
  else
	return -1;
}

float CViewPoints::Theta(int ind) {
  if((ind > -1) && (ind < num)) 
	  return theta[ind];
  else
	  return -1;
}

int CViewPoints::Size()
{
  return num;
}

int CViewPoints::Level()
{
  return mLevel;
}

COctahedronPoints::COctahedronPoints(int n)
{
  mLevel = n;

  if(n < 1) abort();

  num = 2*pow(4,n);

  AllocatePoints(num);
  ComputePoints();
}


void COctahedronPoints::ComputePoints()
{
    object *old = &oct,		/* Default is octahedron */
	   *newm;

    int     i, 
            level,		/* Current subdivision level */
	    maxlevel = mLevel;	/* Maximum subdivision level */


    /* Subdivide each starting triangle (maxlevel - 1) times */
    for (level = 1; level < maxlevel; level++) {
	/* Allocate a new object */
	newm = (object *)malloc(sizeof(object));
	if (newm == NULL) {
	    Abort();
	}
	newm->npoly = old->npoly * 4;

	/* Allocate 4* the number of points in the current approximation */
	newm->poly  = (triangle *)malloc(newm->npoly * sizeof(triangle));
	if (newm->poly == NULL) {
	    Abort();
	}

	/* Subdivide each triangle in the old approximation and normalize
	 *  the new points thus generated to lie on the surface of the unit
	 *  sphere.
	 * Each input triangle with vertices labelled [0,1,2] as shown
	 *  below will be turned into four new triangles:
	 *
	 *			Make new points
	 *			    a = (0+2)/2
	 *			    b = (0+1)/2
	 *			    c = (1+2)/2
	 *	  1
	 *	 /\		Normalize a, b, c
	 *	/  \
	 *    b/____\ c		Construct new triangles
	 *    /\    /\		    [0,b,a]
	 *   /	\  /  \		    [b,1,c]
	 *  /____\/____\	    [a,b,c]
	 * 0	  a	2	    [a,c,2]
	 */
	for (i = 0; i < old->npoly; i++) {
	    triangle
		 *oldt = &old->poly[i],
		 *newt = &newm->poly[i*4];
	    point a, b, c;

	    a = *normalize(midpoint(&oldt->pt[0], &oldt->pt[1]));
	    b = *normalize(midpoint(&oldt->pt[0], &oldt->pt[2]));
	    c = *normalize(midpoint(&oldt->pt[1], &oldt->pt[2]));

	    newt->pt[0] = oldt->pt[0];
	    newt->pt[1] = a;
	    newt->pt[2] = b;
	    newt++;

	    newt->pt[0] = a;
	    newt->pt[1] = oldt->pt[1];
	    newt->pt[2] = c;
	    newt++;

	    newt->pt[0] = c;
	    newt->pt[1] = a;
	    newt->pt[2] = b;
	    newt++;

	    newt->pt[0] = b;
	    newt->pt[1] = c;
	    newt->pt[2] = oldt->pt[2];
	}

	if (level > 1) {
	    free(old->poly);
	    free(old);
	}

	/* Continue subdividing new triangles */
	old = newm;
    }
    

    /* Spit out coordinates for each triangle */
    point p;
    double l1, l2;

    triangle *t;
    for (i = 0; i < old->npoly; i++) {
	t = &old->poly[i];

	p = getmidpoint(t);
	l1 = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
	l2 = sqrt(p.x*p.x + p.y*p.y);
       
	phi[i] = float(acos(p.z/l1)*radtodeg);
	if(l2 == 0) 
	  theta[i] = 0;
	else {
	  theta[i] = float(acos(p.x/l2)*radtodeg);
	  if(p.y < 0) theta[i] = 360 - theta[i];
	}
    }

    free(old);
}
