/*
 *  scanpath.cpp
 *  springstructure
 *
 *  Created by Robert Tubb on 09/06/2011.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */
#include <vector>
#include "scanpath.h"
#include "testApp.h"

ScanPath scanPath;

// audio thread does not touch mesh lumps/springs
//----------------------------------------------------------------
ScanPath::ScanPath() : maxElements(10000) {
	// construct an empty scanpath
	// an ' element ' consists of 1 lump and one spring

    
	numElements = 0; // numElements is a dynamic running count of elements when building - a bit crap
 
    
    frameInterpolator = 0.0;
    currentLength = 0.0;
    restLength = 1.0; // ?

    scanMode = DISPLACEMENT;
    initWavetables();
    framesPerSample = 60.0/SAMPLE_RATE;
    updateAccessing = false;
    audioAccessing = false;
    
}
void ScanPath::init(){
    framesPerSample = ofGetFrameRate()/SAMPLE_RATE;
    
}
//----------------------------------------------------------------
ScanPath::~ScanPath(){


    delete [] wavetableNew;
    delete [] wavetableOld;
    delete [] wavetableUpdate;
    cout << "destructed scanpath\n"; 
}

void ScanPath::clear(){

    for(vector<Element>::iterator elitr = pathElements.begin(); elitr < pathElements.end(); elitr++){
        (*elitr).eLump->removeFromScanPath();
        (*elitr).eSpring->removeFromScanPath();
    }
    pathElements.clear();
    numElements = 0;

}
int ScanPath::howManyElements(){
    //return pathElements.size();
    return numElements;
}
//----------------------------------------------------------------
void ScanPath::inscribe(double ax, double ay){
	// look at coordinates, add the closest lump and it's connecting string 
	// if we're further away from current lump
	
	// check end points of connecting springs, pick closest
    
    // NOW IN MESH
}
void ScanPath::draw(){
    // draw the actual waveform in the corner

    int width = 768;
    int height = 128;
    double sampval = 0.0;
    int leftsampnum = 0;
    int rightsampnum = 0;
    float sampscale = 0.0, prevsampscale = 0.0, interp = 0.0;

     ofSetColor(256, 0, 0);
    double step = double(numElements)/width; // how much we are stepping thru wave per pixel
    for(int i = 0; i < width; i++){
        
        leftsampnum = floor(i * step); // basic nearest neighbour interpolation
        rightsampnum = ceil(i*step);
        interp  = (i*step)-leftsampnum;
        if(rightsampnum < numElements){
            sampval = (1 - interp)*wavetableNew[leftsampnum] + interp*wavetableNew[rightsampnum];
        }
        sampscale = (sampval * 700) + height/2.0; // centre and scale
        ofSetLineWidth(2);
        ofLine(sampscale, i, prevsampscale, i-1); // draw a line from pixel to pixel (?)
        prevsampscale = sampscale;
    }
    
}
void drawCubic(){
    
}
//----------------------------------------------------------------
// add spring
void ScanPath::addSpring(){
	// see add element
}
//----------------------------------------------------------------
// add lump
void ScanPath::addLump(){
    // see add element
}
//----------------------------------------------------------------
// add lump
double ScanPath::getTotalLength(){
    // for interesting modulations...
    currentLength = 0.0;

    for(vector<Element>::iterator elitr = pathElements.begin(); elitr < pathElements.end(); elitr++){

        currentLength = (*elitr).eSpring->getLength();
    }
    return currentLength;
    
}
//----------------------------------------------------------------
void ScanPath::initWavetables(){
    wavetableNew = new double[maxElements];
    wavetableOld = new double[maxElements];
    wavetableUpdate = new double[maxElements];
    
    for(int i = 0; i < maxElements; i++){
        wavetableOld[i] = 0.0;
        wavetableNew[i] = 0.0;
        wavetableUpdate[i] = 0.0;
    }
    
}
//----------------------------------------------------------------
void ScanPath::addElement(Lump* const aLump, Spring * const aSpring){
    // insert ptr to the lump and spring into array

    
    if(numElements + 1 >= maxElements){
        cout << " max elements reached!\n";
        return;
    }

    pathElements.push_back(Element(aLump,aSpring));
    
    // maybe do this in mesh?
    aLump->addToScanPath();
    aSpring->addToScanPath();
    numElements++;
    

}
//----------------------------------------------------------------
Lump * ScanPath::getLump(int index){
    if(index < pathElements.size()){
        return pathElements[index].eLump;
    }else{
        cerr << "bad scanpath::getLump index\n";
        return NULL;
    }
    
}
//----------------------------------------------------------------
Spring * ScanPath::getSpring(int index){
    if(index < pathElements.size()){
        return pathElements[index].eSpring;
    }else{
        cerr << "bad scanpath::getSpring index\n";
        return NULL;
    }
}

