#include "SmartSamplerExceptions.hh"
#include "SmartSamplerData.hh"
#include "Syslog.h"
#include "ssdbg.hh"

/****************************************************************************
 *
 * SmartSamplerData methods
 *
 ****************************************************************************/

SmartSamplerData::SmartSamplerData(SmartSamplerInterfaces *ssiIn)
  :ssi(ssiIn),initdone(false),ssdlogger(this),
#ifdef PROFILE_FROM_DEPTH
   nextdepthidx(0),depthbuffull(false),profilecountnew(false),descending(true),
#endif
   profilecount(0){
  ssdbg(DBG_LOAD)("creating SmartSamplerData");
  initMStructs();
}
SmartSamplerData::~SmartSamplerData(){
  ssici_t ci;
  ssdbg(DBG_ATTACH)("SmartSampler -- destroying SmartSamplerData");
  for(ci=clusters.begin();ci!=clusters.end();ci++){
    free((void *)(*ci).classifier);
  }
}

// This is all incredibly stupid, should be dynamically allocated.
// Just need to get something working for now.
void SmartSamplerData::initMStructs(){
  temperature.track = false;
  conductivity.track = false;
  salinity.track = false;
  oxygen.track = false;
  bbshort.track = false;
  bblong.track = false;
  fluor.track = false;
  nitrate.track = false;
  depth.track = false;

  temperature.isnew = false;
  conductivity.isnew = false;
  salinity.isnew = false;
  oxygen.isnew = false;
  bbshort.isnew = false;
  bblong.isnew = false;
  fluor.isnew = false;
  nitrate.isnew = false;
  depth.isnew = false;
}

bool SmartSamplerData::useMeasurement(ssd_measurement_t id){
  bool isok=false;
  if(initdone){
    throw LoadError("useMeasurement called after initialization!");
    exit(0);
  }
  // First we attach the required device
  switch(id){
  case ssd_temp:
  case ssd_cond:
  case ssd_sal:
  case ssd_oxy:
    isok = ssi->attachCTD();
    break;
  case ssd_bbshort:
  case ssd_bblong:
  case ssd_fluor:
    isok = ssi->attachHS2();
    break;
  case ssd_nitrate:
    isok = ssi->attachIsus();
    break;
  case ssd_depth:
    isok = ssi->attachDepthSensor();
    break;
  default:
    Syslog::write("SmartSampler -- Warning!  Unknown measurement type %d requested by DecisionTool!",id);
  }
  // Next we set the track bit
  switch(id){
  case ssd_temp:temperature.track=true;break;
  case ssd_cond:conductivity.track=true;break;
  case ssd_sal:salinity.track=true;break;
  case ssd_oxy:oxygen.track=true;break;
  case ssd_bbshort:bbshort.track=true;break;
  case ssd_bblong:bblong.track=true;break;
  case ssd_fluor:fluor.track=true;break;
  case ssd_nitrate:nitrate.track=true;break;
  case ssd_depth:depth.track=true;break;
  default:
    Syslog::write("SmartSampler -- Warning!  Unknown measurement type %d requested by DecisionTool!",id);
    //    Syslog::write("Options are %d,%d,%d,%d,%d,%d,%d,%d",ssd_temp,ssd_sal,ssd_cond,ssd_bbshort,ssd_bblong,
    //		  ssd_fluor,ssd_nitrate,ssd_depth);
  }

  return(isok);
}

bool SmartSamplerData::useCluster(char *classifier){
  ssi_cluster_t cl;
  if(initdone){
    throw LoadError("useCluster called after initialization!");
    exit(0);
  }
  bool isok = ssi->attachCluster();
  if(isok && (clusters.end() == findCluster(classifier))){
    ssdbg(DBG_ATTACH)("SSD -- Adding new classifier \"%s\" to cluster vector!",classifier);
    cl.isnew = false;
    cl.best = 0;
    cl.dist = 0;
    cl.classifier = strdup(classifier);
    clusters.push_back(cl);
  }
  return(isok);
}

bool SmartSamplerData::useProfileCounter(){
  if(initdone){
    throw LoadError("useProfileCounter called after initialization!");
    exit(0);
  }
#ifdef PROFILE_FROM_DEPTH
  return(useMeasurement(ssd_depth));
#else
  return(ssi->attachProfileCounter());
#endif
}

void SmartSamplerData::finishInit(){
  // When all configuration files are loaded, we tie off some loose ends.
  // This also ensures that we do no extra dynamic memory allocation.
  ssdlogger.initializeData();
  initdone = true;
}

