
#include "geninc.h"
#include "gfidb.h"
#include "adcp.h"
#include "data_id.h"
#include "gfidb.h"

#define LINE_LENGTH 200

typedef struct {
    double h, p, r;
} ATTITUDE_TYPE;
    
static int year_base;
static FILE_NAME_TYPE dbname, att_file;

static PARAMETER_LIST_ENTRY_TYPE params[]=
{
	{" year_base:"     , " %d"    , (char *) &year_base, TYPE_INT},
	{" dbname:"        , " %79s"  , (char *) dbname    , TYPE_STRING},
	{" att_file:"      , " %79s"  , (char *) att_file  , TYPE_STRING},
	{NULL              , NULL     , (char *) NULL      , 0}
};



void init(void)
{
  
  strcpy(att_file, "");
  strcpy(dbname, "");
  
  return;
}

int get_data(int type, void **data, int *n)
{
  
  int nb;
  int ierr=0;
  float *data_f=NULL;
  char  *data_c=NULL;
  

  /* Release memory from a previous call */
  if(*data)
    free(*data);
  
  switch(type){
    
    case ANCILLARY_1:
    case ANCILLARY_2:
    case BOTTOM_TRACK:

      DBGET(&type, (char *) NULL, &nb, &ierr);
      if(DBERROR(&ierr , "DBGET: number of bytes for data")) return(ierr);
      
      data_c = (char *) calloc(nb, sizeof(char));
      ierr = (data_c == NULL) ? INSUFFICIENT_MEMORY : 0;
      if(DBERROR(&ierr, "calloc(): memory allocation for data"))
        return(ierr);
      
      DBGET(&type, data_c, &nb, &ierr);
      if(DBERROR(&ierr, "DBGET: get data"))
        return(ierr);

      *data = (void *) data_c;
      break;
      
    case U:
    case V:
    case W:

      DBGET_F(&type, (FLOAT *) NULL, n, &ierr);
      if(DBERROR(&ierr , "DBGET_F: number of elements for data")) return(ierr);
      
      data_f = (float *) calloc(*n, sizeof(FLOAT));
      ierr = (data_f == NULL) ? INSUFFICIENT_MEMORY : 0;
      if(DBERROR(&ierr, "calloc(): memory allocation for data"))
        return(ierr);
      
      DBGET_F(&type, (FLOAT *) data_f, n, &ierr);
      if(DBERROR(&ierr, "DBGET: get data"))
        return(ierr);

      *data = (void *) data_f;
      break;
      
    default:
      ierr = INVALID_NUMBER;
      DBERROR(&ierr, "get_data: bad data type");
      return(ierr);
      break;
  }
  
  return(ierr);
}

int put_data(int type, void *data, int n)
{
  
  int nbad, ierr=0;

  switch(type){
    
    case ANCILLARY_1:
    case ANCILLARY_2:
    case BOTTOM_TRACK:
      
      DBPUT(&type, (char *) data, &n, &ierr);
      if(DBERROR(&ierr, "DBPUT: put data"))
        return(ierr);
      break;

    case U:
    case V:
    case W:

      DBPUT_F(&type, (float *) data, &n, &nbad, &ierr);
      if(DBERROR(&ierr, "DBPUT_F: put data"))
        return(ierr);
      break;

    default:
      ierr = INVALID_NUMBER;
      DBERROR(&ierr, "get_data: bad data type");
      return(ierr);
      break;
  }

  return(ierr);
}
      
void correct_velocity(float *u, float *v, float *w, 
                      float h, float p, float r, int n)
{
 
  int i,j,k;
  int   mask[3];
  float R[3][3], vel[3], sum[3];

  
  h *= M_PI/180.0;
  p *= M_PI/180.0;
  r *= M_PI/180.0;

  /* Build the rotation matrix */
  R[0][0] = cos(h)*cos(r)+sin(h)*sin(p)*sin(r);
  R[0][1] = cos(p)*sin(h);
  R[0][2] = cos(h)*sin(r) - cos(r)*sin(h)*sin(p);
  R[1][0] = cos(h)*sin(p)*sin(r) - cos(r)*sin(h);
  R[1][1] = cos(h)*cos(p);
  R[1][2] = -cos(h)*cos(r)*sin(p) - sin(h)*sin(r);
  R[2][0] = -cos(p)*sin(r);
  R[2][1] = sin(p);
  R[2][2] = cos(p)*cos(r);
  
  /* Loop through velocities */
  for(k=0; k<n; k++){
    vel[0] = u[k];
    vel[1] = v[k];
    vel[2] = (w == NULL) ? 0.0 : w[k]; 
    for(i=0; i<3; i++)
      mask[i] = (vel[i] < ADJ_BADFLOAT);
    for(i=0; i<3; i++){
      sum[i] = 0.0;
      for(j=0; j<3; j++){
        sum[i] += R[i][j] * vel[j];
      }
    }
    for(i=0; i<3; i++)
      vel[i] = (mask[i]) ? BADFLOAT : sum[i];
    u[k] = vel[0];
    v[k] = vel[1];
    if(w != NULL) w[k] = vel[2];
  }

  
  

  return;
}

