//
//  6Dbox.mm
//  riftathon
//
//  Created by Robert Tubb on 27/10/2014.
//
//

#include "6Dbox.h"

ofVec3f upUnit = ofVec3f(0.0, 1.0, 0.0);
ofVec3f fwdUnit = ofVec3f(0.0, 0.0, 1.0);
ofVec3f rightUnit = ofVec3f(1.0, 0.0, 0.0);

Leap6DBox::Leap6DBox(float ax,
                         float ay,
                         float awidth,
                         float aheight,
                         float azx,
                         float azy,
                         const UIProps& props) :
Leap3DBoxGL(ax,ay,awidth, aheight, azx, azy, props)
{

    rollVal = 0.0;
    pitchVal = 0.0;
    yawVal = 0.0;

    // set up hand mesh
    // width 0.5
    // length 1
    // height 0.25
    hw = 0.5;
    hh = 0.15;
    hl = 1.0;
    scale = 40;
    
    string fname = ofFilePath::getAbsolutePath(ofToDataPath("turqoise.jpeg"));
    defaultImage.loadImage(fname);
    
    fname = ofFilePath::getAbsolutePath(ofToDataPath("bakedbeans.jpeg"));
    hintImage.loadImage(fname);
    
    setTexture(&defaultImage);
    setHintTexture(&hintImage);
    setUpHandMesh();
    
    showHint(true);
    
}

void Leap6DBox::setHintValues(vector<int> vals){
    if (vals.size() != 6){
        cout << "ERROR: need 6 vals for 6dof box set hint" << endl;
    }
    
    for(int i = 0; i < 6; i++){
        Leap3DBox::setHintValue(i, vals[i]);
    }
}
void Leap6DBox::setValues(vector<int> vals){
    if (vals.size() != 6){
        cout << "ERROR: need 6 vals for 6dof box set hint" << endl;
    }
    
    for(int i = 0; i < 6; i++){
        Leap3DBox::setValueAndScale(i, vals[i]);
    }
}

void Leap6DBox::setHintValue(int which, int val){
    if(which == 0)      hintX =  (val - minVal)/(maxVal - minVal);
    if(which == 1)      hintY =  (val - minVal)/(maxVal - minVal);
    if(which == 2)      hintZ =  (val - minVal)/(maxVal - minVal);
    
    if(which == 3)      hintRoll =  57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
    if(which == 4)      hintPitch = - 57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
    if(which == 5)      hintYaw =    - 57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
};

void Leap6DBox::setValueAndScale(int which, int val){
    
    
    if(which == 0)      xVal =  (val - minVal)/(maxVal - minVal);
    if(which == 1)      yVal =  (val - minVal)/(maxVal - minVal);
    if(which == 2)      zVal =  (val - minVal)/(maxVal - minVal);
    // angles
    
    if(which == 3)      rollVal =  57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
    if(which == 4)      pitchVal = - 57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
    if(which == 5)      yawVal =   - 57.3*  (2.0*(val - minVal)/(maxVal - minVal) - 1.0);
}

void Leap6DBox::makeTexFace(ofPoint LT, ofPoint RT, ofPoint  RB, ofPoint LB){
    
    ofPoint texCoordLT = ofPoint(0, 0.5);
    ofPoint texCoordLB = ofPoint(0, 0);
    ofPoint texCoordRT = ofPoint(0.5, 0.5);
    ofPoint texCoordRB = ofPoint(0.5, 0);
    
    handMesh.addVertex(LT);
    handMesh.addTexCoord(texCoordLT);
    handMesh.addVertex(LB);
    handMesh.addTexCoord(texCoordLB);
    handMesh.addVertex(RB);
    handMesh.addTexCoord(texCoordRB);

    
    
    handMesh.addVertex(LT);
    handMesh.addTexCoord(texCoordLT);

    handMesh.addVertex(RB);
    handMesh.addTexCoord(texCoordRB);
    handMesh.addVertex(RT);
    handMesh.addTexCoord(texCoordRT);
}
void Leap6DBox::setUpHandMesh(){
    ofPoint rtr = ofPoint(hw, hh, -hl);
    ofPoint ltr = ofPoint(-hw, hh, -hl);
    ofPoint rbr = ofPoint(hw, -hh, -hl);
    ofPoint rtf = ofPoint(hw, hh, hl);
    ofPoint lbr = ofPoint(-hw, -hh, -hl);
    ofPoint rbf = ofPoint(hw, -hh, hl);
    ofPoint lbf = ofPoint(-hw, -hh, hl);
    ofPoint ltf = ofPoint(-hw, hh, hl);
 
    int imageWidth = 259;
    int imageHeight = 254;

    // back
    makeTexFace(ltr, rtr, rbr, lbr);
    
    // left
    makeTexFace(ltr,ltf, lbf, lbr);
    
    // front
    makeTexFace(ltf, rtf, rbf, lbf);
    
    // right
    makeTexFace(rtf,rtr, rbr, rbf);
    
    //top
    makeTexFace(ltr, rtr, rtf, ltf);
    
    // bottom
    makeTexFace(lbf, rbf, rbr, lbr);
    
    setUpThumb(ltf, lbf);
  
    handMesh.setupIndicesAuto();
    setNormals(handMesh);
}
void Leap6DBox::setUpThumb(ofPoint baseTR, ofPoint baseBR){
    // 3 triangles

    ofPoint baseTL = baseTR;
    ofPoint baseBL = baseBR;
    baseTL.z -= 0.9*hl;
    baseBL.z -= 0.9*hl;
    ofPoint tip = baseBL;
    tip.x -= hw;
    tip.y += hh;
    
    
    ofPoint texCoordLT = ofPoint(0, 0.5);
    ofPoint texCoordLB = ofPoint(0, 0);
    ofPoint texCoordRT = ofPoint(0.5, 0.5);
    ofPoint texCoordRB = ofPoint(0.5, 0);
    
    // front
    handMesh.addVertex(baseTR);
    handMesh.addTexCoord(texCoordLT);
    handMesh.addVertex(tip);
    handMesh.addTexCoord(texCoordRB);
    handMesh.addVertex(baseBR);
    handMesh.addTexCoord(texCoordLB);

    
    // top

    handMesh.addVertex(baseTL);
    handMesh.addTexCoord(texCoordLB);
    handMesh.addVertex(tip);
    handMesh.addTexCoord(texCoordRB);
    handMesh.addVertex(baseTR);
    handMesh.addTexCoord(texCoordLT);

    // back
    handMesh.addVertex(baseTL);
    handMesh.addTexCoord(texCoordLT);
    handMesh.addVertex(baseBL);
    handMesh.addTexCoord(texCoordLB);
    handMesh.addVertex(tip);
    handMesh.addTexCoord(texCoordRB);


    // left
    handMesh.addVertex(baseBL);
    handMesh.addTexCoord(texCoordLT);
    handMesh.addVertex(baseBR);
    handMesh.addTexCoord(texCoordLB);
    handMesh.addVertex(tip);
    handMesh.addTexCoord(texCoordRB);


    

}