//----------------------------------------------------------------
void ScanPath::updateWavetables(){
    // swap old , new
    double * temp;
    if(pathElements.size() == 0) return;
    // TODO THRED MUTEX HERE!??
    // this is called from graphics thread
    // reset the interp between frames
/*
    int i = 0;
    while(audioAccessing){
        i++;
    }
    if(i > 0){
        cout << "Update wavetables had to wait for audio access " << i << " times\n";
        // hardly ever happens
    }
 */
    updateAccessing = true;
     

    switch(scanMode){
        case DISPLACEMENT:
            // now fill with new values
            for(int i = 0; i < numElements; i++){
                // double check
                
                wavetableUpdate[i] = pathElements[i].eLump->scanDisplacement();
                
                
            }
            break;
        case SPEED:
            for(int i = 0; i < numElements; i++){
                
                wavetableUpdate[i] = pathElements[i].eLump->scanLumpSpeed();
                
            }
            break;
        case SPRING_FORCE:
            for(int i = 0; i < numElements; i++){
                if(pathElements[i].eSpring->isInScanPath){
                    wavetableUpdate[i] = pathElements[i].eSpring->getForceMag();
                }
            }
            break;
        case YPOS:
            for(int i = 0; i < numElements; i++){
                if(pathElements[i].eLump->isInScanPath){
                    wavetableUpdate[i] = pathElements[i].eLump->scanYPos();
                }
            }
            break;
        default:
            break;
    
    }

    temp = wavetableOld;
    wavetableOld = wavetableNew;
    wavetableNew = wavetableUpdate;
    wavetableUpdate = temp;
    
    // END THREAD MUTEX
    updateAccessing = false;
    
    frameInterpolator = 0.0;
    framesPerSample = 2.0*ofGetFrameRate()/SAMPLE_RATE; // attempt to get a reasonable est. of how fast to interp
    
}
//----------------------------------------------------------------
//----------------------------------------------------------------
// AUDIO THREAD STUFF
//----------------------------------------------------------------
// get next sample

