#include <Attributes.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <FloatAttribute.h>
#include <Syslog.h>
#include <float.h>
#include <math.h>

#include "dtFrontMomentum.hh"
#include "SmartSamplerExceptions.hh"
#include "System.h"

#define DEFAULT_DATASTRING "temperature"
#define DEFAULT_KALMANF 1
#define DEFAULT_KALMANH 1
#define DEFAULT_KALMANQ 1
#define DEFAULT_KALMANR 1
#define DEFAULT_MO_THRESH 25
#define DEFAULT_DECEL 6

#define DATA_UPPER_BOUND DBL_MAX

/*********************************************************************************
 **
 ** class dtFrontMomentum  -- Decision tool for choosing to gulp in a front
 **       based on momentum algorithm.
 **
 *********************************************************************************/

// structors :

dtFrontMomentum::dtFrontMomentum(char *nameIn)
  :name(strdup(nameIn)),ssd(NULL),
   prevEst(DATA_UPPER_BOUND),kalmanP(1),
   up_mo(0),down_mo(0),
   yo_max(-DATA_UPPER_BOUND),yo_min(DATA_UPPER_BOUND),
   kalmanF(0),kalmanH(0),kalmanQ(0),kalmanR(0),
   mo_thresh(0),decel(0),
   column(0),d_down(0),d_up(0),u_down(0),u_up(0),
   datatype(SmartSamplerData::ssd_unknown),first_profile(false)
{
  ssdbg(DBG_LOAD)("creating dtFrontMomentum \"%s\"\n",name);
}
dtFrontMomentum::~dtFrontMomentum(){
  ssdbg(DBG_CLEAN)("destroying dtFrontMomentum \"%s\"\n",name);  
  free((void *)name);
}

// Manipulators :

// Parse configuration file for the datatype and threshold.
void dtFrontMomentum::loadConfig(char *cfgname,SmartSamplerData *ssdIn){
  char *datastring;
  Attributes cfg_attributes(cfgname);
  const char *cfgFileName = System::configurationFile( cfgname );
  System::copyToLogDir(cfgFileName);

  ssdbg(DBG_LOAD)("dtFrontMomentum loading configuration file \"%s\"\n",cfgname);  
  ssd = ssdIn;

  cfg_attributes.add(new StringAttribute("dataType", "data type to monitor", 
					&datastring, DEFAULT_DATASTRING));
  cfg_attributes.add(new FloatAttribute("kalmanF", "Kalman filter dynamics", 
					&kalmanF, DEFAULT_KALMANF));
  cfg_attributes.add(new FloatAttribute("kalmanH", "Kalman filter sensor model", 
					&kalmanH, DEFAULT_KALMANH));
  cfg_attributes.add(new FloatAttribute("kalmanQ", "Kalman filter process noise", 
					&kalmanQ, DEFAULT_KALMANQ));
  cfg_attributes.add(new FloatAttribute("kalmanR", "Kalman filter sensor noise", 
					&kalmanR, DEFAULT_KALMANR));
  cfg_attributes.add(new FloatAttribute("mo_thresh", "Momentum function absolute threshold", 
					&mo_thresh, DEFAULT_MO_THRESH));
  cfg_attributes.add(new FloatAttribute("decel", "Momentum function decelleration", 
					&decel, DEFAULT_DECEL));

  // pick up attributes defined at this level:
  AttributeParser::reset();
  try{
      AttributeParser::parse(cfgFileName, &cfg_attributes); 
  }catch(...){
    throw LoadError("Failed to parse dtFrontMomentum attributes.");
    if(datastring != NULL)free((void *)datastring);
    exit(0);
  }

  datatype = ssd->stringToMType(datastring);
  free((void *)datastring);
  if(datatype == SmartSamplerData::ssd_unknown){
    throw LoadError("dtFrontMomentum loaded bad data type!");
    exit(0);
  }
  ssd->useMeasurement(datatype);
  ssd->useProfileCounter();
}

// update the state based on current temperature value, request
// gulper fire if it's over threshold
bool dtFrontMomentum::update(){
  SmartSamplerData::data_t data;
  long cur_pc;
  double kalmanPminus,kalmanK,curDelta,curEst,delta_est;

  if(ssd->getMeasurement(datatype, &data)){
    if(data > yo_max) 
      yo_max = (double)data;
    if(data < yo_min) 
      yo_min = (double)data;
  }

  // getProfileCount returns true only if the count has changed,
  // but we also want to check and make sure this is not the first profile.
  if(ssd->getProfileCount(&cur_pc)){
    if(first_profile){
      // Ignore the first dive to avoid some junky data.
      first_profile = false;
      return(false);
    }
    ssdbg(DBG_GEN)("FrontMomentum -- new profile (count = %d).  Updating state.",cur_pc);
    curDelta = yo_max - yo_min;
    
    // End of first column; estimate is value; no momentum
    if(curDelta <= 0)
      Syslog::write("SmartSampler -- FrontMomentum error:  Bad max/min!  Ignoring this profile.");
    else if(prevEst == DATA_UPPER_BOUND)
      prevEst = curDelta;
    else {
      // Kalman filter
      kalmanPminus = (kalmanF * kalmanP * kalmanF) + kalmanQ;
      kalmanK = kalmanPminus * kalmanH / (kalmanH * kalmanPminus * kalmanH + kalmanR);
      curEst = (kalmanF * prevEst) + kalmanK * (curDelta - kalmanH * kalmanF * prevEst);
      kalmanP = (1 - kalmanK * kalmanH) * kalmanPminus;
      
      // Momentum calculations
      delta_est = curEst - prevEst;
      if(delta_est < 0) {  // Downward step
	d_down++;
	down_mo = down_mo + (fabs(delta_est) * d_down);
	u_down++; 
	up_mo = up_mo - (fabs(delta_est) * decel * u_down);
	if(up_mo < 0) {	// End of previous trend
	  up_mo = 0;	// Momentum can never be less than 0
	  u_down = 0;
	  u_up = 0;
	}
      }
      else {	// Upward step
	d_up++;
	down_mo = down_mo - (delta_est * decel * d_up);
	if(down_mo < 0) {	// End of previous trend
	  down_mo = 0;	// Momentum can never be less than 0
	  d_down = 0;
	  d_up = 0;
	}

	u_up++; 
	up_mo = up_mo + (delta_est * u_up); 
      } 
      //if(down_mo > mo_thresh) {}// Possible front!
      //if(up_mo > mo_thresh) {}// Possible front!
      prevEst = curEst;
    } // end if first column, else update

    // Every new yo, reset max and min for next one.
    yo_max = -DATA_UPPER_BOUND;
    yo_min = DATA_UPPER_BOUND;
    log.callWrite(curDelta,prevEst,down_mo,up_mo);
  }

  return(false);
}

void dtFrontMomentum::reset(){
  ssdbg(DBG_CMD)("dtFrontMomentum resetting state");
  down_mo = 0;
  up_mo = 0;
  prevEst = DATA_UPPER_BOUND;
  d_down = 0;
  d_up = 0;
  u_down = 0;
  u_up = 0;
  yo_max = -DATA_UPPER_BOUND;
  yo_min = DATA_UPPER_BOUND;
}