// YagiAntennaExample.java

package emuExample;

import edu.cmu.emulator.*;
import edu.cmu.emulator.world.*;
import java.io.*;
import java.util.*;
import java.lang.Class.*;

public class YagiAntennaExample extends SimpleUserClass {

	protected String			nodeName;
	protected EmuNode			node;
	protected YagiAntenna		antenna;
	protected int 				antennaIndex;
	protected String			orientationMode;
	protected double[][]		newOrientation;
	protected double[]			orientationX;
	protected double[]			orientationY;
	protected double[]			orientationZ;
	protected double[][]		currentOrientation;
	public static final int OM_PARENT = 0;
	public static final int OM_VELOCITY = 1;
	
	
	protected EmuNode getNodeByName(String nodeName) {
		EmuNode	node;
		
		node = world.getNode(nodeName);
		if (node == null) {
			throw new RuntimeException("unknown node: "+ nodeName);
		}
		return node;
	}
	protected void getOrientationFromInput(){
		
		newOrientation[0] = EVector.copyAndNormalize(orientationX);
		newOrientation[1] = EVector.copyAndNormalize(orientationY);
		newOrientation[2] = EVector.crossProduct(newOrientation[0],newOrientation[1]);
		
		if (EVector.magnitude(newOrientation[2]) > 0){
			EVector.normalize(newOrientation[2]);
			}
		else{
			newOrientation[2] = EVector.copy(0.0,0.0,1.0);
		}

	}
	
	protected void setNodeOrientationMode(String newOrientationMode){
	
		if (newOrientationMode.equals("PARENT")){
				node.setOrientationMode(OM_PARENT);
				return;
			}
		if(newOrientationMode.equals("VELOCITY")){
				node.setOrientationMode(OM_VELOCITY);
				return;
			}
		if(newOrientationMode.equals("FIX")){
				getOrientationFromInput();
				node.fixOrientation(newOrientation);
				return;
		}
		
		System.out.println("unrecognized orientationMode" + orientationMode);
		System.out.println("Orientation Mode Unchanged");
		return;
	}
	
	protected void setAntennaOrientationMode(String newOrientationMode){
	
		if (newOrientationMode.equals("PARENT")){
				antenna.setOrientationMode(OM_PARENT);
				return;
			}
		if(newOrientationMode.equals("VELOCITY")){
				antenna.setOrientationMode(OM_VELOCITY);
				return;
			}
		if(newOrientationMode.equals("FIX")){
				getOrientationFromInput();
				antenna.fixOrientation(newOrientation);
				return;
		}
		
		System.out.println("unrecognized orientationMode" + orientationMode);
		System.out.println("Orientation Mode Unchanged");
		return;
	}
	
	protected void printOrientation(){
		System.out.println("X:("+ currentOrientation[0][0] + "," + currentOrientation[0][1]+"," + currentOrientation[0][2]+")");
		System.out.println("Y:("+ currentOrientation[1][0] + "," + currentOrientation[1][1]+"," + currentOrientation[1][2]+")");
		System.out.println("Z:("+ currentOrientation[2][0] + "," + currentOrientation[2][1]+"," + currentOrientation[2][2]+")");
	}
	
	protected void showNodeOrientation(){
		System.out.println("Orientation (" + nodeName +") is:");
		printOrientation();
	}
	
	protected void showAntennaOrientation(){
		System.out.println("Orientation (" + nodeName +",Antenna " + antennaIndex + ")");
		printOrientation();
	}
	
	public void start(String[] args) {
		if (args.length < 2 ) {
			System.out.println("args: <nodeName> <Antenna #> <OrientationMode> <OrientationX>");
			return;
		}
		
		orientationX = new double[EVector.DIMENSIONS];
		orientationY = new double[EVector.DIMENSIONS];
		orientationZ = new double[EVector.DIMENSIONS];
		newOrientation = new double[EVector.DIMENSIONS][EVector.DIMENSIONS];
		currentOrientation = new double[EVector.DIMENSIONS][EVector.DIMENSIONS];
		
		nodeName = args[0];
		node = getNodeByName(nodeName);
		antennaIndex = Integer.parseInt(args[1]);
		System.out.println(node.getAntenna(antennaIndex).getClass().getName());
		antenna = (YagiAntenna)node.getAntenna(antennaIndex);
		
		orientationMode = args[2];
		
		orientationX[0] = Double.parseDouble(args[3]);
		orientationX[1] = Double.parseDouble(args[4]);
		orientationX[2] = Double.parseDouble(args[5]);
//		orientationY[0] = Double.parseDouble(args[6]);
//		orientationY[1] = Double.parseDouble(args[7]);
//		orientationY[2] = Double.parseDouble(args[8]);
//		orientationZ = EVector.crossProduct(orientationX,orientationY);
		orientationX = EVector.copyAndNormalize(orientationX);
		orientationZ = EVector.copy(0.0,0.0,1.0);
		orientationY = EVector.crossProduct(orientationZ,orientationX);
		
		/* you can choose to change the orientation of antenna. or a single node .*/
		//setNodeOrientationMode(orientationMode);
		setAntennaOrientationMode(orientationMode);
		//currentOrientation = node.getAbsOrientation();
		currentOrientation = antenna.getAbsOrientation();
		showAntennaOrientation();
		
		System.out.println("wait for 5 seconds, and switch to orientation 2");
		
		try {
			Thread.sleep(5000);
		} catch (InterruptedException e) {
			e.printStackTrace();
        }


		orientationX[0] = Double.parseDouble(args[6]);
		orientationX[1] = Double.parseDouble(args[7]);
		orientationX[2] = Double.parseDouble(args[8]);
		orientationX = EVector.copyAndNormalize(orientationX);
		orientationZ = EVector.copy(0.0,0.0,1.0);
		orientationY = EVector.crossProduct(orientationZ,orientationX);
		
		/* you can choose to change the orientation of antenna. or a single node .*/
		setAntennaOrientationMode(orientationMode);
		currentOrientation = antenna.getAbsOrientation();
		showAntennaOrientation();
		
		System.out.println("DONE");
	}
	// some interesting stuff could be: spinning the orientation by computing a table and load newOrientation periodically
}