#include "crp.h"

#include <iostream>
#include <fstream>
#include <numeric>
#include <cmath>

bool readSampleData( const std::string &file, std::vector<float> &vec );
void normalize( std::vector<float> &sin );
std::ostream &operator<<( std::ostream &ostr, const std::vector<float> &vec );

bool crp( int m, int t, float e, const std::vector<float> &x_in, boost::multi_array< float, 2> &s, bool debug )
{
    std::vector<float> x(x_in);

    normalize( x );

    if( debug )
    {
        std::ofstream ofstr( "x_cpp.txt" );
        for( unsigned int cnt = 0; cnt < x.size(); ++cnt )
        {
            ofstr << x[cnt] << std::endl;
        }
    }

    int Nx = x.size();
    Nx = Nx - (t * (m-1) );

    if( Nx < 1 )
        return false;

    boost::multi_array< float, 2> x2(boost::extents[Nx][m]);
//    boost::multi_array< float, 2> y2(boost::extents[Nx][m]); // Looks to me like x2 and y2 are going to be identical. Better check that. Checked and they are identical

//    std::vector<float> j;
//    for( int cnt = 0, max = ((m - 1) * t); cnt <= max; cnt += t )
//        j.push_back( cnt );
//    std::cout << j;

    // generate x embedding vector
    for( int j = 0; j < m; j++ )
    {
        for( int cnt = 0; cnt < Nx; ++cnt )
        {
            int ind = cnt + (j * t);
            x2[cnt][j] = x[ind];
        }
    }

    if( debug )
    {
        std::ofstream ofstr( "x2_cpp.txt" );
        for( int cnt = 0; cnt < Nx; ++cnt )
        {
            for( int j = 0; j < m; j++ )
            {
                ofstr << x2[cnt][j] << " ";
            }
            ofstr << std::endl;
        }
    }

    // create an NY by NX matrix all set to zero
    // permute x2 and y2 to create px and py, but also add an extra, empty dimension.
    // s1 = subtract py from px (with some extra ones thrown in there somehow)
    // s = max( abs(s1), [], 3 );
    // X2 = s < e
    // X = uint8( X2 )';

//    boost::multi_array< float, 3> s1(boost::extents[Nx][Nx][m]);
//    boost::multi_array< float, 2> s(boost::extents[Nx][Nx]);

    s.resize( boost::extents[Nx][Nx] );
    for( int x_m = 0; x_m < m; ++x_m )
            for( int x_nx = 0; x_nx < Nx; ++x_nx )
            {
                for( int y_nx = 0; y_nx < Nx; ++y_nx )
                {
                    float absDiff = std::abs( x2[x_nx][x_m] - x2[y_nx][x_m] );
//                    if( 0 == x_m )
//                    {
//                        s[x_nx][y_nx] = absDiff;
//                    }
//                    else
                        if( absDiff > s[x_nx][y_nx] )
                    {
                        s[x_nx][y_nx] = absDiff;
                    }
//                    s1[x_nx][y_nx][x_m] = x2[x_nx][x_m] - x2[y_nx][x_m];
                }
            }

    for( int x_nx = 0; x_nx < Nx; ++x_nx )
    {
        for( int y_nx = 0; y_nx < Nx; ++y_nx )
        {
            s[x_nx][y_nx] = s[x_nx][y_nx] < e;
        }
    }

//    if( debug )
//    {
//        std::ofstream ofstr( "s1a_cpp.txt" );
//        std::ofstream ofstrb( "s1b_cpp.txt" );
//        for( int cnt = 0; cnt < Nx; ++cnt )
//        {
//            for( int j = 0; j < Nx; j++ )
//            {
//                ofstr << s1[cnt][j][0] << " ";
//                ofstrb << s1[cnt][j][1] << " ";
//            }
//            ofstr << std::endl;
//            ofstrb << std::endl;
//        }
//    }

    if( debug )
    {
        std::ofstream ofstr( "s_cpp.txt" );
        for( int cnt = 0; cnt < Nx; ++cnt )
        {
            for( int j = 0; j < Nx; j++ )
            {
                ofstr << s[cnt][j] << " ";
            }
            ofstr << std::endl;
        }
    }

    // Each s that is less than e is set to one
    return true;
}

std::ostream &operator<<( std::ostream &ostr, const std::vector<float> &vec )
{
    for( unsigned int ind = 0; ind < vec.size(); ++ind )
    {
        ostr << vec[ind];
        if( ind < (vec.size()-1) )
            ostr << ", ";
    }
    ostr << std::endl;
    return ostr;
}

void normalize( std::vector<float> &sin )
{
    double sum = std::accumulate( sin.begin(), sin.end(), 0.0 );
    double mean = sum / static_cast<double>(sin.size());
    double diff_squared = 0.0;

    for( std::vector<float>::iterator iter = sin.begin(), last = sin.end(); iter != last; ++iter )
    {
        float x = *iter - mean;
        diff_squared += static_cast<double>(x) * static_cast<double>(x);
    }

    double variance = diff_squared / static_cast<double>(sin.size()-1);
    double stdDev = std::sqrt( variance );

//    std::cout << "Mean: " << mean << ",  stdev: " << stdDev << std::endl;

    for( std::vector<float>::iterator iter = sin.begin(), last = sin.end(); iter != last; ++iter )
    {
        *iter = (*iter - mean) / stdDev;
    }
}