bool SmartSamplerData::getMeasurement(ssd_measurement_t id, data_t *val){
  bool tracking = true;
  bool isnew = false;
  switch(id){
  case ssd_temp:
    *val = (data_t)temperature.val;
    tracking = temperature.track;
    isnew = temperature.isnew;
    break;
  case ssd_cond:
    *val = (data_t)conductivity.val;
    tracking = conductivity.track;
    isnew = conductivity.isnew;
    break;
  case ssd_sal:
    *val = (data_t)salinity.val;
    tracking = salinity.track;
    isnew = salinity.isnew;
    break;
  case ssd_oxy:
    *val = (data_t)oxygen.val;
    tracking = oxygen.track;
    isnew = oxygen.isnew;
    break;
  case ssd_bbshort:
    *val = (data_t)bbshort.val;
    tracking = bbshort.track;
    isnew = bbshort.isnew;
    break;
  case ssd_bblong:
    *val = (data_t)bblong.val;
    tracking = bblong.track;
    isnew = bblong.isnew;
    break;
  case ssd_fluor:
    *val = (data_t)fluor.val;
    tracking = fluor.track;
    isnew = fluor.isnew;
    break;
  case ssd_nitrate:
    *val = (data_t)nitrate.val;
    tracking = nitrate.track;
    isnew = nitrate.isnew;
    break;
  case ssd_depth:
    *val = (data_t)depth.val;
    tracking = depth.track;
    isnew = depth.isnew;
    break;
  default:
    Syslog::write("SmartSampler -- Warning!  Unknown measurement type (%d) requested!",id);
  }
  if(!tracking){
    Syslog::write("SmartSampler -- Warning!  Measurement data (type %d) requested but not tracked!",id);
  }
  return(isnew);
}

bool SmartSamplerData::getCluster(char *classifier, unsigned short *best, double *dist){
  ssici_t cli = findCluster(classifier);
  bool isnew;
  if(cli == clusters.end()){
    Syslog::write("SmartSampler -- Warning!  Cluster data (classifier \"%s\") requested but not tracked!",
		  classifier);
    isnew = false;
  }else{
    isnew = (*best != (*cli).best) || (*dist != (*cli).dist);
    *best = (*cli).best;
    *dist = (*cli).dist;
  }
  return(isnew);
}

bool SmartSamplerData::getProfileCount(long *cnt){
  *cnt = profilecount;
  return(profilecountnew);
}

void SmartSamplerData::update(){
  ssici_t cli;
  if(!initdone){
    throw LoadError("SmartSamplerData::update called during initialization!");
    exit(0);
  }
  ssdbg(DBG_ATTACH)("SSD -- Updating data");
  ssi->updateCTD(&temperature,&conductivity,&salinity,&oxygen);
  ssi->updateHS2(&bbshort,&bblong,&fluor);
  ssi->updateIsus(&nitrate);
  ssi->updateDepth(&depth);
  for(cli=clusters.begin();cli!=clusters.end();cli++){
    ssi->updateCluster(&(*cli));
  }

  profilecountnew = false;
#ifdef PROFILE_FROM_DEPTH
  // Here's the meat of the depth-based profile counter.  The constant
  // terms here embed a least squares quadratic fit to the last 11
  // depth values (5 seconds) with time t-offset counting backwards
  // like 0:.5:5.  Since the current instant is t=0 in that frame, the
  // derivative of the curve here is just the linear term.
  double grad = 0;
  double thisdepth = 0;
  // We only bother to check if we've passed 10 meter depth in either 
  // direction.  First condition here checks to make sure we can get a valid
  // depth value for this instant.
  if((depth.isnew || depthbuffull || (nextdepthidx != 0)) &&
     ((descending && (depth.val > 10)) ||
      (!descending && (depth.val < 10)))){
    if(depth.isnew)
      thisdepth = depth.val;
    else{
      ssdbg(DBG_GEN)("Warning! Data not new!");
      if(nextdepthidx == 0)
	thisdepth = depthbuf[11];
      else
	thisdepth = depthbuf[nextdepthidx-1];
    }
    //    ssdbg(DBG_GEN)("Adding %.04f to depthbuffer location %d",thisdepth,nextdepthidx);
    depthbuf[nextdepthidx] = thisdepth;
    if((nextdepthidx=(nextdepthidx+1)%11)==0){
      depthbuffull = true;
    }
    if(nextdepthidx > 11){
      Syslog::write("SmartSampler -- ERROR!!! Profile counter buffer index gone haywire!!!");
      nextdepthidx = 0;
      depthbuffull = false; 
    }else if(depthbuffull){
      int cur = nextdepthidx;
      // Here we have the hardcoded coefficients.  This calculates the second term
      // of pinv(F*F')*F*depth, where F is the polynomial basis vector in 0:.5:5.
      grad = grad + -0.25874126*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + -0.06713287*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.07785548*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.17622378*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.22797203*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.23310023*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.19160839*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + 0.10349650*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + -0.03123543*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + -0.21258741*depthbuf[cur];cur=(cur+1)%11;
      grad = grad + -0.44055944*depthbuf[cur];
      //      ssdbg(DBG_GEN)("Profile gradient = %.02f",grad);
      // sanity check (should end on the current location.
      if(depthbuf[cur] != depth.val){
	Syslog::write("SmartSampler -- ERROR!!! Profile counter multiply gone haywire!!!");
	nextdepthidx = 0;
	depthbuffull = false;
	// NOTE: the lag between peaks and my counter can be improved by
	// changing this to grad >= -.1 and grad <= .1, but that runs into
	// some problems for some of the runs (specifically June 2011, where
	// the pitch angle was not so steady).
      }else if((descending && (grad >= 0)) || (!descending && (grad <= 0))){
	descending = !descending;
	profilecount = profilecount + 1;
	profilecountnew = true;
	ssdbg(DBG_GEN)("NEW PROFILE COUNT:\t\t\t %d at depth %.02f",profilecount,thisdepth);
      }
    }
  }else{
    nextdepthidx = 0;
    depthbuffull = false;
  }

#else // PROFILE_FROM_DEPTH not defined
  long tmpcount;
  ssi->updateProfileCount(&tmpcount);
  if(tmpcount != profilecount){
    profilecountnew = true;
    profilecount = tmpcount;
  }
#endif

  ssdlogger.write();
}

