/*
//#     Filename:       SpatialVector.c
//#
//#     The SpatialVector functions are here.
//#
//#     Author:         Peter Z. Kunszt based on A. Szalay's code
//#
//#     Date:           October 15, 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:
//#
*/
#define HTM_LIB
#include <stdlib.h>//added 8/15/02
#include <string.h>//added 8/15/02
#include "SpatialVector.h"
#include "assert.h"
/*
//==============================================================
//
// This 3D vector lives on the surface of the sphere.
// Its length is always 1.
//
//==============================================================
*/

/* /////////////CONSTRUCTOR//////////////////////////////////
//
// constructs (1,0,0), ra=0, dec=0.
*/
SpatialVector* spatialVectorNew( ) {

  SpatialVector *v;
  v = (SpatialVector *)malloc(sizeof(SpatialVector));

  v->x_ 	= 1.0;
  v->y_ 	= 0.0;
  v->z_ 	= 0.0;
  v->ra_	= 0.0;
  v->dec_	= 0.0;
  v->okRaDec_	= true;

  return v;
}

/* /////////////DESTRUCTOR///////////////////////////////////
 */
void spatialVectorDelete( SpatialVector *v )
{
  if(v)free(v);
}

/* /////////////CONSTRUCTOR//////////////////////////////////
 */
SpatialVector* spatialVectorNewXYZ( float64 x,
				    float64 y,
				    float64 z ) {

  SpatialVector *v;
  v = (SpatialVector *)malloc(sizeof(SpatialVector));

  v->x_ 	= x;
  v->y_ 	= y;
  v->z_ 	= z;
  v->ra_	= 0.0;
  v->dec_	= 0.0;
  v->okRaDec_	= false;

  return v;
}

/* /////////////CONSTRUCTOR//////////////////////////////////
//
// Constructor from ra/dec, this is always normed to 1
*/
SpatialVector* spatialVectorNewRaDec( float64 ra,
				      float64 dec ) {

  SpatialVector *v;
  v = spatialVectorNew();

  v->ra_      = ra;
  v->dec_     = dec;
  v->okRaDec_ = true;
  spatialVectorUpdateXYZ(v);
  spatialVectorUpdateRaDec(v);

  return v;
}

/* /////////////COPY CONSTRUCTOR/////////////////////////////
 */
SpatialVector* spatialVectorNewCopy( const SpatialVector *vv ) {
  SpatialVector *v;
  v = (SpatialVector *)malloc(sizeof(SpatialVector));

  v->x_ 	= vv->x_;
  v->y_ 	= vv->y_;
  v->z_ 	= vv->z_;
  v->ra_	= vv->ra_;
  v->dec_	= vv->dec_;
  v->okRaDec_	= vv->okRaDec_;

  return v;
}

/* /////////////ASSIGNMENT///////////////////////////////////
 */
void spatialVectorAssign( SpatialVector *v, const SpatialVector * vv ) {

  if ( v != vv ) { /* beware of self-assignment */
    v->x_ 	= vv->x_;
    v->y_ 	= vv->y_;
    v->z_ 	= vv->z_;
    v->ra_	= vv->ra_;
    v->dec_	= vv->dec_;
    v->okRaDec_	= vv->okRaDec_;
  }
}

/* /////////////SET//////////////////////////////////////////
//
// Set member function: set values - always normed to 1
*/
void spatialVectorSet( SpatialVector *v,
		       const float64 x,
		       const float64 y,
		       const float64 z ) {

  v->x_ 	= x;
  v->y_ 	= y;
  v->z_ 	= z;

  spatialVectorNormalize(v);
  spatialVectorUpdateRaDec(v);

}

/* /////////////SET//////////////////////////////////////////
//
// Set member function: set values - always normed to 1
*/
void spatialVectorSetRaDec( SpatialVector *v,
			    const float64 ra,
			    const float64 dec ) {
  v->ra_  = ra;  
  v->dec_ = dec;
  spatialVectorUpdateXYZ(v);

}

/* /////////////GET//////////////////////////////////////////
//
// Get x,y,z
*/
void spatialVectorGet( SpatialVector *v,
		       float64 *x,
		       float64 *y,
		       float64 *z ) {

  *x = v->x_;
  *y = v->y_;
  *z = v->z_;
}

/* /////////////GET//////////////////////////////////////////
//
// Get ra,dec - normalizes x,y,z
*/
void spatialVectorGetRaDec( SpatialVector *v,
			    float64 *ra,
			    float64 *dec ) {
  if(!v->okRaDec_) {
    spatialVectorNormalize(v);
    spatialVectorUpdateRaDec(v);
  }
  *ra = v->ra_;
  *dec = v->dec_;
}

