#include <Attributes.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <FloatAttribute.h>
#include <Syslog.h>
#include "ssdbg.hh"
#include "dtGPThreshold.hh"
#include "SmartSamplerExceptions.hh"
#include "System.h"
#include <vector>
#include <iostream>
#include <math.h>
#include <fstream>
#include <stdlib.h>

#define DEFAULT_THRESHOLD -1
#define DEFAULT_DATASTRING "dummy"
#define BOOL_STR(b) ((b)?"1":"0")
#define AUGMENTED_DIMS  1
/*********************************************************************************
 ** Jnaneshwar Das, University of Southern California jnaneshd@usc.edu
 ** class dtGPThreshold  -- Decision tool for choosing to gulp based on 
 ** prediction of plankton abundance learned using GP regression
 *********************************************************************************/

// structors :
dtGPThreshold::dtGPThreshold(char *nameIn)
:name(strdup(nameIn)),ssd(NULL),log(name)
{
    ssdbg(DBG_CLEAN)("\nNAME IN: %s\n\n",name);
    gulpCount = 0;
    optimumCorrThreshold = 9999;
    tickCounter = 0;
    tickLastFired = 0;
    ssdbg(DBG_LOAD)("creating dtGPThreshold \"%s\"",name);
}

dtGPThreshold::~dtGPThreshold(){
    ssdbg(DBG_CLEAN)("destroying dtGPThreshold \"%s\"",name);  
    
    free((void *)name);
}


long double dtGPThreshold::sq_dist(long double vec1[], long double vec2[],int length){
    long double sum = 0;
    for(int i=0;i<length;i++){
        sum = sum + pow(((vec1[i]-vec2[i])/ell),2.0);
    }
    return sum;
}


long double dtGPThreshold::minCorrelationWithExistingDataPoints(long double testVector[]){
    long double K[10];
    long double minCorr = -9999.0;
    for(int i=0;i<gulpCount;i++){
        long double _sq_dist = sq_dist(testVector,gulpsTakenData[i],NUM_TRAIN_DIM+AUGMENTED_DIMS);
        K[i] = sf2*exp(-_sq_dist/2.0); // initially, was divided by 2. 
        if(K[i] > minCorr){
            minCorr = K[i];
        }
    }
    return minCorr;
}


void dtGPThreshold::loadDataFiles(){
    ifstream fin;
    ifstream fin_alpha;
    ifstream fin_meanSensor;
    ifstream fin_stdevSensor;
    
    // load trainingMatrix
    fin.open(trainMatrixFilenameFullpath);
    if (! fin.fail()) {
        char line_s[2000];         
        int row = 0;
        while (! fin.eof()) {
            fin >> line_s;
            string line(line_s);
            if(line.length()) {
                for (size_t col = 0; col < NUM_TRAIN_DIM; col++) {
                    long double val;
                    //trainVector[row][col] = atof(popToken(line, ",").c_str());
                    trainVector[row][col] = strtod(popToken(line, ",").c_str(),NULL);
                    
                }
                row++;
            }
        }
        fin.close();
    }
    
    
    // load alphaVector
    fin_alpha.open(alphaVectorFilenameFullpath);
    if (! fin_alpha.fail()) {
        char line_s[2000]; 
        int row = 0;
        while (! fin_alpha.eof()) {
            fin_alpha >> line_s;
            string line(line_s);
            if(line.length()) {
                double val;
                alphaVector[row] = atof(popToken(line, ",").c_str());
                row++;
            }
        }
        fin_alpha.close();
    }
    
    // mean of sensor readings
    fin_meanSensor.open(meanSensorFilenameFullpath);
    if (! fin_meanSensor.fail()) {
        char line_s[2000]; 
        int row = 0;
        while (! fin_meanSensor.eof()) {
            fin_meanSensor >> line_s;
            string line(line_s);
            if(line.length()) {
                double val;
                meanSensorVector[row] = atof(popToken(line, ",").c_str());
                row++;
            }
        }
        fin_meanSensor.close();
    }
    
    // load standard deviation of sensor readings
    fin_stdevSensor.open(stdSensorFilenameFullpath);
    if (! fin_stdevSensor.fail()) {
        char line_s[2000]; 
        int row = 0;
        while (! fin_stdevSensor.eof()) {
            fin_stdevSensor >> line_s;
            string line(line_s);
            if(line.length()) {
                double val;
                stdevSensorVector[row] = atof(popToken(line, ",").c_str());
                row++;
            }
        }
        fin_stdevSensor.close();
    }
    
    for(int kk = 0;kk <NUM_TRAIN_DIM;kk++){
        ssdbg(DBG_CLEAN)("mean: %f std: %f\n",meanSensorVector[kk],stdevSensorVector[kk]);
    }
    
}