void do_it(FILE *fpcnt)
{

  YMDHMS_TIME_TYPE  time;
  ATTITUDE_TYPE     att;
  ANCILLARY_1_TYPE  *anc1=NULL;
  ANCILLARY_2_TYPE  *anc2=NULL;
  BOTTOM_TRACK_TYPE *bt=NULL;
  
  FILE *fp=NULL;
  float *u=NULL, *v=NULL, *w=NULL;
  float h, p, r;
  int n, ierr=0, dbaccess=READ_WRITE, dbsearch=TIME_SEARCH;
  char buff[LINE_LENGTH];
  double yd;

  init();

	/* Get the parameters */
	if (get_parameters(fpcnt, params, NULL)) exit(-1);
	print_parameters(stdout, params, NULL, "\n");
	printf("\n");
  
	/* Initializations */
  ierr = dbopen_cnf(1, dbname, READ_WRITE, DIR_ON_DISK);
  if(error_found(ierr, "Opening database")) exit(-1);
  fp = fopen(att_file, "r");
  ierr = (fp == NULL) ? UNABLE_TO_OPEN : 0;
  if(error_found(ierr, "Opening ATT file")) goto on_error;
 
  /* Loop through input file */
  while(getline_nc(fp, buff, LINE_LENGTH) != EOF){
    
    /* Parse input line */
    n = sscanf(buff, "%lf, %lf, %lf %lf", &yd, &att.h, &att.p, &att.r);
    ierr = (n != 4) ? READ_ERROR : 0;
    if(error_found(ierr, "Reading next input from ATT file")) goto on_error;

    /* Position database at that profile */
    yd_to_ymdhms_time(yd, year_base, &time);
    ierr = db_search(dbsearch, (char *) &time);
    if(error_found(ierr, "searching profile")) goto on_error;
    
    /* Get current attitudes and velocities from database */
    ierr = get_data(ANCILLARY_1, (void *) anc1);
    if(error_found(ierr, "ancillary_1")) goto on_error;
    ierr = get_data(ANCILLARY_2, (void *) anc2);
    if(error_found(ierr, "ancillary_2")) goto on_error;
    ierr = get_data(BOTTOM_TRACK, (void *) bt);
    if(error_found(ierr, "bottom_track")) goto on_error;
    ierr = get_data_f(U, (void *) &u, &n);
    if(error_found(ierr, "U")) goto on_error;
    ierr = get_data_f(V, (void *) &v, &n);
    if(error_found(ierr, "V")) goto on_error;
    ierr = get_data_f(W, (void *) &w, &n);
    if(error_found(ierr, "W")) goto on_error;
    
    /* Calculate heading */
    h =  anc1->mn_heading;
    p =  anc2->mn_pitch;
    r = -anc2->mn_roll;
    
    /* Rotate back velocities */
    correct_velocity(u, v, w, -h, -p, -r, n);
    correct_velocity(&(bt->u), &(bt->v), &(bt->w), -h, -p, -r, 1);

    /* Correction values */
    h = h - att.h;
    p = att.p;
    r = att.r;
    
    /* Correct velocities */
    correct_velocity(u, v, w, h, p, r, n);
    correct_velocity(&(bt->u), &(bt->v), &(bt->w), h, p, r, 1);
    anc1.mn_heading =  h;
    anc2.mn_pitch   =  p;
    anc2.mn_roll    = -r;
    
    /* Store new values */
    ierr = put_data(U, (void *) u, n);
    if(error_found(ierr, "U")) goto on_error;
    ierr = put_data(V, (void *) v, n);
    if(error_found(ierr, "V")) goto on_error;
    ierr = put_data(W, (void *) w, n);
    if(error_found(ierr, "W")) goto on_error;
    ierr = put_data(BOTTOM_TRACK, (void *) bt, sizeof(BOTTOM_TRACK_TYPE));
    if(error_found(ierr, "BOTTOM_TRACK")) goto on_error;
    ierr = put_data(ANCILLARY_1, (void *) bt, sizeof(ANCILLARY_1_TYPE));
    if(error_found(ierr, "ANCILLARY_1")) goto on_error;
    ierr = put_data(ANCILLARY_2, (void *) bt, sizeof(ANCILLARY_2_TYPE));
    if(error_found(ierr, "ANCILLARY_2")) goto on_error;

  }

  on_error:
  
  DBCLOSE(&ierr);
  DBERROR(&ierr, "DBCLOSE");

  if(fp)   fclose(fp);
  if(anc1) free(anc1);
  if(anc2) free(anc2);
  if(bt)   free(bt);
  if(u)    free(u);
  if(v)    free(v);
  if(w)    free(w);

	if(ierr)
    printf("\n\n\t !!! Function `do_it' ended with error !!!\n\n");
  else
    printf("\n\n\t Function `do_it' ended at EOF \n\n");
	
  return;
}
    
      


