//Compute the regression data that will be stored at this node
bool RegressionTree::computeNodeRegressionData( const RegressionData &trainingData, VectorDouble &regressionData ){
    
    const UINT M = trainingData.getNumSamples();
    const UINT N = trainingData.getNumInputDimensions();
    const UINT T = trainingData.getNumTargetDimensions();
    
    if( M == 0 ){
        Regressifier::errorLog << "computeNodeRegressionData(...) - Failed to compute regression data, there are zero training samples!" << endl;
        return false;
    }
    
    //Make sure the regression data is the correct size
    regressionData.clear();
    regressionData.resize( T, 0 );
    
    //The regression data at this node is simply an average over all the training data at this node
    for(unsigned int j=0; j<N; j++){
        for(unsigned int i=0; i<M; i++){
            regressionData[j] += trainingData[i].getTargetVector()[j];
        }
        regressionData[j] /= M;
    }
    
    return true;
}

void  SegmentorOTSU::NdGrid (const VectorDouble&  x,
                             const VectorDouble&  y,
                             Matrix&              xm,
                             Matrix&              ym
                            )
{
  kkuint32  xLen = (kkuint32)x.size ();
  kkuint32  yLen = (kkuint32)y.size ();

  xm.ReSize (xLen, xLen);
  ym.ReSize (yLen, yLen);

  kkuint32  row, col;

  for  (row = 0;  row < xLen;  ++row)
  {
    for  (col = 0;  col < yLen;  ++col)
      xm[row][col] = x[row];
  }

  for  (row = 0;  row < xLen;  ++row)
  {
    for  (col = 0;  col < yLen;  ++col)
      ym[row][col] = y[col];
  }
}

bool BernoulliRBM::predict_(VectorDouble &inputData,VectorDouble &outputData){
    
    if( !trained ){
        errorLog << "predict_(VectorDouble &inputData,VectorDouble &outputData) - Failed to run prediction - the model has not been trained." << endl;
        return false;
    }
    
    if( inputData.size() != numVisibleUnits ){
        errorLog << "predict_(VectorDouble &inputData,VectorDouble &outputData) - Failed to run prediction - the input data size (" << inputData.size() << ")";
        errorLog << " does not match the number of visible units (" << numVisibleUnits << "). " << endl;
        return false;
    }
    
    if( outputData.size() != numHiddenUnits ){
        outputData.resize( numHiddenUnits );
    }
    
    //Scale the data if needed
    if( useScaling ){
        for(UINT i=0; i<numVisibleUnits; i++){
            inputData[i] = scale(inputData[i],ranges[i].minValue,ranges[i].maxValue,0,1);
        }
    }
    
    //Propagate the data up through the RBM
    double x = 0.0;
    for(UINT i=0; i<numHiddenUnits; i++){
        for(UINT j=0; j<numVisibleUnits; j++) {
            x += weightsMatrix[i][j] * inputData[j];
        }
        outputData[i] = sigmoid( x + hiddenLayerBias[i] );
    }
    
    return true;
}

void DTW::smoothData(VectorDouble &data,UINT smoothFactor,VectorDouble &resultsData){

	const UINT M = (UINT)data.size();
	const UINT N = (UINT) floor(double(M)/double(smoothFactor));
	resultsData.resize(N,0);
	for(UINT i=0; i<N; i++) resultsData[i]=0.0;

	if(smoothFactor==1 || M<smoothFactor){
		resultsData = data;
		return;
	}

	for(UINT i=0; i<N; i++){
	    double mean = 0.0;
		UINT index = i*smoothFactor;
		for(UINT x=0; x<smoothFactor; x++){
			mean += data[index+x];
		}
		resultsData[i] = mean/smoothFactor;
	}
	//Add on the data that does not fit into the window
	if(M%smoothFactor!=0.0){
		double mean = 0.0;
			for(UINT i=N*smoothFactor; i<M; i++) mean += data[i];
        mean/=M-(N*smoothFactor);
		//Add one to the end of the vector
		VectorDouble tempVector(N+1);
		for(UINT i=0; i<N; i++) tempVector[i] = resultsData[i];
		tempVector[N] = mean;
		resultsData = tempVector;
	}

}

TrainingSampleCheckerResult checkTrainingSample(const MatrixDouble &in) {
    VectorDouble stddev = in.getStdDev();
    if (*max_element(stddev.begin(), stddev.end()) < 0.1)
        return TrainingSampleCheckerResult(TrainingSampleCheckerResult::WARNING,
            "Warning: Gesture contains very little movement.");
    return TrainingSampleCheckerResult::SUCCESS;
}

