/*
//#     Filename:       SpatialConstraint.c
//#
//#     The SpatialConstraint functions are written here
//#
//#     Author:         Peter Z. Kunszt based on A. Szalay's code
//#     
//#     Date:           October 23, 1998
//#
//#
//#
//# (c) Copyright The Johns Hopkins University 1998
//# All Rights Reserved
//#
//# The software and information contained herein are proprietary to The
//# Johns Hopkins University, Copyright 1998.  This software is furnished
//# pursuant to a written license agreement and may be used, copied,
//# transmitted, and stored only in accordance with the terms of such
//# license and with the inclusion of the above copyright notice.  This
//# software and information or any other copies thereof may not be
//# provided or otherwise made available to any other person.
//#
//#
//#     Modification History:
//#
*/
#include <stdlib.h>//added 8/15/02
#include <string.h>//added 8/15/02
#define HTM_LIB
#include "SpatialConstraint.h"

#define COMMENT '#'

/*
// ===========================================================================
//
// Member functions for class SpatialConstraint
//
// ===========================================================================

/////////////CONSTRUCTOR//////////////////////////////////
//
*/
SpatialConstraint * spatialConstraintNew() {

  SpatialConstraint *c;
  c = (SpatialConstraint *)malloc(sizeof(SpatialConstraint));
  spatialConstraintInit(c);

  return c;
}

void spatialConstraintInit( SpatialConstraint * c ) 
{
  c->a_.x_ 	= 1.0;
  c->a_.y_ 	= 0.0;
  c->a_.z_ 	= 0.0;
  c->a_.ra_	= 0.0;
  c->a_.dec_	= 0.0;
  c->a_.okRaDec_= true;
  c->d_         = 0.0;
  c->s_		= 0.0;
  c->sign_	= zERO;
}

/* /////////////DESTRUCTOR///////////////////////////////////
 */
void spatialConstraintDelete( SpatialConstraint *c )
{
  if(c)free(c);
}

/* Initialization constructor */
SpatialConstraint * spatialConstraintNewVec( const SpatialVector *v, 
					     float64 d )
{
  SpatialConstraint *c = spatialConstraintNew();
  spatialVectorAssign( &(c->a_) , v );
  spatialVectorNormalize( &(c->a_) );
  c->d_ = d;
  c->s_ = acos(d);
  if(c->d_ <= -gEpsilon) c->sign_ = nEG;
  if(c->d_ >=  gEpsilon) c->sign_ = pOS;

  return c;
}

/* /////////////COPY CONSTRUCTOR/////////////////////////////
 */
SpatialConstraint * spatialConstraintCopy( const SpatialConstraint *cc ) {

  SpatialConstraint *c;
  c = (SpatialConstraint *)malloc(sizeof(SpatialConstraint));

  spatialVectorAssign( &(c->a_), (&cc->a_) );
  c->d_ = cc->d_;
  c->s_ = cc->s_;
  c->sign_ = cc->sign_;

  return c;
}

/* /////////////ASSIGNMENT///////////////////////////////////
 */
void spatialConstraintAssign( SpatialConstraint *c1, 
			      const SpatialConstraint *c2 )
{
  if ( c1 != c2 ) { /* beware of self-assignment */
    spatialVectorAssign( &(c1->a_), &(c2->a_) );
    c1->d_ = c2->d_;
    c1->s_ = c2->s_;
    c1->sign_ = c2->sign_;
  }
}

/* set */
void spatialConstraintSet( SpatialConstraint *c, 
			     const SpatialVector *v, float64 d )
{
  spatialVectorAssign( &(c->a_) , v );
  spatialVectorNormalize( &(c->a_) );
  c->d_ = d;
  c->s_ = acos(d);
  c->sign_ = zERO;
  if(c->d_ <= -gEpsilon) c->sign_ = nEG;
  if(c->d_ >=  gEpsilon) c->sign_ = pOS;

}

/* /////////////CONTAINS/////////////////////////////////////
// check whether a vector is inside this
*/
bool spatialConstraintContains( const SpatialConstraint *c, 
				const SpatialVector *v )
{
    if ( acos( spatialVectorDot( v , &(c->a_) ) ) < c->s_ ) return true;
    return false;
}

/* /////////////INVERT///////////////////////////////////////
 */