bool dtGPThreshold::sensorReadingsWithinRange(long double sensorReadingVector[])
{   
    bool result=true;
    for(int i = 0;i < NUM_TRAIN_DIM;i++){
        double mean = meanSensorVector[i];
        double threeSigma = spikeFilterSigma*stdevSensorVector[i];
        bool isSensorReadingWithinRange = ((sensorReadingVector[i] < mean+threeSigma) && (sensorReadingVector[i] > mean-threeSigma));
        if(!isSensorReadingWithinRange){
            ssdbg(DBG_CLEAN)("sensor spiky  %d : reading%f ",i,sensorReadingVector[i]);    
        }
        result = result & isSensorReadingWithinRange;
    }
    return result;
}

// Manipulators :

// Parse configuration file for the datatype and threshold.
void dtGPThreshold::loadConfig(char *cfgname,SmartSamplerData *ssdIn){
    STData_t info;
    Attributes cfg_attributes(cfgname);
    const char *cfgFileName; 
    char *trainMatrixFilename;
    char *alphaVectorFilename;
    
    char *meanSensorFilename;
    char *stdSensorFilename;
    
    cfgFileName = System::configurationFile( cfgname );
    ssdbg(DBG_LOAD)("dtGPThreshold -- Loading config file at \"%s\"",cfgFileName);
    System::copyToLogDir(cfgFileName);
    
    ssd = ssdIn;
    
    cfg_attributes.add(new FloatAttribute("ell", "kernel parameter for covSEiso, length scale", 
                                          &ell, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("sf2", "kernel parameter for covSEiso, scaling param", 
                                          &sf2, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("org_abun_thresh_high", "kernel parameter for covSEiso, scaling param", 
                                          &org_abun_thresh_high, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("org_abun_thresh_low", "kernel parameter for covSEiso, scaling param", 
                                          &org_abun_thresh_low, DEFAULT_THRESHOLD));
    cfg_attributes.add(new StringAttribute("trainMatrixFilename", "trainMatrixFilename", 
                                           &trainMatrixFilename, DEFAULT_DATASTRING));
    cfg_attributes.add(new StringAttribute("alphaVectorFilename", "alphaVectorFilename", 
                                           &alphaVectorFilename, DEFAULT_DATASTRING));
    cfg_attributes.add(new FloatAttribute("depth_thresh", "depth threshold", 
                                          &depth_thresh, DEFAULT_THRESHOLD));
    cfg_attributes.add(new StringAttribute("meanSensorFilename", "meanSensorFilename", 
                                           &meanSensorFilename, DEFAULT_DATASTRING));
    cfg_attributes.add(new StringAttribute("stdSensorFilename", "stdSensorFilename", 
                                           &stdSensorFilename, DEFAULT_DATASTRING));
    cfg_attributes.add(new FloatAttribute("aug_vector_y_scaling", "aug_vector_y_scaling", 
                                          &aug_vector_y_scaling, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("skipCorrFlag", "skipCorrFlag", 
                                          &skipCorrFlag, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("org_abun_thresh_high_override", "org_abun_thresh_high_override", 
                                          &org_abun_thresh_high_override, DEFAULT_THRESHOLD));
    
    cfg_attributes.add(new FloatAttribute("spikeFilterSigma", "spikeFilterSigma", 
                                          &spikeFilterSigma, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("correlationThreshold", "correlationThreshold", 
                                          &correlationThreshold, DEFAULT_THRESHOLD));
    
    cfg_attributes.add(new FloatAttribute("tickFireThreshold", "tickFireThreshold", 
                                          &tickFireThreshold, DEFAULT_THRESHOLD));
    
    cfg_attributes.add(new FloatAttribute("minTickWaitTimeThreshold", "minTickWaitTimeThreshold", 
                                          &minTickWaitTimeThreshold, DEFAULT_THRESHOLD));
    
    
    ssdbg(DBG_LOAD)("dtGPThreshold -- Parsing attributes.");
    // pick up attributes defined at this level:
    AttributeParser::reset();
    try{
        AttributeParser::parse(cfgFileName, &cfg_attributes); 
    }catch(...){
        throw LoadError("Failed to parse dtGPThreshold attributes.");
        exit(0);
    }
    
    
    char auvDirName[512];
    
    
    char *auvDirectory = getenv(AuvDirNameEnv);
    
    if (auvDirectory == 0) {
        strcpy(auvDirName,"/home/dorado1/auv-qnx/auv/altex/onboard/");
    }else{
        strcpy(auvDirName,auvDirectory);
    }
    
    
    
    strcat(auvDirName,"/smartSampler/");
    
    strcat(trainMatrixFilenameFullpath,auvDirName);
    strcat(trainMatrixFilenameFullpath,trainMatrixFilename); 
    
    strcat(alphaVectorFilenameFullpath,auvDirName);
    strcat(alphaVectorFilenameFullpath,alphaVectorFilename);
    
    strcat(meanSensorFilenameFullpath,auvDirName);
    strcat(meanSensorFilenameFullpath,meanSensorFilename);
    
    strcat(stdSensorFilenameFullpath,auvDirName);
    strcat(stdSensorFilenameFullpath,stdSensorFilename);
    
    System::copyToLogDir(trainMatrixFilenameFullpath);
    System::copyToLogDir(alphaVectorFilenameFullpath);
    System::copyToLogDir(meanSensorFilenameFullpath);
    System::copyToLogDir(stdSensorFilenameFullpath);
    
    
    loadDataFiles();
    
    ssdbg(DBG_LOAD)("GP kernel papram for distance computation %f %f",ell,sf2);
    
    info.type = SmartSamplerData::ssd_temp;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    
    
    info.type = SmartSamplerData::ssd_sal;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    info.type = SmartSamplerData::ssd_oxy;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    info.type = SmartSamplerData::ssd_nitrate;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    info.type = SmartSamplerData::ssd_fluor;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    info.type = SmartSamplerData::ssd_bbshort;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);
    
    info.type = SmartSamplerData::ssd_depth;
    ssd->useMeasurement(info.type);
    cfg.push_back(info);

    
    log.addElement((int)1,"tick");
    log.addElement((int)2,"corr");
    log.addElement((int)3,"abnd");
    log.addElement((int)4,"flgstr");
    
    reset();
    
    for(di_t di=cfg.begin();di!=cfg.end();di++){
        char datastr[16];
        ssd->mTypeToString((*di).type,datastr);
        Syslog::write("SmartSampler GPThreshold -- Tracking data type %s with initial threshold %f",datastr,(*di).thresh);
    }
}

// update the state based on current values, request
// gulper fire if any thresholds exceeded
bool dtGPThreshold::update(){
    bool fire = false;
    tickCounter++;
    di_t di;
    
    vector<int> v;
    
    SmartSamplerData::ssd_measurement_t id;
    SmartSamplerData::data_t curval;
    int ctr = 0;
    long double sensorReadings[8];
    for(di=cfg.begin();di!=cfg.end();di++){
        id = (*di).type;
        ssd->getMeasurement(id,&curval);
        sensorReadings[ctr]=curval;
        ctr++;
    }
    
    long double testVector[NUM_TRAIN_DIM]; 
    testVector[0]  = sensorReadings[0];
    testVector[1]  = sensorReadings[1];
    testVector[2]  = sensorReadings[2];
    testVector[3]  = sensorReadings[3];
    testVector[4]  = sensorReadings[4]*10000.0;
    testVector[5]  = sensorReadings[5]*1000.0;
    
    double depth = sensorReadings[6];
    
    //ssdbg(DBG_CLEAN)("%4.8f\t%4.8f\t%4.8f\t%4.8f\t%4.8f\t%4.8f\t%4.8f",testVector[0],testVector[1],testVector[2],testVector[3],testVector[4],testVector[5]);
    
    
    long double K[NUM_TRAIN_DATA];
    for(int i=0;i<NUM_TRAIN_DATA;i++){
        long double _sq_dist = sq_dist(testVector,trainVector[i],NUM_TRAIN_DIM);
        K[i] = sf2*exp(-_sq_dist/2.00);
    }
    
    double result = 0;
    long double augTestVector[NUM_TRAIN_DIM+2];
    for(int j=0;j<NUM_TRAIN_DATA;j++){
        result = result + alphaVector[j]*K[j];
//        ssdbg(DBG_CLEAN)("%4.8f\t%4.16f",alphaVector[j],K[j]);
    }
    
    
    for(int pp=0;pp<NUM_TRAIN_DIM;pp++){
        augTestVector[pp] = testVector[pp];
    }
    augTestVector[NUM_TRAIN_DIM]=result*aug_vector_y_scaling;
    
    
    long double corr = minCorrelationWithExistingDataPoints(augTestVector);
    bool isCorrOK = corr < correlationThreshold;
    
    bool isThresholdOK = result > org_abun_thresh_high || result < org_abun_thresh_low;
    bool isDepthOK = depth > depth_thresh;
    bool isThresholdSuperOK = result > org_abun_thresh_high_override;
    bool isCorrRelatedCheckOK = (isCorrOK || skipCorrFlag==1 || gulpCount == 0 || isThresholdSuperOK);
    double tickSinceLastFired = tickCounter - tickLastFired;
    bool isItAboutTimeToFire = tickSinceLastFired > tickFireThreshold;
    bool isItTooEarlyToFire = tickSinceLastFired < minTickWaitTimeThreshold;
    
    bool isGulperRemaining = gulpCount < 10;
    bool isSensorReadingWithinRange = sensorReadingsWithinRange(testVector);
    
    bool shouldFire = isGulperRemaining && isDepthOK && !isItTooEarlyToFire && (isItAboutTimeToFire || (isThresholdOK && isCorrRelatedCheckOK && isSensorReadingWithinRange));
    
    char flagString[40];
    sprintf(flagString,"%s.%s%s%s%s%s%s%s",BOOL_STR(shouldFire),BOOL_STR(isGulperRemaining),BOOL_STR(isDepthOK),BOOL_STR(isThresholdSuperOK),BOOL_STR(isItAboutTimeToFire),BOOL_STR(isThresholdOK),BOOL_STR(isCorrRelatedCheckOK),BOOL_STR(isSensorReadingWithinRange));
    
    double flagStringInFloat = atof(flagString);
    
    ssdbg(DBG_CLEAN)("%ld\t%4.4f\t%4.8f\t%4.8f\t%4.8f\t%d\t%1.7f",tickCounter, tickSinceLastFired, result, corr,correlationThreshold, gulpCount, flagStringInFloat);
    
    
    
    
    if(shouldFire){
        fire = true;
        tickLastFired = tickCounter;
        
        ssdbg(DBG_CLEAN)("!!!!! FIRED:> result : gulpCount : isThresh : isCorr %f\t%.8f\t%d\t%s\t%s", result, corr, gulpCount, BOOL_STR(isThresholdOK), BOOL_STR(isCorrOK));
        
        for(int ll = 0;ll<NUM_TRAIN_DIM;ll++){
            gulpsTakenData[gulpCount][ll] = testVector[ll];   
         }
        log.setValue(1,(int)tickCounter);
        log.setValue(2,(double)result);
        log.setValue(3,(double)corr);
        log.setValue(4,(float)flagStringInFloat);
        
        gulpsTakenData[gulpCount][NUM_TRAIN_DIM] = result*aug_vector_y_scaling;
        gulpsTakenData[gulpCount][NUM_TRAIN_DIM+1] = tickCounter/(2*60*60);       
        gulpCount++;
    }
    
    
    log.callWrite();
    return(fire);
}

void dtGPThreshold::reset(){
    di_t di;
    for(di=cfg.begin();di!=cfg.end();di++){
        (*di).thresh = (*di).initthresh;
    }
}

std::string dtGPThreshold::popToken(std::string& text, const std::string& delimiter)
{
    std::string result = "";
    std::string::size_type pos = text.find(delimiter);
    if (pos != std::string::npos) {
        result.assign(text.begin(), text.begin() + pos);
        text.erase(text.begin(), text.begin() + pos + delimiter.length());        
    } else {
        text.swap(result);
    }
    return result;
}