import java.awt.*;
import java.util.*;
import javax.vecmath.*;

public class SignalProcessor implements StringMessageHandler
{
    int m_numFrames;        // number of frames in signal
    int m_vectorsPerLine;   // number of vectors per frame
    int m_frequencyBands;   // number of frequency bands
    
    int m_signalAngle;      // which angle (0 - numAngles) to process
    Vector m_motionVector;  // Vector of all Vector3f angles and displacements
    float [][]m_lowpass;    // lowpass sequence (not really a pyramid .. no subsampling done)
    float [][]m_bandpass;   // bandpass sequence
    float []m_kernel;       // b-spline convolution kernel
    
    SignalProcessor(Vector motionVector, int numFrames, int angleNum, boolean toGraphInfo) {
        m_motionVector = motionVector;
        m_signalAngle = angleNum;
        m_numFrames = numFrames;
    
        m_vectorsPerLine = (m_motionVector.size() + 1) / m_numFrames;
        
        // find number of frequency bands (not the greatest algorithm!)
        //   (2 ^ m_frequencyBands) <= m_numFrames <= (2 ^ (m_frequencyBands + 1))
        int frames = 1;
        m_frequencyBands = 0;
        do {
            frames *= 2;
            if (frames < m_numFrames) m_frequencyBands++;
        } while (frames < m_numFrames);
                
        // static kernel
        m_kernel = new float[5];
        m_kernel[0] = m_kernel[4] = 1f/16f;
        m_kernel[1] = m_kernel[3] = 1f/4f;
        m_kernel[2]               = 3f/8f;
        
        // calculate sequences
        calculateLowpass();
        calculateBandpass();
        
        if (toGraphInfo) showGraphs();
        /*for (int i = 0; i < m_numFrames; i++) {
            System.out.print(m_lowpass[0][i]+"\t =? ");
            
            float reconstructVal = m_lowpass[m_frequencyBands-1][i];
            for (int count = 0; count < m_frequencyBands-1; count++) {
                reconstructVal += m_bandpass[count][i];
            }
            
            System.out.println(reconstructVal);
        }*/
    }
    
    public int getNumberOfBands() {
        return m_frequencyBands;
    }
    
    public void reconstructSignal(Vector gains) {
        float [][]newBandpass = new float[m_frequencyBands-1][m_numFrames];
        // Multiply bandpass sequence with gain values
        for (int n = 0; n < m_frequencyBands - 1; n++) {
            float gainValue = ((Float)gains.elementAt(n)).floatValue();
            for (int i = 0; i < m_numFrames; i++) {
                newBandpass[n][i] = m_bandpass[n][i] * gainValue;
            }
            
            //System.out.println("band "+n+": "+gainValue);
        }
        
        // reconstruct signal
        for (int i = 0; i < m_numFrames; i++) {
            Vector3f angle = getAngle(i);
            //System.out.print(m_lowpass[0][i]+"\t =");
            
            float reconstructVal = m_lowpass[m_frequencyBands-1][i];
            for (int count = 0; count < m_frequencyBands-1; count++) {
                reconstructVal += newBandpass[count][i];
            }
         
        /*for (int i = 0; i < numFrames; i++) {
            Vector3f angle = getAngle(i);
            setAngle(i, new Vector3f(0f, 0f, 0f));
        }*/
            
            setAngle(i, new Vector3f(angle.x, angle.y, reconstructVal));
            //System.out.println("? "+reconstructVal);
        }
    }
    