void spatialConstraintInvert( SpatialConstraint *c ) {
  c->d_ = - c->d_;
  c->s_ = acos(c->d_);
  if(c->sign_ == nEG) c->sign_ = pOS;
  if(c->sign_ == pOS) c->sign_ = nEG;
}

/* /////////////READ/////////////////////////////////////////
 */
void spatialConstraintRead( SpatialConstraint *c, FILE *in)
{
  char buffer[HTM_READBUFSIZE];
  char first;
  buffer[0] = '\0';

  /* ignore lines that begin with the comment character */
  first = (char)getc(in);
  while(first == COMMENT) {
    fgets(buffer, HTM_READBUFSIZE, in);
    first = (char)getc(in);
  }
  ungetc( (int)first, in );

  /* read the vector from the stream first, normalize it */
  spatialVectorRead( &(c->a_), in );
  spatialVectorNormalize( &(c->a_) );

  /* now read the distance and calculate the angle */
  getStreamToken(buffer,in);
  sscanf(buffer,"%lf", &(c->d_));  
  c->s_ = acos(c->d_);

  /* set sign */
  if     (c->d_ <= -gEpsilon) c->sign_ = nEG;
  else if(c->d_ >=  gEpsilon) c->sign_ = pOS;
  else                c->sign_ = zERO;
}

/* /////////////READRADEC/////////////////////////////////////
 */
void spatialConstraintReadRaDec( SpatialConstraint *c, FILE *in)
{
  char buffer[HTM_READBUFSIZE];
  char first;
  float64 ra,dec;
  buffer[0] = '\0';

  /* ignore lines that begin with the comment character */
  first = (char)getc(in);
  while(first == COMMENT) {
    fgets(buffer, HTM_READBUFSIZE, in);
    first = (char)getc(in);
  }
  ungetc( (int)first, in );

  /* read ra,dec from the stream first set vector */
  getStreamToken(buffer,in);
  sscanf(buffer,"%lf", &ra);  
  getStreamToken(buffer,in);
  sscanf(buffer,"%lf", &dec);

  spatialVectorSetRaDec( &(c->a_), ra, dec );

  /* now read the distance and calculate the angle */
  getStreamToken(buffer,in);
  sscanf(buffer,"%lf", &(c->d_));  
  c->s_ = acos(c->d_);

  /* set sign */
  if     (c->d_ <= -gEpsilon) c->sign_ = nEG;
  else if(c->d_ >=  gEpsilon) c->sign_ = pOS;
  else                c->sign_ = zERO;
}

/* /////////////WRITE////////////////////////////////////////
 */
void spatialConstraintWrite( const SpatialConstraint *c, FILE *f) {
  spatialVectorWrite( &(c->a_), f );
  fprintf(f," %19.15f\n",c->d_);
}


size_t spatialConstraintVecAppend( SpatialConstraintVec *v, 
				   const SpatialConstraint *c )
{

  size_t newsize = v->capacity_ * 2;
  if(v->length_ == v->capacity_) {
    if(newsize == 0){
      newsize = 2;
      v->vector_ = (SpatialConstraint *)malloc( newsize * 
						sizeof(SpatialConstraint) );
    } else
      v->vector_ = (SpatialConstraint *)realloc( v->vector_, newsize * 
						 sizeof(SpatialConstraint) );
    v->capacity_ = newsize;
  }
  spatialConstraintAssign( &(v->vector_[v->length_]), c );
  return v->length_++;
}

void spatialConstraintVecClear( SpatialConstraintVec *v , bool keep ) {
  if(!keep){
    free(v->vector_);
    v->capacity_ = 0;
  }
  v->length_ = 0;
}

void spatialConstraintVecRemove( SpatialConstraintVec *v , size_t index ) {

  SpatialConstraint * newvec;
  newvec = (SpatialConstraint *)malloc( sizeof(SpatialConstraint) *
					v->capacity_ );
  /* copy the elements up to index */
  memcpy(newvec,v->vector_, index * sizeof(SpatialConstraint) );
  /* now copy all elements from index+1 */
  memcpy(newvec + index, v->vector_ + index + 1, v->length_ - index - 1);
  v->length_--;

  free(v->vector_);
  v->vector_ = newvec;
}