void Leap6DBox::setTexture(ofImage* img){
    texImg = img;
    
    
}
void Leap6DBox::setHintTexture(ofImage* img){
    hintImageRef = img;
    
    
}

void Leap6DBox::draw(){
    if(hidden)return;
    if(on){
        ofSetColor(foregroundHi);
    }else{
        ofSetColor(foregroundLo);
        
    }
    if(inactive){
        ofSetColor(fgInactive);
    }
    
    
    
    ofPushMatrix();
    ofDisableAlphaBlending();
    ofEnableDepthTest();
    // move to correct pos
    
    ofTranslate( x+width/2, y+height/2, camTrans);
    ofRotate( angleX, 1, 0, 0 );
    ofRotate( angleY, 0, 1, 0 );
    
    
    ofSetColor(foregroundHi);
    boxMesh.draw();
    
    // draw indicators
    drawIndicator();
    ofPopMatrix();
    
    drawLabels();
};

void Leap6DBox::drawIndicator(){
    
    if (hintShowing && (hintZ > zVal)){
        hintColor = calculateHintColor();
        draw3DCrossHairs(hintX, hintY, hintZ,hintColor, true);
        
    }
    
    // draw indicator
    draw3DCrossHairs(xVal,yVal,zVal, indicatorColor);
    // put light in indicateor
    //
    
    
    if (hintShowing && hintZ <= zVal){
        hintColor = calculateHintColor();
        draw3DCrossHairs(hintX, hintY, hintZ,hintColor, true);
        
    }
}
void Leap6DBox::draw3DCrossHairs(float x , float y, float z, ofColor c, bool isHint){
    
    float ix = x*width - width/2;
    float iy = (1-y)*width - width/2;
    float iz = (1-z)*width - width/2;
    
    float left = - width/2;
    float bot = width/2;
    float front = width/2;
    //
    ofSetColor(c);
    ofSetLineWidth(2.);
    // line to bottom
    ofLine(ix,iy,iz,ix,bot,iz);
    // line to left
    ofLine(ix,iy,iz,left,iy,iz);
    
    //blob
    drawIndicatorBlob(ix,iy,iz,c, isHint);
    // line to front (a bit wierde?)
    ofLine(ix,iy,iz,ix,iy,front);
  
}


void Leap6DBox::drawIndicatorBlob(float x, float y, float z, ofColor c, bool isHint){
    ofImage * atexImg;
    if(isHint){
        atexImg = hintImageRef;
    }else{
        atexImg = texImg;
    }
    
    if (isHint){
        draw6DOFIndicatorBlob(x, y, z, c, hintRoll, hintPitch, 	hintYaw, hintImageRef);
    }else{
        draw6DOFIndicatorBlob(x, y, z, c, rollVal, pitchVal, yawVal, texImg);

    }
}
void Leap6DBox::draw6DOFIndicatorBlob(float x, float y, float z, ofColor c, float r, float p, float yaw, ofImage* atexImg){

    
    static ofMatrix4x4 m;
    
	m.makeScaleMatrix(scale,scale,scale);
    
    
    ofMatrix4x4 rot = getRotationQuat(r, p, yaw);
	m.translate(x,y,z);
    
    ofPushMatrix();
    ofMultMatrix(m);
    ofMultMatrix(rot);
    
    ofSetColor(255,255,255);
    // render
    (*atexImg).bind();
    
    handMesh.draw();
    (*atexImg).unbind();
    ofPopMatrix();
    ofSetColor(c);
    
}

// TODO animation
ofMatrix4x4 Leap6DBox::getRotationQuat(float roll, float pitch, float yaw){
    static ofMatrix4x4 rot;
    rot.makeRotationMatrix(roll, fwdUnit, pitch, rightUnit, yaw, upUnit);
    return rot;
}
void Leap6DBox::animateHintToNewValues(float tx, float ty, float tz, float tr, float tp, float tyaw){
    ofVec3f curAnimPos = ofVec3f(hintX, hintY,hintZ);
    ofVec3f animtargetPos = ofVec3f(tx, ty, tz);
    
    ofMatrix4x4 curRot = getRotationQuat(hintRoll, hintPitch, hintYaw);
    
    
}

// for animation
void Leap6DBox::update(){
    
    
}