VectorDouble MatrixDouble::multiple(const VectorDouble &b) const{
    
    const unsigned int M = rows;
    const unsigned int N = cols;
    const unsigned int K = (unsigned int)b.size();
    
    if( N != K ){
        warningLog << "multiple(vector b) - The size of b (" << b.size() << ") does not match the number of columns in this matrix (" << N << ")" << std::endl;
        return VectorDouble();
    }
    
    VectorDouble c(M);
    const double *pb = &b[0];
    double *pc = &c[0];
    
    unsigned int i,j = 0;
    for(i=0; i<rows; i++){
        pc[i] = 0;
        for(j=0; j<cols; j++){
            pc[i] += dataPtr[i*cols+j]*pb[j];
        }
    }
    
    return c;
}

//--------------------------------------------------------------
void testApp::draw(){
    
    ofBackground(0, 0, 0);
    
    string text;
    int textX = 20;
    int textY = 20;
    
    //Draw the training info
    ofSetColor(255, 255, 255);
    text = "------------------- TrainingInfo -------------------";
    ofDrawBitmapString(text, textX,textY);
    
    if( record ) ofSetColor(255, 0, 0);
    else ofSetColor(255, 255, 255);
    textY += 15;
    text = record ? "RECORDING" : "Not Recording";
    ofDrawBitmapString(text, textX,textY);
    
    ofSetColor(255, 255, 255);
    textY += 15;
    text = "TargetVector: ";
    for(UINT i=0; i<targetVector.size(); i++){
        text += ofToString(targetVector[i]) + "  ";
    }
    ofDrawBitmapString(text, textX,textY);
    
    textY += 15;
    text = "NumTrainingSamples: " + ofToString(trainingData.getNumSamples());
    ofDrawBitmapString(text, textX,textY);
    
    
    //Draw the prediction info
    textY += 30;
    text = "------------------- Prediction Info -------------------";
    ofDrawBitmapString(text, textX,textY);
    
    textY += 15;
    text =  pipeline.getTrained() ? "Model Trained: YES" : "Model Trained: NO";
    ofDrawBitmapString(text, textX,textY);
    
    textY += 15;
    text = "PredictedOutputVector: ";
    VectorDouble regressionData = pipeline.getRegressionData();
    for(UINT i=0; i<regressionData.size(); i++){
        text += ofToString(regressionData[i]) + "  ";
    }
    ofDrawBitmapString(text, textX,textY);

    
    //Draw the info text
    textY += 30;
    text = "InfoText: " + infoText;
    ofDrawBitmapString(text, textX,textY);
    
}

VectorDouble  SegmentorOTSU::Round (const VectorDouble&  v)
{
  VectorDouble  r (v.size (), 0.0);
  for  (kkuint32 x = 0;  x < v.size ();  ++x)
  {
    r[x] = (kkint32)(v[x] + 0.5f);
  }

  return  r;
}  /* Round */

VectorDouble  operator* (const VectorDouble& left,
                         double              right
                        )
{
  VectorDouble  result (left.size (), 0.0);

  for  (kkuint32 x = 0;  x < left.size ();  ++x)
    result[x] = left[x] * right;

  return  result;
}

bool SVM::predictSVM(VectorDouble &inputVector,double &maxProbability, VectorDouble &probabilites){

		if( !trained || param.probability == 0 || inputVector.size() != numInputDimensions ) return false;

		double *prob_estimates = NULL;
		svm_node *x = NULL;

		//Setup the memory for the probability estimates
		prob_estimates = new double[ model->nr_class ];

		//Copy the input data into the SVM format
		x = new svm_node[numInputDimensions+1];
		for(UINT j=0; j<numInputDimensions; j++){
			x[j].index = (int)j+1;
			x[j].value = inputVector[j];
		}
		//The last value in the input vector must be set to -1
		x[numInputDimensions].index = -1;
		x[numInputDimensions].value = 0;

		//Scale the input data if required
		if( useScaling ){
			for(UINT j=0; j<numInputDimensions; j++)
				x[j].value = scale(x[j].value,ranges[j].minValue,ranges[j].maxValue,SVM_MIN_SCALE_RANGE,SVM_MAX_SCALE_RANGE);
		}

		//Perform the SVM prediction
		double predict_label = svm_predict_probability(model,x,prob_estimates);

		predictedClassLabel = 0;
		maxProbability = 0;
		probabilites.resize(model->nr_class);
		for(int k=0; k<model->nr_class; k++){
			if( maxProbability < prob_estimates[k] ){
				maxProbability = prob_estimates[k];
                predictedClassLabel = k+1;
                maxLikelihood = maxProbability;
            }
			probabilites[k] = prob_estimates[k];
		}

        if( !useNullRejection ) predictedClassLabel = (UINT)predict_label;
        else{
            if( maxProbability >= classificationThreshold ){
                predictedClassLabel = (UINT)predict_label;
            }else predictedClassLabel = GRT_DEFAULT_NULL_CLASS_LABEL;
        }

		//Clean up the memory
		delete[] prob_estimates;
		delete[] x;

		return true;
}

