#include <Attributes.h>
#include <AttributeParser.h>
#include <StringAttribute.h>
#include <FloatAttribute.h>
#include <Syslog.h>
#include "ssdbg.hh"
#include "dtGPOnline.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 dtGPOnline  -- Decision tool for choosing to gulp based on 
 ** prediction of plankton abundance learned using GP regression and online auctions 
 *********************************************************************************/

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

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


long double dtGPOnline::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 dtGPOnline::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 dtGPOnline::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]);
    }
    
}

double dtGPOnline::meanOfVector(double resultHistory_[])
{
    double mean = 0;
    for(int i=0;i<6;i++){
        mean = mean + resultHistory_[i];
    }
    return mean/6.0;
}

double dtGPOnline::envSpaceDistance(long double x[], long double z[]) {
    double dist_ =  ((x[0]-z[0])/tempRange)*((x[0]-z[0])/tempRange) + ((x[1]-z[1])/salRange)*((x[1]-z[1])/salRange) +  ((x[2]-z[2])/chlRange)*((x[2]-z[2])/chlRange);
    return dist_;
}

bool dtGPOnline::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 dtGPOnline::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)("dtGPOnline -- 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));
    
    cfg_attributes.add(new FloatAttribute("totalTicks", "totalTicks",
                                          &totalTicks, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("numberOfWindows", "numberOfWindows", &numberOfWindows, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("noGulpers", "noGulpers", &noGulpers, DEFAULT_THRESHOLD));
    

    cfg_attributes.add(new FloatAttribute("resultFilterAllowance", "resultFilterAllowance", &resultFilterAllowance, DEFAULT_THRESHOLD));
    cfg_attributes.add(new FloatAttribute("windowDivisor", "windowDivisor", &windowDivisor, DEFAULT_THRESHOLD));
    
    
    
    ssdbg(DBG_LOAD)("dtGPOnline -- Parsing attributes.");
    // pick up attributes defined at this level:
    AttributeParser::reset();
    try{
        AttributeParser::parse(cfgFileName, &cfg_attributes); 
    }catch(...){
        throw LoadError("Failed to parse dtGPOnline 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();
    gulpWindowLength = floor(totalTicks/double(numberOfWindows));
    gulpsPerWindow = noGulpers/numberOfWindows;
    eWINDOW_TICKS_LENGTH = floor(gulpWindowLength/windowDivisor);
    
    ssdbg(DBG_CLEAN)("totalTicks =%f numberOfWindows =%f gulpWindowLength=%f gulpsPerWindow=%f eWINDOW_TICKS_LENGTH=%f noGulpers=%f",totalTicks,numberOfWindows,gulpWindowLength,gulpsPerWindow,eWINDOW_TICKS_LENGTH,noGulpers);
    
    
    
    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.setValue(1,(int)tickCounter);
//    log.setValue(2,(double)depth);
//    log.setValue(3,(double)testVector[0]);
//    log.setValue(4,(double)testVector[1]);
//    log.setValue(5,(double)maxScore);
//    log.setValue(6,(double)result);
//    log.setValue(7,(double)gulpCount);
//    log.setValue(8,(int)fired);

    
    log.addElement((int)1,"tick");
    log.addElement((double)2,"dpth");
    log.addElement((double)3,"temp");
    log.addElement((double)4,"chl");
    log.addElement((double)5,"mxsr");
    log.addElement((double)6,"rsl");
    log.addElement((double)7,"gcnt");
    log.addElement((int)8,"fire");
    
    
    
    reset();
    
    for(di_t di=cfg.begin();di!=cfg.end();di++){
        char datastr[16];
        ssd->mTypeToString((*di).type,datastr);
        Syslog::write("SmartSampler GPOnline -- 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 dtGPOnline::update(){
    bool fire = false;
    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++;
    }
    
//    minTemp=10.2200;
//    rangeTemp=4.4800;
//    minChl=0.0001;
//    ranheChl=0.0050;

    
    long double testVector[NUM_TRAIN_DIM]; 
    testVector[0]  = (sensorReadings[0]-10.22)/4.48;
    testVector[1]  = (sensorReadings[4]-0.000107)/0.005031;
    
    double depth = sensorReadings[6];
    bool isDepthOK = depth > depth_thresh;
    
    if(isDepthOK){
        tickCounter++;
    }
  
    
    
    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]);
    }
    
         
    bool shouldConsiderResult = (tickCounter> 6);
    
    double meanResult = 0;
    if(shouldConsiderResult){
        meanResult = meanOfVector(resultHistory);
    }
    
    if(tickCounter < 7){
        resultHistory[tickCounter-1] = result;
    }else{
        for(int avCtr = 0;avCtr < 5;avCtr++){
            resultHistory[avCtr] = resultHistory[avCtr+1];
        }
        resultHistory[5] = result;
    }
    
     
    ssdbg(DBG_CLEAN)("%f",noGulpers);

    
    bool isGulperRemaining = gulpCount < noGulpers;
//   bool isSensorReadingWithinRange = sensorReadingsWithinRange(testVector) && (fabs(result-meanResult) < resultFilterAllowance);
   bool isSensorReadingWithinRange = (fabs(result-meanResult) < resultFilterAllowance);
    //bool isSensorReadingWithinRange = true;
    ssdbg(DBG_CLEAN)("Data: %f\t%f\t%ld\t%f\t%f\t%d\t%d\t%4.8f\t%4.8f\t%s\t%s\t%s",depth,noGulpers,tickCounter,gulpWindowLength,eWINDOW_TICKS_LENGTH,windowCtr,gulpCount,maxScore,result,BOOL_STR(haveGulpsForWindow),BOOL_STR(isGulperRemaining),BOOL_STR(isDepthOK));
    

    
    
    
    if(windowCtr < eWINDOW_TICKS_LENGTH){
        if(isSensorReadingWithinRange && isDepthOK && (result > maxScore)){
            maxScore = result;
            okToGulp = false;
            statsSet = false;
        }
    }else{
        okToGulp = true;
        if(!statsSet){
            distCutoff = ((tempStd/tempRange)*(tempStd/tempRange)) + ((salStd/salRange)*(salStd/salRange)) + ((chlStd/chlRange)*(chlStd/chlRange));
            statsSet = true;
            eWindowPreviousInd = tickCounter;
        }
    }
    
    if(gulpInWindowCtr < gulpsPerWindow){
        haveGulpsForWindow = true;
    } else {
        haveGulpsForWindow = false;
    }
    
    if(windowCtr  <= gulpWindowLength){
        if(isDepthOK){
        windowCtr++;
        }
    } else
    {
        windowCtr = 0;
        maxScore = 0;
        if(haveGulpsForWindow && isGulperRemaining && isDepthOK){
            fire = true;
            for(int ll = 0;ll<NUM_TRAIN_DIM;ll++){
                gulpsTakenData[gulpCount][ll] = testVector[ll];
            }
            gulpCount++;
        }
        gulpInWindowCtr = 0;
        stdScaling = stdScalingDefault;
        haveGulpsForWindow = false;
        
    }
    long double dist = maxDistanceWithExistingDataPoints(testVector);
    bool isThresholdOK = result > maxScore;
    bool isDistOK = dist > distCutoff;
    if(haveGulpsForWindow && isThresholdOK && isDepthOK && isGulperRemaining && okToGulp && isSensorReadingWithinRange){
        ssdbg(DBG_CLEAN)("GULPING: %d\t%f\t%f\t%4.8f\t%4.8f\t%4.8f\t1",tickCounter,gulpsPerWindow,gulpInWindowCtr,maxScore,result,dist);
        fire = true;
        for(int ll = 0;ll<NUM_TRAIN_DIM;ll++){
            gulpsTakenData[gulpCount][ll] = testVector[ll];
            gulpsTakenInWindowData[gulpInWindowCtr][ll] = testVector[ll];
        }
        gulpCount++;
        gulpInWindowCtr++;
    }
    int fired = 0;
    
    if(fire){fired=1;}
    log.setValue(1,(int)tickCounter);
    log.setValue(2,(double)depth);
    log.setValue(3,(double)sensorReadings[0]);
    log.setValue(4,(double)sensorReadings[4]);
    log.setValue(5,(double)maxScore);
    log.setValue(6,(double)result);
    log.setValue(7,(double)gulpCount);
    log.setValue(8,(int)fired);
    
    
    log.callWrite();
    return(fire);

}

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

long double dtGPOnline::maxDistanceWithExistingDataPoints(long double testVector[]){
    long double K[10];
    long double minDistance = 9999.0;
    for(int i=0;i<gulpInWindowCtr;i++){
        long double _sq_dist = envSpaceDistance(testVector,gulpsTakenInWindowData[i]);
        if(_sq_dist < minDistance){
            minDistance = _sq_dist;
        }
    }
    return minDistance;
}


std::string dtGPOnline::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;
}