package awa.imu;

import java.awt.GridBagConstraints;
import java.awt.GridBagLayout;
import java.awt.GridLayout;
import java.awt.Insets;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.io.File;
import java.text.DecimalFormat;
import java.text.DecimalFormatSymbols;
import java.text.NumberFormat;
import java.util.Locale;

import javax.swing.*;
import javax.swing.border.Border;

public class IMUControlPanel extends JPanel implements ActionListener
{
    private static final DecimalFormatSymbols usfs = new DecimalFormatSymbols(Locale.US);
    private static final NumberFormat msFormat = new DecimalFormat(" 0.000;-0.000", usfs);
    
    private IMUState imu = IMUState.getInstance();
    private Kalman kalmanFilter = new Kalman();
    
    private boolean trim = false;
    private int tcount;
    
    private long lastUpdate;
    private int update;
    
    private static final int MODE_DEMO = 0;
    private static final int MODE_SHOWACC = 1;
    private static final int MODE_ACC = 2;
    private static final int MODE_GYRO = 3;
    private static final int MODE_KF = 4;
    private int mode = MODE_DEMO;
    
    private Main3dScreen screen3d;
    //private RS232Reader reader;
    private LogFileReader reader;
    private File logfile = new File("ezstar_20080513k.log");
    //private UDPReader reader;
    
    private JPanel radioPanel = new JPanel();
    private JPanel dataPanel = new JPanel();
    private JPanel eulerPanel = new JPanel();
    private JPanel buttonPanel = new JPanel();
    
    private Border etchedBorder = BorderFactory.createEtchedBorder();
    
    private ButtonGroup bgroup = new ButtonGroup();
    private JRadioButton demoButton = new JRadioButton("Demo", true);
    private JRadioButton showAccButton = new JRadioButton("Raw Data");
    private JRadioButton imuAccButton = new JRadioButton("IMU Acc");
    private JRadioButton imuGyroButton = new JRadioButton("IMU Gyro");
    private JRadioButton kalmanButton = new JRadioButton("IMU KF");
    
    private JLabel xAccLabel = new JLabel("xAcc:");
    private JLabel yAccLabel = new JLabel("yAcc:");
    private JLabel zAccLabel = new JLabel("zAcc:");
    private JLabel rollLabel = new JLabel("roll:");
    private JLabel pitchLabel = new JLabel("pitch:");
    private JLabel yawLabel = new JLabel("yaw:");
    private JLabel rateLabel = new JLabel("rate:");
    
    /*
    private JTextField xAccDisplay = new JTextField();
    private JTextField yAccDisplay = new JTextField();
    private JTextField zAccDisplay = new JTextField();
    private JTextField rollDisplay = new JTextField();
    private JTextField pitchDisplay = new JTextField();
    private JTextField yawDisplay = new JTextField();
    private JTextField rateDisplay = new JTextField();
    */
    private IndicatorField xAccDisplay = new IndicatorField();
    private IndicatorField yAccDisplay = new IndicatorField();
    private IndicatorField zAccDisplay = new IndicatorField();
    private IndicatorField rollDisplay = new IndicatorField();
    private IndicatorField pitchDisplay = new IndicatorField();
    private IndicatorField yawDisplay = new IndicatorField();
    private IndicatorField rateDisplay = new IndicatorField();
    
    private JLabel phiLabel = new JLabel("phi:");
    private JLabel thetaLabel = new JLabel("theta:");
    private JLabel psiLabel = new JLabel("psi:");
    
    private JTextField phiDisplay = new JTextField();
    private JTextField thetaDisplay = new JTextField();
    private JTextField psiDisplay = new JTextField();
    
    private JButton trimButton = new JButton("Calibrate");
    //private JButton resObjButton = new JButton("Reset Object");
    private JButton resViewButton = new JButton("Reset View");
    