int main (int argc, const char * argv[])
{
    
    //Load the example data
    ClassificationData data;
    
    if( !data.loadDatasetFromFile("WiiAccShakeData.txt") ){
        cout << "ERROR: Failed to load data from file!\n";
        return EXIT_FAILURE;
    }

    //The variables used to initialize the zero crossing counter feature extraction
    UINT searchWindowSize = 20;
    double deadZoneThreshold = 0.01;
    UINT numDimensions = data.getNumDimensions();
    UINT featureMode = ZeroCrossingCounter::INDEPENDANT_FEATURE_MODE; //This could also be ZeroCrossingCounter::COMBINED_FEATURE_MODE
    
    //Create a new instance of the ZeroCrossingCounter feature extraction
    ZeroCrossingCounter zeroCrossingCounter(searchWindowSize,deadZoneThreshold,numDimensions,featureMode);
    
    //Loop over the accelerometer data, at each time sample (i) compute the features using the new sample and then write the results to a file
    for(UINT i=0; i<data.getNumSamples(); i++){
        
        //Compute the features using this new sample
        zeroCrossingCounter.computeFeatures( data[i].getSample() );
        
        //Write the data to the file
        cout << "InputVector: ";
        for(UINT j=0; j<data.getNumDimensions(); j++){
           cout << data[i].getSample()[j] << "\t";
        }
        
        //Get the latest feature vector
        VectorDouble featureVector = zeroCrossingCounter.getFeatureVector();
        
        //Write the features to the file
        cout << "FeatureVector: ";
        for(UINT j=0; j<featureVector.size(); j++){
            cout << featureVector[j];
            if( j != featureVector.size()-1 ) cout << "\t";
        }
        cout << endl;
    }
    
    //Save the zero crossing counter settings to a file
    zeroCrossingCounter.saveModelToFile("ZeroCrossingCounterSettings.txt");
    
    //You can then load the settings again if you need them
    zeroCrossingCounter.loadModelFromFile("ZeroCrossingCounterSettings.txt");
    
    return EXIT_SUCCESS;
}

bool LabelledRegressionData::addSample(const VectorDouble &inputVector,const VectorDouble &targetVector){
	if( inputVector.size() == numInputDimensions && targetVector.size() == numTargetDimensions ){
        data.push_back( LabelledRegressionSample(inputVector,targetVector) );
        totalNumSamples++;

        //The dataset has changed so flag that any previous cross validation setup will now not work
        crossValidationSetup = false;
        crossValidationIndexs.clear();
        return true;
    }
    errorLog << "addSample(const VectorDouble &inputVector,const VectorDouble &targetVector) - The inputVector size or targetVector size does not match the size of the numInputDimensions or numTargetDimensions" << endl;
    return false;
}

double MovingAverageFilter::filter(const double x){
    
    //If the filter has not been initialised then return 0, otherwise filter x and return y
    if( !initialized ){
        errorLog << "filter(const double x) - The filter has not been initialized!" << endl;
        return 0;
    }
    
    VectorDouble y = filter(VectorDouble(1,x));
    
    if( y.size() == 0 ) return 0;
    return y[0];
}

double Derivative::computeDerivative(const double x){
    
    if( numInputDimensions != 1 ){
        errorLog << "computeDerivative(const double x) - The Number Of Input Dimensions is not 1! NumInputDimensions: " << numInputDimensions << endl;
        return 0;
    }
    
    VectorDouble y = computeDerivative( VectorDouble(1,x) );
    
    if( y.size() == 0 ) return 0 ;
    
	return y[0];
}

void  RandomSampleJobList::CalcAccuracyStats (double&  accuracyMean, 
                                              double&  accuracyStdDev,
                                              double&  trainTimeMean,
                                              double&  testTimeMean,
                                              double&  supportVectorsMean
                                             )  const
{
  VectorDouble  accuracies;
  VectorDouble  trainTimes;
  VectorDouble  testTimes;
  VectorDouble  supportVectors;

  RandomSampleJobList::const_iterator  idx;
  for  (idx = begin();  idx != end ();  idx++)
  {
    RandomSampleJobPtr j = *idx;
    accuracies.push_back (j->Accuracy ());
    trainTimes.push_back (j->TrainTime ());
    testTimes.push_back  (j->TestTime ());
    supportVectors.push_back (j->SupportVectors ());
  }  

  CalcMeanAndStdDev (accuracies, accuracyMean, accuracyStdDev);

  double  stdDev;
  CalcMeanAndStdDev (trainTimes,     trainTimeMean,      stdDev);
  CalcMeanAndStdDev (testTimes,      testTimeMean,       stdDev);
  CalcMeanAndStdDev (supportVectors, supportVectorsMean, stdDev);

}  /* CalcAccuracyStats */