    private void showGraphs() {
        GraphsFrame graphFrame;
        SimpleGraph graph;
        Vector []data = new Vector[2];
        int columns[] = new int[2];
        String labels[] = new String[2];
        Color colors[] = new Color[2];
        
        // bandpass graph
        graphFrame = new GraphsFrame();
        data[0] = new Vector();
        data[1] = new Vector();
        for (int i = 0; i < m_numFrames; i++) {
            data[0].addElement(new Integer((int)(m_bandpass[0][i]*1000)));
            data[1].addElement(new Integer((int)(m_bandpass[m_frequencyBands/2][i]*1000)));
        }
        graph = new SimpleGraph(data, this);
        graphFrame.addGraph(graph);
        graphFrame.setLocation(300, 300);
        graphFrame.setVisible(true);
        
        
        columns[0] = 0;
        columns[1] = 1;
        
        labels[0] = "Bandpass L(0)";
        labels[1] = "Bandpass L("+(m_frequencyBands/2)+")";
        
        colors[0] = Color.white;
        colors[1] = Color.yellow;
        
        try {
            graph.showData(columns, labels, colors, "frame #", "joint angle");
        } catch(Exception e) {
            System.out.println("exception during showData:\n");
            System.out.println(e);
        }
        
        graphFrame.repaint();
        
        
        // low pass graph
        data = new Vector[2];
        columns = new int[2];
        labels = new String[2];
        colors = new Color[2];
        
        graphFrame = new GraphsFrame();
        data[0] = new Vector();
        data[1] = new Vector();
        for (int i = 0; i < m_numFrames; i++) {
            data[0].addElement(new Integer((int)(m_lowpass[0][i]*1000)));
            data[1].addElement(new Integer((int)(m_lowpass[m_frequencyBands/2][i]*1000)));
        }
        graph = new SimpleGraph(data, this);
        graphFrame.addGraph(graph);
        graphFrame.setLocation(500, 300);
        graphFrame.setVisible(true);
        
        columns[0] = 0;
        columns[1] = 1;
        
        labels[0] = "Lowpass G(0)";
        labels[1] = "Lowpass G("+(m_frequencyBands/2)+")";
        
        colors[0] = Color.white;
        colors[1] = Color.yellow;
        
        try {
            graph.showData(columns, labels, colors, "frame #", "joint angle");
        } catch(Exception e) {
            System.out.println("exception during showData:\n");
            System.out.println(e);
        }
        
        graphFrame.repaint();
    }
    
    private void calculateLowpass() {
        m_lowpass = new float[m_frequencyBands][m_numFrames];
        
        for (int i = 0; i < m_numFrames; i++) {
            Vector3f angle = getAngle(i);
            m_lowpass[0][i] = angle.z;
        }
        
        for (int k = 0; k < m_frequencyBands-1; k++) {
            for (int i = 0; i < m_numFrames; i++) {
                float sum = 0;
                
                for (int m = -2; m <= 2; m++) {
                    float kernelVal;
                    int lowpassIndex;
                    float oldLowpassVal;
                    
                    // kernel goes from 0 -> 4; map from -2 -> 2
                    kernelVal = m_kernel[m+2];
                    
                    // constant beyond boundary value
                    lowpassIndex = (int)(i + Math.pow(2, k)*m);
                    if (lowpassIndex < 0) lowpassIndex = 0;
                    else if (lowpassIndex >= m_numFrames) lowpassIndex = m_numFrames-1;
                    oldLowpassVal = m_lowpass[k][lowpassIndex];
                    
                    sum += kernelVal * oldLowpassVal;
                }
                
                m_lowpass[k+1][i] = sum;
            }
        }        
    }
    
    private void calculateBandpass() {
        m_bandpass = new float[m_frequencyBands-1][m_numFrames];
        
        for (int k = 0; k < m_frequencyBands-1; k++) {
            for (int i = 0; i < m_numFrames; i++) {
                m_bandpass[k][i] = m_lowpass[k][i] - m_lowpass[k+1][i];
            }
        }
    }
    
    private Vector3f getAngle(int frame) {
        int position = frame*m_vectorsPerLine + 1 + m_signalAngle;
        return (Vector3f)m_motionVector.elementAt(position);
    }

    private void setAngle(int frame, Vector3f newAngle) {
        int position = frame*m_vectorsPerLine + 1 + m_signalAngle;
        m_motionVector.setElementAt(newAngle, position);
    }
    
    public void handleMessage(String message) {
        System.out.println(message);
    }

}