    public IMUControlPanel(Main3dScreen screen) 
    {
        screen3d = screen;
        
        setLayout(new GridBagLayout());
        
        radioPanel.setLayout(new GridLayout(5, 1));
        
        bgroup.add(demoButton);
        bgroup.add(showAccButton);
        bgroup.add(imuAccButton);
        bgroup.add(imuGyroButton);
        bgroup.add(kalmanButton);
        
        //Register a listener for the radio buttons.
        demoButton.addActionListener(this);
        showAccButton.addActionListener(this);
        imuAccButton.addActionListener(this);
        imuGyroButton.addActionListener(this);
        kalmanButton.addActionListener(this);
        
        radioPanel.add(demoButton);
        radioPanel.add(showAccButton);
        radioPanel.add(imuAccButton);
        radioPanel.add(imuGyroButton);
        radioPanel.add(kalmanButton);
        //kalmanButton.setEnabled(false);
        
        radioPanel.setBorder(etchedBorder);
        add(radioPanel, new GridBagConstraints(0,0,1,1,1.0,0.2,GridBagConstraints.NORTH,GridBagConstraints.HORIZONTAL,new Insets(3,3,3,3),0,0));
        
        dataPanel.setLayout(new GridLayout(7, 2));
        
        xAccDisplay.setEditable(false);
        yAccDisplay.setEditable(false);
        zAccDisplay.setEditable(false);
        rollDisplay.setEditable(false);
        pitchDisplay.setEditable(false);
        yawDisplay.setEditable(false);
        rateDisplay.setEditable(false);
        
        dataPanel.add(xAccLabel);
        dataPanel.add(xAccDisplay);
        dataPanel.add(yAccLabel);
        dataPanel.add(yAccDisplay);
        dataPanel.add(zAccLabel);
        dataPanel.add(zAccDisplay);
        
        dataPanel.add(rollLabel);
        dataPanel.add(rollDisplay);
        dataPanel.add(pitchLabel);
        dataPanel.add(pitchDisplay);
        dataPanel.add(yawLabel);
        dataPanel.add(yawDisplay);
        
        dataPanel.add(rateLabel);
        dataPanel.add(rateDisplay);
        
        dataPanel.setBorder(etchedBorder);
        add(dataPanel, new GridBagConstraints(0,1,1,1,1.0,0.3,GridBagConstraints.NORTH,GridBagConstraints.HORIZONTAL,new Insets(3,3,3,3),0,0));
        
        eulerPanel.setLayout(new GridLayout(3, 2));
        
        phiDisplay.setEditable(false);
        thetaDisplay.setEditable(false);
        psiDisplay.setEditable(false);
        
        eulerPanel.add(phiLabel);
        eulerPanel.add(phiDisplay);
        eulerPanel.add(thetaLabel);
        eulerPanel.add(thetaDisplay);
        eulerPanel.add(psiLabel);
        eulerPanel.add(psiDisplay);
        
        eulerPanel.setBorder(etchedBorder);
        add(eulerPanel, new GridBagConstraints(0,2,1,1,1.0,0.3,GridBagConstraints.NORTH,GridBagConstraints.HORIZONTAL,new Insets(3,3,3,3),0,0));
        
        buttonPanel.setLayout(new GridLayout(2, 1));
        buttonPanel.add(trimButton);
        //buttonPanel.add(resObjButton);
        buttonPanel.add(resViewButton);
        
        //resObjButton.setEnabled(false);
        
        trimButton.addActionListener(this);
        //resObjButton.addActionListener(this);
        resViewButton.addActionListener(this);
        
        buttonPanel.setBorder(etchedBorder);
        add(buttonPanel, new GridBagConstraints(0,3,1,1,1.0,0.3,GridBagConstraints.NORTH,GridBagConstraints.HORIZONTAL,new Insets(3,3,3,3),0,0));
    }
    
    public void actionPerformed(ActionEvent event) {
        Object object = event.getSource();
        if (object == demoButton)
            demoButton_actionPerformed();        
        else if (object == showAccButton)
            showAccButton_actionPerformed();
        else if (object == imuAccButton)
            imuAccButton_actionPerformed();
        else if (object == imuGyroButton)
            imuGyroButton_actionPerformed();
        else if (object == kalmanButton)
            kalmanButton_actionPerformed();
        else if (object == trimButton)
            trimButton_actionPerformed();
        else if (object == resViewButton)
            resViewButton_actionPerformed();
    }
    
    private void kalmanButton_actionPerformed()
    {
        screen3d.setDemo(false);
        mode = MODE_KF;
        //if (reader != null && reader.isAlive())
        //    reader.stopPlay();
        //dummy = new DummyReader(this);
        //dummy.start();
        if (reader == null || !reader.isAlive()) {
            //reader = new RS232Reader(this);
            reader = new LogFileReader(this, logfile);
            //reader = new UDPReader(this);
            reader.start();
        }
    }
    
    private void demoButton_actionPerformed()
    {
        screen3d.setDemo(true);
        screen3d.setShowRaw(false);
        //if (dummy != null && dummy.isAlive())
        //    dummy.stopPlay();
        if (reader != null && reader.isAlive())
            reader.stopPlay();
        mode = MODE_DEMO;
    }
        
    private void showAccButton_actionPerformed()
    {
        screen3d.setDemo(false);
        screen3d.setShowRaw(true);
        mode = MODE_SHOWACC;
        //if (dummy != null && dummy.isAlive())
        //    dummy.stopPlay();
        if (reader == null || !reader.isAlive()) {
            //reader = new RS232Reader(this);
            reader = new LogFileReader(this, logfile);
            //reader = new UDPReader(this);
            reader.start();
        }
    }
    
    private void imuAccButton_actionPerformed()
    {
        screen3d.setDemo(false);
        screen3d.setShowRaw(false);
        mode = MODE_ACC;
        //if (dummy != null && dummy.isAlive())
        //    dummy.stopPlay();
        if (reader == null || !reader.isAlive()) {
            //reader = new RS232Reader(this);
            reader = new LogFileReader(this, logfile);
            //reader = new UDPReader(this);
            reader.start();
        }
    }
    