double LowPassFilter::filter(double x){
    
#ifdef GRT_SAFE_CHECKING
    //If the filter has not been initialised then return 0, otherwise filter x and return y
    if( !initialized ){
        errorLog << "filter(double x) - The filter has not been initialized!" << endl;
        return 0;
    }
#endif
    
    VectorDouble y = filter(VectorDouble(1,x));
    
    if( y.size() == 0 ) return 0;
    return y[0];

}

VectorDouble MovingAverageFilter::filter(const VectorDouble &x){
    
    //If the filter has not been initialised then return 0, otherwise filter x and return y
    if( !initialized ){
        errorLog << "filter(const VectorDouble &x) - The filter has not been initialized!" << endl;
        return VectorDouble();
    }
    
    if( x.size() != numInputDimensions ){
        errorLog << "filter(const VectorDouble &x) - The size of the input vector (" << x.size() << ") does not match that of the number of dimensions of the filter (" << numInputDimensions << ")!" << endl;
        return VectorDouble();
    }
    
    if( ++inputSampleCounter > filterSize ) inputSampleCounter = filterSize;
    
    //Add the new value to the buffer
    dataBuffer.push_back( x );
    
    for(unsigned int j=0; j<numInputDimensions; j++){
        processedData[j] = 0;
        for(unsigned int i=0; i<inputSampleCounter; i++) {
            processedData[j] += dataBuffer[i][j];
        }
        processedData[j] /= double(inputSampleCounter);
    }
    
    return processedData;
}

VectorDouble Derivative::computeDerivative(const VectorDouble &x){
    
#ifdef GRT_SAFE_CHECKING
    if( !initialized ){
        errorLog << "computeDerivative(const VectorDouble &x) - Not Initialized!" << endl;
        return vector<double>();
    }
    
    if( x.size() != numInputDimensions ){
        errorLog << "computeDerivative(const VectorDouble &x) - The Number Of Input Dimensions (" << numInputDimensions << ") does not match the size of the input vector (" << x.size() << ")!" << endl;
        return vector<double>();
    }
#endif
    VectorDouble y;
    if( filterData ){
        y = filter.filter( x );
    }else y = x;
    
    for(UINT n=0; n<numInputDimensions; n++){
        processedData[n] = (y[n]-yy[n])/delta;
        yy[n] = y[n];
    }
    
    if( derivativeOrder == SECOND_DERIVATIVE ){
        double tmp = 0;
        for(UINT n=0; n<numInputDimensions; n++){
            tmp = processedData[n];
            processedData[n] = (processedData[n]-yyy[n])/delta;
            yyy[n] = tmp;
        }
    }
    
    return processedData;
}

bool LinearRegression::predict(VectorDouble inputVector){
    
    if( !trained ){
        errorLog << "predict(VectorDouble inputVector) - Model Not Trained!" << endl;
        return false;
    }
    
    if( !trained ) return false;
    
	if( inputVector.size() != numFeatures ){
        errorLog << "predict(VectorDouble inputVector) - The size of the input vector (" << inputVector.size() << ") does not match the num features in the model (" << numFeatures << endl;
		return false;
	}
    
    if( useScaling ){
        for(UINT n=0; n<numFeatures; n++){
            inputVector[n] = scale(inputVector[n], inputVectorRanges[n].minValue, inputVectorRanges[n].maxValue, 0, 1);
        }
    }
    
    regressionData[0] =  w0;
    for(UINT j=0; j<numFeatures; j++){
        regressionData[0] += inputVector[j] * w[j];
    }
	regressionData[0] = sigmoid( regressionData[0] );
    
    if( useScaling ){
        for(UINT n=0; n<numOutputDimensions; n++){
            regressionData[n] = scale(regressionData[n], 0, 1, targetVectorRanges[n].minValue, targetVectorRanges[n].maxValue);
        }
    }
    
    return true;
}

bool Classifier::setNullRejectionThresholds(VectorDouble newRejectionThresholds){
	if( newRejectionThresholds.size() == getNumClasses() ){
		nullRejectionThresholds = newRejectionThresholds;
		return true;
	}
	return false;
}