package awa.imu;

import java.io.*;
import java.net.*;
import java.util.StringTokenizer;

/**
 * Reads UDP packages sent by FlightGear simulator to display accelerations and angle rates
 * FlightGear must be launched using the command line argument --generic=socket,out,50,<target_IP>,5505,udp,fgsimout
 * The protocol file fgsimout.xml must be placed into the FlightGear Protocol directory
 *
 * @author awa
 *
 */
public class UDPReader extends Thread
{
    private static final double Ftpss2G = 0.3048 / 9.826;
    private static final double Deg2Rad = Math.PI / 180.0;

    private IMUControlPanel parent;
    private boolean stop = false;
    
    public UDPReader(IMUControlPanel panel)
    {
        parent = panel;
    }
    
    public void run()
    {
        try {
            
            byte[] inData = new byte[1024];
            String message;
            IMUState imu = IMUState.getInstance();

            // Socket erzeugen
            DatagramSocket socket;
            socket = new DatagramSocket(5505);
            
            // Antwort empfangen und ausgeben.
            DatagramPacket in = new DatagramPacket(inData, inData.length);
            while (!stop) {
                socket.receive(in);
    
                message = new String(in.getData(),0,in.getLength());

                //System.out.println("read: " + message);
                StringTokenizer st = new StringTokenizer(message, ",");

                // first field is GMT time in format "hh:mm:ss"
                String inTime = st.nextToken();     // sim/time/gmt-string
                int inDay   = Integer.parseInt(st.nextToken());   // sim/time/utc/day
                int inMonth = Integer.parseInt(st.nextToken());                 // sim/time/utc/month
                int inYear  = Integer.parseInt(st.nextToken());                   // sim/time/utc/year
                double inElapsedSec = Double.parseDouble(st.nextToken());         // sim/time/elapsed-sec
                double inLatDeg = Double.parseDouble(st.nextToken());             // position/latitude-deg
                double inLonDeg = Double.parseDouble(st.nextToken());             // position/longitude-deg
                double inAltFt = Double.parseDouble(st.nextToken());              // position/altitude-ft
                double inAltAglFt = Double.parseDouble(st.nextToken());           // position/altitude-agl-ft
                double inAccXFtpss = Double.parseDouble(st.nextToken());
                double inAccYFtpss = Double.parseDouble(st.nextToken());
                double inAccZFtpss = Double.parseDouble(st.nextToken());
                double inPitchRateDegS = Double.parseDouble(st.nextToken());
                double inRollRateDegS = Double.parseDouble(st.nextToken());
                double inYawRateDegS = Double.parseDouble(st.nextToken());
                double inRollDeg = Double.parseDouble(st.nextToken());
                double inPitchDeg = Double.parseDouble(st.nextToken());
                double inHeadDeg = Double.parseDouble(st.nextToken());
                double inAirspeedKt = Double.parseDouble(st.nextToken());
                double inGroundspeedKt = Double.parseDouble(st.nextToken());
                double inEngineRPM = Double.parseDouble(st.nextToken());          // engines/engine/rpm
                
                // convert accelerations into IMU-g values
                //System.out.println("Acc:" + Ftpss2G * inAccXFtpss + ", " + Ftpss2G * inAccYFtpss + ", " + Ftpss2G * inAccZFtpss);
                int accYSim = limitAcc((int) (Ftpss2G * inAccXFtpss * imu.xAccGain) + imu.xAccNeutral);
                int accXSim = limitAcc((int) (Ftpss2G * inAccYFtpss * imu.yAccGain) + imu.yAccNeutral);
                int accZSim = limitAcc((int) ((1.0 + Ftpss2G * inAccZFtpss) * imu.zAccGain) + imu.zAccNeutral);
                
                String accx = Integer.toHexString(accXSim);
                String accy = Integer.toHexString(accYSim);
                String accz = Integer.toHexString(accZSim);
                
                // convert gyro rates
                int pitchSim = (int) (-Deg2Rad * inPitchRateDegS / imu.pitchGain) + imu.pitchNeutral;
                int rollSim = (int) (-Deg2Rad * inRollRateDegS / imu.rollGain) + imu.rollNeutral;
                int yawSim = (int) (Deg2Rad * inYawRateDegS / imu.yawGain) + imu.yawNeutral;
                
                String pitch = Integer.toHexString((int) pitchSim);
                String roll = Integer.toHexString((int) rollSim);
                String yaw = Integer.toHexString((int) yawSim);
                parent.setIMUValues(pitch, roll, yaw, accy, accx, accz);
            }
            socket.close();
        } catch (IOException e) {
            System.out.println();
            System.err.println("Can't open input stream: write-only");
        } catch (Throwable t) {
            System.out.println();
            t.printStackTrace();
            //Ensure the application exits with an error condition.
        }
        
    }
    
    public void stopPlay()
    {
        stop = true;
    }
    
    private int limitAcc(int val)
    {
        if (val < 0) return 0;
        if (val > 4000) return 4000;
        return val;
    }
    
}