    private void imuGyroButton_actionPerformed()
    {
        screen3d.setDemo(false);
        screen3d.setShowRaw(false);
        mode = MODE_GYRO;
        //if (dummy != null && dummy.isAlive())
        //    dummy.stopPlay();
        if (reader == null || !reader.isAlive()) {
            //reader = new RS232Reader(this);
            reader = new LogFileReader(this, logfile);
            //reader = new UDPReader(this);
            reader.start();
        }
    }
    
    private void trimButton_actionPerformed()
    {
        trim = true;
        tcount = 0;
        trimButton.setEnabled(false);
    }
    
    private void resViewButton_actionPerformed() 
    {
        screen3d.resetView();
    }
    
    public void setIMUValues(String pitchH, String rollH, String yawH, String accyH, String accxH, String acczH) 
    {
        update++;
        
        long curr = System.currentTimeMillis();
        if ((curr - lastUpdate) > 5000) {
            rateDisplay.setText(String.valueOf(update / 5));
            update = 0;
            lastUpdate = curr;
        }
        
        int xAccI = Integer.parseInt(accxH, 16);
        int yAccI = Integer.parseInt(accyH, 16);
        int zAccI = Integer.parseInt(acczH, 16);
        
        int rRollI = Integer.parseInt(rollH, 16);
        int rPitchI = Integer.parseInt(pitchH, 16);
        int rYawI = Integer.parseInt(yawH, 16);
        
        if (trim) {
            if (tcount < 50) {
                tcount++;
                imu.xAccTrim += xAccI;
                imu.yAccTrim += yAccI;
                imu.zAccTrim += zAccI; // + (int) imu.zAccGain;
                imu.rollTrim += rRollI;
                imu.pitchTrim += rPitchI;
                imu.yawTrim += rYawI;
            } else {
                System.out.println("tcount="+tcount);
                imu.xAccNeutral = imu.xAccTrim / tcount;
                imu.yAccNeutral = imu.yAccTrim / tcount;
                imu.zAccNeutral = imu.zAccTrim / tcount;
                imu.pitchNeutral = imu.pitchTrim / tcount;
                imu.rollNeutral = imu.rollTrim / tcount;
                imu.yawNeutral = imu.yawTrim / tcount;
                
                imu.xAccTrim = 0;
                imu.yAccTrim = 0;
                imu.zAccTrim = 0;
                imu.rollTrim = 0;
                imu.pitchTrim = 0;
                imu.yawTrim = 0;
                
                System.out.println("imu=" + imu);
                
                // initialize Kalman Filter
                kalmanFilter.ahrs_init(0.0, 0.0, 1.0, 0.0);
                
                trim = false;
                trimButton.setEnabled(true);
            }
        }
        
        // convert accelerometer values into body frame (-x->y, y->x, -z->z)
        double yAccB = -(xAccI - imu.xAccNeutral) / imu.xAccGain; // [116 - 1F0 - 2CE]
        double xAccB = (yAccI - imu.yAccNeutral) / imu.yAccGain; // [146 - 212 - 2F0]
        double zAccB = 1.0 - (zAccI - imu.zAccNeutral) / imu.zAccGain;// [0D0 - 190 - 280]
        
        double rRoll = -(rRollI - imu.rollNeutral) * imu.rollGain;   // rad / sec
        double rPitch = -(rPitchI - imu.pitchNeutral) * imu.pitchGain;
        double rYaw = (rYawI - imu.yawNeutral) * imu.yawGain;
        
        xAccDisplay.setDoubleValue(xAccB);
        yAccDisplay.setDoubleValue(yAccB);
        zAccDisplay.setDoubleValue(zAccB);
        
        if (mode == MODE_SHOWACC) screen3d.showRaw(xAccB, yAccB, zAccB, rRoll, rPitch, rYaw);
        
        if (mode == MODE_ACC) screen3d.setXYZ(xAccB, yAccB, zAccB);
        
        rollDisplay.setDoubleValue(rRoll);
        pitchDisplay.setDoubleValue(rPitch);
        yawDisplay.setDoubleValue(rYaw);
        
        if (mode == MODE_GYRO) {
            //screen3d.addRotation(0.02*rRoll, 0.02*rPitch, -0.02*rYaw);
            Euler e = kalmanFilter.addRotation(0.02, rRoll, rPitch, rYaw);
            screen3d.setRotation(e.phi, e.theta, e.psi);
        }
        
        if (mode == MODE_KF) {
            kalmanFilter.ahrs_step(0.02, 
                    rRoll, rPitch, rYaw,
                    xAccB, yAccB, zAccB, rYaw);
            
            screen3d.setRotation(kalmanFilter.angles.phi, 
                    kalmanFilter.angles.theta, 
                    kalmanFilter.angles.psi);
        }
    }
    
    public void displayRotation(double phi, double theta, double psi)
    {
        phiDisplay.setText(msFormat.format(Math.toDegrees(phi)));
        thetaDisplay.setText(msFormat.format(Math.toDegrees(theta)));
        psiDisplay.setText(msFormat.format(Math.toDegrees(psi)));
    }
    
}