float64 spatialVectorRa( SpatialVector *v ) {

  if(!v->okRaDec_) {
    spatialVectorNormalize(v);
    spatialVectorUpdateRaDec(v);
  }
  return v->ra_;
}

float64 spatialVectorDec( SpatialVector *v ) {

  if(!v->okRaDec_) {
    spatialVectorNormalize(v);
    spatialVectorUpdateRaDec(v);
  }
  return v->dec_;
}


/* /////////////NORMALIZE////////////////////////////////////
 */
void spatialVectorNormalize( SpatialVector *v ) {
  float64 sum;
  sum = v->x_*v->x_ + v->y_*v->y_ + v->z_*v->z_;
  sum = sqrt(sum);
  v->x_ /= sum;
  v->y_ /= sum;
  v->z_ /= sum;
}

/* /////////////LENGTH///////////////////////////////////////
 */
float64 spatialVectorLength( const SpatialVector *v ) {
  float64 sum;
  sum = v->x_*v->x_ + v->y_*v->y_ + v->z_*v->z_;
  return (sum > gEpsilon ? sqrt(sum) : 0.0);
}

/* /////////////UPDATERADEC//////////////////////////////////
 */
void spatialVectorUpdateRaDec( SpatialVector *v ) {

  float64 cd;

  v->dec_ = asin(v->z_)/gPr; /* easy.*/
  cd = cos(v->dec_*gPr);
  if(cd>gEpsilon || cd<-gEpsilon)
    if(v->y_>gEpsilon || v->y_<-gEpsilon)
      if (v->y_ < 0.0)
	v->ra_ = 360 - acos(v->x_/cd)/gPr;
      else
	v->ra_ = acos(v->x_/cd)/gPr;
    else
      v->ra_ = (v->x_ < 0.0 ? 180.0 : 0.0);
  else 
    v->ra_=0.0;
  v->okRaDec_ = true;
}

/* /////////////UPDATEXYZ////////////////////////////////////
 */
void spatialVectorUpdateXYZ( SpatialVector *v ) {
    float64 cd = cos(v->dec_*gPr);
    v->x_ = cos(v->ra_*gPr) * cd;
    v->y_ = sin(v->ra_*gPr) * cd;
    v->z_ = sin(v->dec_*gPr);
}
/* /////////////OPERATOR *=//////////////////////////////////
// multiply with a number
*/
void spatialVectorMul( SpatialVector *v, float64 a ) {

  v->x_ *= a;  
  v->y_ *= a;  
  v->z_ *= a;
  v->okRaDec_ = false;

}

/* /////////////OPERATOR * ///////////////////////////////////
// dot product
*/
float64 spatialVectorDot( const SpatialVector *v1, const SpatialVector*v2 ) {

  return (v1->x_*v2->x_)+(v1->y_*v2->y_)+(v1->z_*v2->z_);
}

/* /////////////OPERATOR +///////////////////////////////////
 */
SpatialVector * spatialVectorAddNew( const SpatialVector *v1, 
				     const SpatialVector*v2 )
{
  SpatialVector *v = spatialVectorNewXYZ( v1->x_ + v2->x_, 
					  v1->y_ + v2->y_, 
					  v1->z_ + v2->z_ );
  return v;
}

/* /////////////OPERATOR -///////////////////////////////////
 */
SpatialVector * spatialVectorSubNew( const SpatialVector *v1, 
				     const SpatialVector*v2 )
{
  SpatialVector *v = spatialVectorNewXYZ( v1->x_ - v2->x_, 
					  v1->y_ - v2->y_, 
					  v1->z_ - v2->z_ );
  return v;
}

/* /////////////OPERATOR ^///////////////////////////////////
// cross product
*/
SpatialVector * spatialVectorCrossNew( const SpatialVector *v1, 
				       const SpatialVector*v2 )
{
  SpatialVector *v = spatialVectorNewXYZ(v1->y_ * v2->z_ - v2->y_ * v1->z_,
					 v1->z_ * v2->x_ - v2->z_ * v1->x_,
					 v1->x_ * v2->y_ - v2->x_ * v1->y_);
					 
  return v;
}

/* /////////////OPERATOR +///////////////////////////////////
 */
void spatialVectorAddNorm( const SpatialVector *v1, const SpatialVector*v2,
			   SpatialVector *v)
{
  spatialVectorSet( v, v1->x_ + v2->x_, v1->y_ + v2->y_, v1->z_ + v2->z_ );
}