double ScanPath::getNextSample(double aPhasor){
	// move along path, interpolating between points
    // move between frames too
    if(pathElements.size() == 0) return 0.0;
    
    double alongPath = aPhasor*double(numElements);
    
    // indexes for interpolated points
    int n0 = floor(alongPath);
    int n1 = n0+1;
    if(n1 >= numElements){
        n1 = 0;
    }

    double frac = alongPath - double(n0);
    
    // TODO THRED MUTEX HERE!??
    // this is called from audio thread
/*
    int i = 0;
    while(updateAccessing){
        i++;
    }
 */
    audioAccessing = true;
    // if(i>0) cout << "Audio thread had to wait for wavetable update " << i << " times\n";

    
    double oldsample = (1 - frac) * wavetableOld[n0] + frac * wavetableOld[n1];
    
    double newsample = (1 - frac) * wavetableNew[n0] + frac * wavetableNew[n1];
    
    // END THREAD MUTEX
    audioAccessing = false;
    
    frameInterpolator += framesPerSample;
    if(frameInterpolator >= 1.0){
        //cout << "frame interp > 1\n";
        frameInterpolator = 1.0; // just stays outputting new
    }
    
    double sample = (1 - frameInterpolator)*oldsample + frameInterpolator*newsample;
    //cout << sample << endl;
    // keep within the bounds of acceptability

    return sample;
	
}
//----------------------------------------------------------------
// get next sample: just one sample at a time at sample rate
double ScanPath::getNextSample(){
	// move along wavetable, no interpolation ie: length of path is pitch
    static int n = 0;
    double oldsample = wavetableOld[n];
    
    double newsample = wavetableNew[n]; 
    n++;
    if (n >= numElements){
        n = 0;
    }

    frameInterpolator += framesPerSample;
    if(frameInterpolator >= 1.0){
        //cout << "frame interp > 1\n";
        frameInterpolator = 1.0; // just stays outputting new
    }
    
    double sample = (1 - frameInterpolator)*oldsample + frameInterpolator*newsample;
    // beef up
    sample = sample*10.0;
    
    if(sample > 0.99){
        cout << "BIG" << endl;
        
        sample = 0.99;
    }else if(sample < -0.0){
        cout << "SMALL" << endl;
        sample = 0.0;
    }
	
}
//----------------------------------------------------------------
//----------------------------------------------------------------
// get next sample with cubic interpolation
double ScanPath::getNextSampleCubic(double aPhasor){
	// move along path, interpolating between points
    // move between frames too
    double alongPath = aPhasor*double(numElements);
    
    // indexes for interpolated points
    int n1 = floor(alongPath);
    
    int n0 = n1-1;
    if(n0 < 0){
        n0 = numElements;
    }
    int n2 = n1+1;
    if(n2 >= numElements){
        n2 = 0;
    }
    int n3 = n2+1;
    if(n3 >= numElements){
        n3 = 0;
    }
    double frac = alongPath - double(n0);
    double fracSquared = frac * frac;
    double fracCubed = fracSquared * frac;
    double a0,a1,a2,a3;
    //cout << n0 << endl;
    // cubic interp
    /*
     double y0,double y1,
     double y2,double y3,
     double mu)
     {
     double a0,a1,a2,a3,mu2;
     
     mu2 = mu*mu;
     a0 = y3 - y2 - y0 + y1;
     a1 = y0 - y1 - a0;
     a2 = y2 - y0;
     a3 = y1;
     
     return(a0*mu*mu2+a1*mu2+a2*mu+a3);
     */
    a0 = wavetableOld[n3] - wavetableOld[n2] - wavetableOld[n0] + wavetableOld[n1];
    a1 = wavetableOld[n0] - wavetableOld[n1] - a0; //  y0 - y1 - a0;
    a2 = wavetableOld[n2] - wavetableOld[n0]; // y2 - y0;
    a3 = wavetableOld[n1];
    
    double oldsample = a0*fracCubed + a1*fracSquared + a2*frac + a3;
    
    a0 = wavetableNew[n3] - wavetableNew[n2] - wavetableNew[n0] + wavetableNew[n1];
    a1 = wavetableNew[n0] - wavetableNew[n1] - a0; //  y0 - y1 - a0;
    a2 = wavetableNew[n2] - wavetableNew[n0]; // y2 - y0;
    a3 = wavetableNew[n1];
    
    double newsample = a0*fracCubed + a1*fracSquared + a2*frac + a3;
    
    frameInterpolator += framesPerSample;
    if(frameInterpolator >= 1.0){
        frameInterpolator = 1.0; // just stays outputting new
    }
    
    double sample = (1 - frameInterpolator)*oldsample + frameInterpolator*newsample;
    //cout << sample << endl;
    // keep within the bounds of acceptability
    
    // beef up
    sample = sample*10.0;
    
    if(sample > 0.99){
        cout << "BIG" << endl;
        
        sample = 0.99;
    }else if(sample < -0.0){
        cout << "SMALL" << endl;
        sample = 0.0;
    }
    
    return sample;
	
}
//----------------------------------------------------------------

Json::Value ScanPath::convertToJsonForSaving(){
    Json::Value jscanpath;

    for(int i=0;i<pathElements.size();i++){
        //scanPath.addElement(&lumps[scanPathElements[i]["lumpNo"].asInt()], &springs[scanPathElements[i]["springNo"].asInt()]);
        jscanpath[i]["lumpNo"] = pathElements[i].eLump->myIndexInMesh;
        jscanpath[i]["springNo"] = pathElements[i].eSpring->myIndexInMesh;
    }
    
    return jscanpath;
}

//----------------------------------------------------------------

//----------------------------------------------------------------

//----------------------------------------------------------------