SmartSamplerData::ssd_measurement_t SmartSamplerData::stringToMType(char *stringIn){
  if(!strncmp("temp",stringIn,4) || !strncmp("Temp",stringIn,4) || !strncmp("TEMP",stringIn,4)){
    return(ssd_temp);
  }
  if(!strncmp("sal",stringIn,3) || !strncmp("Sal",stringIn,3) || !strncmp("SAL",stringIn,3)){
    return(ssd_sal);
  }
  if(!strncmp("oxy",stringIn,3) || !strncmp("Oxy",stringIn,3) || !strncmp("OXY",stringIn,3)){
    return(ssd_oxy);
  }
  if(!strncmp("cond",stringIn,4) || !strncmp("Cond",stringIn,4) || !strncmp("COND",stringIn,4)){
    return(ssd_cond);
  }
  if((!strncmp("bbsh",stringIn,4)) || (!strncmp("bb4",stringIn,3)) ||
     (!strncmp("BBSH",stringIn,4)) || (!strncmp("BB4",stringIn,3))){
    return(ssd_bbshort);
  }
  if((!strncmp("bblo",stringIn,4)) || (!strncmp("bb7",stringIn,3)) || (!strncmp("bb6",stringIn,3)) ||
     (!strncmp("BBLO",stringIn,4)) || (!strncmp("BB7",stringIn,3)) || (!strncmp("BB6",stringIn,3))){
    return(ssd_bblong);
  }
  if(!strncmp("fluor",stringIn,5) || !strncmp("Fluor",stringIn,5) || !strncmp("FLUOR",stringIn,5)){
    return(ssd_fluor);
  }
  if(!strncmp("nitrate",stringIn,4) || !strncmp("Nitrate",stringIn,4) || !strncmp("NITRATE",stringIn,4)){
    return(ssd_nitrate);
  }
  if(!strncmp("depth",stringIn,4) || !strncmp("Depth",stringIn,4) || !strncmp("DEPTH",stringIn,4)){
    return(ssd_depth);
  }
  Syslog::write("SmartSamplerData -- stringToMType called with unknown data type \"%d\"",stringIn);
  return(ssd_unknown);
}

void SmartSamplerData::mTypeToString(SmartSamplerData::ssd_measurement_t id, char *stringOut){
  switch(id){
  case ssd_temp:strcpy(stringOut,"temperature");break;
  case ssd_sal:strcpy(stringOut,"salinity");break;
  case ssd_oxy:strcpy(stringOut,"oxygen");break;
  case ssd_cond:strcpy(stringOut,"conductivity");break;
  case ssd_bbshort:strcpy(stringOut,"bbshort");break;
  case ssd_bblong:strcpy(stringOut,"bblong");break;
  case ssd_fluor:strcpy(stringOut,"fluorescence");break;
  case ssd_nitrate:strcpy(stringOut,"nitrate");break;
  case ssd_depth:strcpy(stringOut,"depth");break;
  default:strcpy(stringOut,"unknown");break;
  }
}

// Internals
SmartSamplerData::ssici_t SmartSamplerData::findCluster(char *classifier){
  ssici_t cli;
  ssdbg(DBG_ATTACH)("SSD -- Looking for classifier \"%s\"",classifier);
  for(cli=clusters.begin();cli!=clusters.end();cli++){
    if(!strcmp((*cli).classifier,classifier)){
      ssdbg(DBG_ATTACH)("SSD -- Found it!",classifier);
      break;
    }else{
      ssdbg(DBG_ATTACH)("SSD -- Rejecting classifier \"%s\"",classifier);
    }
  }
  return(cli);
}

// Just a little routine to tell us if any of our tracked data is new.
bool SmartSamplerData::anyNew(){
  ssici_t cli;
  for(cli=clusters.begin();cli!=clusters.end();cli++){
    if((*cli).isnew)return(true);
  }
  return(temperature.isnew ||
	 conductivity.isnew ||
	 salinity.isnew ||
	 oxygen.isnew ||
	 bbshort.isnew ||
	 bblong.isnew ||
	 fluor.isnew ||
	 nitrate.isnew ||
	 depth.isnew);
}