/* /////////////OPERATOR +///////////////////////////////////
 */
void spatialVectorAdd( const SpatialVector *v1, const SpatialVector*v2,
		       SpatialVector *v)
{
  v->x_ = v1->x_ + v2->x_;
  v->y_ = v1->y_ + v2->y_;
  v->z_ = v1->z_ + v2->z_;
}

/* /////////////OPERATOR -///////////////////////////////////
 */
void spatialVectorSubNorm( const SpatialVector *v1, const SpatialVector*v2,
			   SpatialVector *v)
{
  spatialVectorSet( v, v1->x_ - v2->x_, v1->y_ - v2->y_, v1->z_ - v2->z_ );
}

/* /////////////OPERATOR -///////////////////////////////////
 */
void spatialVectorSub( const SpatialVector *v1, const SpatialVector*v2,
		       SpatialVector *v)
{
  v->x_ = v1->x_ - v2->x_;
  v->y_ = v1->y_ - v2->y_;
  v->z_ = v1->z_ - v2->z_;
}

/* /////////////OPERATOR ^///////////////////////////////////
// cross product
*/
void spatialVectorCrossNorm( const SpatialVector *v1, const SpatialVector*v2,
			     SpatialVector *v)
{
  spatialVectorSet( v, 
		    v1->y_ * v2->z_ - v2->y_ * v1->z_,
		    v1->z_ * v2->x_ - v2->z_ * v1->x_,
		    v1->x_ * v2->y_ - v2->x_ * v1->y_);
}

/* /////////////OPERATOR ^///////////////////////////////////
// cross product
*/
void spatialVectorCross( const SpatialVector *v1, const SpatialVector*v2,
			 SpatialVector *v)
{
  v->x_ =    v1->y_ * v2->z_ - v2->y_ * v1->z_;
  v->y_ =    v1->z_ * v2->x_ - v2->z_ * v1->x_;
  v->z_ =    v1->x_ * v2->y_ - v2->x_ * v1->y_;
}

/* /////////////OPERATOR ==//////////////////////////////////
 */
int spatialVectorEQ( const SpatialVector *v1, const SpatialVector *v2 ) {
  return ((v1->x_ == v2->x_ && v1->y_ == v2->y_ && v1->z_ == v2->z_) ? 1 : 0 );
}

/* /////////////SHOW/////////////////////////////////////////
// print to stdout
*/
void spatialVectorShow( const SpatialVector *v )
{
  printf(" %11.8lf %11.8lf %11.8lf \n",v->x_,v->y_,v->z_);
}

/* /////////////READ/////////////////////////////////////////
// read from file
*/
void spatialVectorRead( SpatialVector *v, FILE * in )
{
  char buffer[HTM_READBUFSIZE];
  assert(getStreamToken(buffer,in));
  sscanf(buffer,"%lf", &(v->x_));
  assert(getStreamToken(buffer,in));
  sscanf(buffer,"%lf", &(v->y_));
  assert(getStreamToken(buffer,in));
  sscanf(buffer,"%lf", &(v->z_));
  v->okRaDec_ = false;
  /*  printf(" READ = %19.15lf %19.15lf %19.15lf \n", v->x_, v->y_, v->z_ );
   */
}

/* /////////////WRITE////////////////////////////////////////
// write to file
*/
void spatialVectorWrite( const SpatialVector *v, FILE * f )
{
  fprintf(f," %19.15lf %19.15lf %19.15lf ",v->x_,v->y_,v->z_);
}

/* get a token from the stream */
char * getStreamToken( char *buf, FILE *f) {

  char t, *p=buf;

  /* skip whitespace */
  while( (t = (char)getc(f)) != EOF )
    if(t != ' ' && t != '\t' && t != '\n' && t != '\r')break;

  if(t == EOF)return NULL;
  *(p++) = t;

  /* get everything until next whitespace */
  while( (t = (char)getc(f)) != EOF )
    if(t == ' ' || t == '\t' || t == '\n' || t == '\r')break;
    else *(p++) = t;

  /* get also all following whitespace and skip it */
  while( (t = (char)getc(f)) != EOF )
    if(t != ' ' && t != '\t' && t != '\n' && t != '\r')break;
  ungetc((int)t,f);

  *p = '\0';
  return buf;
}

void spatialVectorVecAppend( SpatialVectorVec *v, const SpatialVector *c )
{

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

void spatialVectorVecInit( SpatialVectorVec *v ) {
  v->vector_ = NULL;
  v->length_ = 0;
  v->capacity_ = 0;
}

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