import com.ridgesoft.io.Display;
import com.ridgesoft.robotics.PushButton;
import com.ridgesoft.io.I2CMaster;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.I2CEEPROM;
import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.Servo;
import com.ridgesoft.robotics.ShaftEncoder;
import com.ridgesoft.robotics.AnalogInput;

/* Notice This code is not yet working. 
 * It is a snapshot of a work-in-progress rewrite redesign of earlier code.
 *
 * Tom Murphy Intellibrain Circumnavigation Code
 *
 * Stay tuned for a working tested code
 *
 * My personal programming style is to do a whole cloth rewrite of code for my own amusement 
 * since I like the debugging process and the challenge of keeping design in my head.
 *
 * I had hoped by publication this new improved version would be in place. Oooops.
 *
 * Here are design goals of this version
 *
 * Overall goal is to support circumnavigation 
 * of a particular circular hallway in one of the school buildings
 *
 * Secondary goal in support of this is to design reusable code where the classes for each sensor 
 * have a self calibrating capability. I was inspired by the compass I added.
 *
 * Tertiary goal is to have support goal to develop navigational support code, 
 * where I can basically set a declination to the bot, so I can use my old scout compass to define all
 * directional measurements in the building. Further, I can have the bot measure my pace allowing me to 
 * specify a coarse course through the building. As the bot traverses the path, it stop at each waypoint 
 * so I can read off botworld coordinates and measure distance to desired waypoint. 
 * The system will be designed to take either the coarse Murphy polar system or 
 * botworld x-y coordinates for each waypoint, 
 * allowing me  flexibility in migrating to the final circumnavigational solution.
 * 
 * The next rewrite after this will be to weave in the CMU cam 
 * to see if I can have bot auto-locate landmarks in the building
 *
 *		Cheers
 *		... Tom
 *
 * copyright Tom Murphy, except for code copyrighted to RidgeSoft
 *
 */

// know crossed finish line when following equation goes negative (useful if just coming out of evasive manouver)
//      and can then move on to the next point
// (xs-xt)(x-xt) + (ys-yt)(y-yt)
// where (xs,ys) is start point, (xy,yt) is ending point, and (x,y) is current point
// later, want to use a close enough factor based on the needed start of turn to optimize keeping at full speed

// find out conservative time to wait on getting compass results
/*
want to drive by keeping to heading, keep one motor speed fixed and adjust the other to keep to heading, 
this will change depending on heading
*/

/* to set navigation, have bot go forward to its true north, set declination on compass to match, 
pace off distances. Give bot direction and distance. Alter bot so it delays at each waypoint a set # seconds
as it travels it displays coordinates. measure x, y offset to desired point, now have updated
desired coordinates.
*/

/* great if can use remote control to pause bot, another way to read current position */

/* need to create motor stalled thread - for a style of evasive behavior */

/* need cw ccw code as part of establishing position - need another set of sonar, 
can get around it by sampling, turning 90 degrees sampling, turning back sampling(to verify). 
It relies on being able to turn in place
	- calibrate compass
	- measure distance
	- calculate first start and max speed, calculate speeds points in increments of 1/8, 
	- forwards and backwards
	- same with rotational speeds cw and ccw so can rotate in position
	
Abstract this to integer scale heading from compass, +-10000 forward/backward speed, +-10000 rotational speed

*/

// =====================================================================
// ====================== Public Interfaces ============================
// =====================================================================

public interface Navigator {
	public void moveTo(float x, float y, boolean wait);
	public void moveTo(float x, float y, NavigatorListener listener);
	public void turnTo(float heading, boolean wait);
	public void turnTo(float heading, NavigatorListener listener);
	public void go(float heading);
	public void stop();
}

public interface NavigatorListener {
	public void navigationOperationTerminated(boolean completed);
}

public interface DirectionListener {
	public void navigationOperationTerminated(boolean completed);
}

public interface Localizer {
	public Pose getPose();
	public void setPose(Pose pose);
	public void setPosition(float x, float y);
	public void setHeading(float heading);
}

public interface Compass {
	public void calibrate();
	public void heading();
}

// =====================================================================
// ====================== Main Code ====================================
// =====================================================================

public class MyBot {
	public static void main(String args[]) {

		try {
		// make runManager handle all this
		// and use Screen manager to select which function to run
		// create button watcher thread to know when start is pressed and released
		
		/*
		AnalogInput leftWheelInput = IntelliBrain.getAnalogInput(4);
		AnalogInput rightWheelInput = IntelliBrain.getAnalogInput(5);
		*/
		
		/*
		ShaftEncoder leftEncoder = new AnalogShaftEncoder(
			leftWheelInput, 250, 750, 30, Thread.MAX_PRIORITY);
		ShaftEncoder rightEncoder = new AnalogShaftEncoder(
			rightWheelInput, 250, 750, 30, Thread.MAX_PRIORITY);
		*/
		
		// want to add in remote controller to see if can change displays on the fly
			Display display = IntelliBrain.getLcdDisplay();
			PushButton startButton = IntelliBrain.getStartButton();
			PushButton stopButton = IntelliBrain.getStopButton();
			Runnable functions[] = new Runnable[] {
				new DoInitCompass(),
				new DoInitXY(),
				new DoTuneConst(),
				new DoTakeTrip(),
			};
			startButton.waitReleased();
			IntelliBrain.setTerminateOnStop(false);
			int selectedFunction = 0;
			display.print(0, "Function");
			display.print(1, functions[selectedFunction].toString());
			while (!startButton.isPressed()) {
				if (stopButton.isPressed()) {
					if (++selectedFunction >= functions.length)
						selectedFunction = 0;
					display.print(1, functions[selectedFunction].toString());
					stopButton.waitReleased();
				}
			}
			IntelliBrain.setTerminateOnStop(true);
			Screen[] screens = new Screen[] {
				new StaticTextScreen("MyBot", "Version 0.3"),
				new StaticTextScreen("Screen 1", "abcd"),
				new StaticTextScreen("Screen 2", "1234"),
			};
			new ScreenManager(display, screens, IntelliBrain.getThumbWheel(), Thread.MIN_PRIORITY, 500);
			functions[selectedFunction].run();
		}
		catch (Throwable t)
			t.printStackTrace();
	}
}

public class DoInitCompass implements Runnable {
	public void run() {
		Compass compass(75,40000);
	}
	public String toString() {
		return "Initialize Compass";
	}
}

public class DoInitXY implements Runnable {
	private Compass compass;
	// snap current heading
	// start move to heading
	//stop after x seconds and report left and right wheel counts and heading
	
	public void run() {
	}
	public String toString() {
		return "Initialize XY";
	}
}

public class DoTuneConst implements Runnable {
	private Compass compass;
	// run multiple cw and ccw runs (self-calibrate against special corner), 
	// tune wheel diameters and wheel base
	
	public void run() {
	}
	public String toString() {
		return "Tune Constants";
	}
}

public class DoTakeTrip implements Runnable {
	private Compass compass;
	// store sequence of heading/distance pairs that constitute trip
		// calculate coordinates
		// from this calculate heading and number of seconds to travel
		// this sets up for doing collision detection, where have to recalculate heading and # secs to travel
		// done with pair if cross infinitely wide finish line
	public void run() {
	}
	public String toString() {
		return "Take a Trip";
	}
}

// =====================================================================
// ====================== Screen Manager Code ==========================
// =====================================================================

public class ScreenManager extends Thread {
// not clear how functions can use this, be good if had effective screens being used and then allow it to return a screen to write
//		to, which can be given to sub tasks/threads
	private Display mDisplay;
	private Screen[] mScreens;
	private AnalogInput mUserInput;
	private int mPeriod;

	public ScreenManager(Display display, Screen[] screens, AnalogInput scrollDevice, int threadPriority, int period) {
		mDisplay = display;
		mScreens = screens;
		mUserInput = scrollDevice;
		mPeriod = period;
		setPriority(threadPriority);
		start();
	}

	public void run() {
		try {
			int divisor = mUserInput.getMaximum() + 1;
			while (true) {
				try {
					int index = (mUserInput.sample() * mScreens.length) / divisor;
					mScreens[index].update(mDisplay);
				} 
				catch (Exception e)
					e.printStackTrace();
				Thread.sleep(mPeriod);
			}
		} catch (Throwable t)
			t.printStackTrace();
	}
}

public class WheelSensorScreen implements Screen {
// put this in display list
// new WheelSensorScreen(leftWheelInput, rightWheelInput),
	private AnalogInput mLeftWheelInput;
	private AnalogInput mRightWheelInput;
	
	public WheelSensorScreen(AnalogInput leftWheelInput, AnalogInput rightWheelInput) {
		mLeftWheelInput = leftWheelInput;
		mRightWheelInput = rightWheelInput;
	}
	
	public void update(Display display) {
		display.print(0, "L Wheel: " + mLeftWheelInput.sample());
		display.print(1, "R Wheel: " + mRightWheelInput.sample());
	}
}

public class EncoderCountsScreen implements Screen {
// put this in display list
// new EncoderCountsScreen(leftEncoder, rightEncoder),
	private ShaftEncoder mLeftEncoder;
	private ShaftEncoder mRightEncoder;
	
	public EncoderCountsScreen(ShaftEncoder leftEncoder, ShaftEncoder rightEncoder) {
		mLeftEncoder = leftEncoder;
		mRightEncoder = rightEncoder;
	}
	
	public void update(Display display) {
		int leftCounts = mLeftEncoder.getCounts();
		int rightCounts = mRightEncoder.getCounts();
		display.print(0, "L enc: " + leftCounts);
		display.print(1, "R enc: " + rightCounts);
	}
}

// =====================================================================
// ====================== Public Interfaces ============================
// =====================================================================

// =====================================================================
// ====================== Servo ============================
// =====================================================================

public class ContinuousRotationServo implements Motor {
	private Servo mServo;
	private boolean mReverse;
	private int mRange;
	private boolean mIsForward;
	private int mPower;
	private DirectionListener mDirectionListener;
	
	public ContinuousRotationServo(Servo servo, boolean reverse, int range) {
		mServo = servo;
		mReverse = reverse;
		mRange = range;
		mDirectionListener = null;
		mPower = 0;
	}
	public ContinuousRotationServo(Servo servo, boolean reverse, int range, DirectionListener directionListener) {
		ContinuousRotationServo(servo, reverse, range);
		mDirectionListener = directionListener;
	}
	public void setDirectionListener(
		DirectionListener directionListener) {
		mDirectionListener = directionListener;
	}
	public void brake() {
		// servos don't provide braking, just turn the power off
		setPower(Motor.STOP);
	}
	public void setPower(int power) {
		if (mPower == power) return;
		if (mPower == 0) mIsForward = power < 0;
		mPower = power;
		if (power == 0) {
			mServo.off();
			return;
		}
		
//		if ((mDirectionListener != null) && (power != 0))
		if (mDirectionListener) {
			if (mIsForward != (power<0)) {
				mIsForward = !mIsForward;
				mDirectionListener.updateDirection(mIsForward);
			}
		}
		
		if (mReverse) power = -power;
		power = power > Motor.MAX_FORWARD ? Motor.MAX_FORWARD : 
			(power < Motor.MAX_REVERSE ? Motor.MAX_REVERSE : power);
		mServo.setPosition((power * mRange) / Motor.MAX_FORWARD + 50);
	}
}

// =====================================================================
// ====================== Navigation Code ============================
// =====================================================================

public class DifferentialDriveNavigator extends Thread implements Navigator {
	private static final float PI = 3.14159f; // see if can use arccos
	private static final float TWO_PI = PI * 2.0f;
	private static final float PI_OVER_2 = PI / 2.0f;
	private static final int STOP = 0;
	private static final int GO = 1;
	private static final int MOVE_TO = 2;
	private static final int ROTATE = 3;
	private Motor mLeftMotor;
	private Motor mRightMotor;
	private Localizer mLocalizer;
	private int mDrivePower;
	private int mRotatePower;
	private int mPeriod;
	private int mState;
	private float mDestinationX;
	private float mDestinationY;
	private float mTargetHeading;
	private float mGain;
	private float mGoToThreshold;
	private float mRotateThreshold;
	private NavigatorListener mListener;	
	
	public DifferentialDriveNavigator(
		Motor leftMotor, Motor rightMotor,
		Localizer localizer
		int drivePower, int rotatePower,
		float gain,
		float goToThreshold, float rotateThreshold,
		int threadPriority, int period) {
		mLeftMotor = leftMotor;
		mRightMotor = rightMotor;
		mLocalizer = localizer;
		mDrivePower = drivePower;
		mRotatePower = rotatePower;
		mGain = gain;
		mGoToThreshold = goToThreshold;
		mRotateThreshold = rotateThreshold;
		mPeriod = period;
		mState = STOP;
		mListener = null;
		setPriority(threadPriority);
		setDaemon(true);
		start();
	}
	private void updateListener(boolean completed, NavigatorListener newListener) {
		if (mListener != null)
			mListener.navigationOperationTerminated(completed);
		mListener = newListener;
	}
	private float normalizeAngle(float angle) {
		while (angle < -PI)
		angle += TWO_PI;
		while (angle > PI)
		angle -= TWO_PI;
		return angle;
	}
	
	public void run() {
		try {
			while (true) {
				switch (mState) {
				case MOVE_TO:
					goToPoint();
					break;
				case GO:
					goHeading();
					break;
				case ROTATE:
					doRotate();
					break;
				default: // stopped
					break;
				}
				Thread.sleep(mPeriod);
			}
		}
		catch (Throwable t)
			t.printStackTrace();
	}
	private synchronized void goToPoint() {
		Pose pose = mLocalizer.getPose();
		float xError = mDestinationX - pose.x;
		float yError = mDestinationY - pose.y;
		float absXError = (xError > 0.0f) ? xError : -xError;
		float absYError = (yError > 0.0f) ? yError : -yError;
		if ((absXError + absYError) < mGoToThreshold) {
			// stop
			mLeftMotor.setPower(Motor.STOP);
			mRightMotor.setPower(Motor.STOP);
			mState = STOP;
			// notify listener the operation is complete
			updateListener(true, null);
			// signal waiting thread we are at the destination
			notify();
		} else {
			// adjust heading and go that way
			mTargetHeading = (float)Math.atan2(yError, xError);
			goHeading();
		}
	}
	private synchronized void goHeading() {
		Pose pose = mLocalizer.getPose();
		float error = mTargetHeading - pose.heading;
		error += error > PI ? -TWO_PI : (error < -PI ? TWO_PI : 0);
		int differential = (int)(mGain * error + 0.5f);
		mLeftMotor.setPower(mDrivePower - differential);
		mRightMotor.setPower(mDrivePower + differential);
	}
	private synchronized void doRotate() {
		Pose pose = mLocalizer.getPose();
		float error = mTargetHeading - pose.heading;
		// choose the direction of rotation that results
		// in the smallest angle
		error += error > PI ? -TWO_PI : (error < -PI ? TWO_PI : 0);
		float absError = (error >= 0.0f) ? error : -error;
		if (absError < mRotateThreshold) {
			mLeftMotor.setPower(Motor.STOP);
			mRightMotor.setPower(Motor.STOP);
			mState = STOP;
			// notify listener the operation is complete
			updateListener(true, null);
			// signal waiting thread we are at the destination
			notify();
		} else if (error > 0.0f) {
			mLeftMotor.setPower(-mRotatePower);
			mRightMotor.setPower(mRotatePower);
		} else {
			mLeftMotor.setPower(mRotatePower);
			mRightMotor.setPower(-mRotatePower);
		}
	}
	private synchronized void moveTo(float x, float y, boolean wait, NavigatorListener listener) {
		updateListener(false, listener);
		mDestinationX = x;
		mDestinationY = y;
		mState = MOVE_TO;
		if (wait) {
			try {
				wait();
			}
			catch (InterruptedException e);
		}
	}
	public void moveTo(float x, float y, boolean wait) {
		moveTo(x, y, wait, null);
	}
	public void moveTo(float x, float y,
		NavigatorListener listener) {
		moveTo(x, y, false, listener);
	}
	public synchronized void turnTo(float heading, boolean wait, NavigatorListener listener) {
		updateListener(false, listener);
		mTargetHeading = normalizeAngle(heading);
		mState = ROTATE;
		if (wait) {
			try {
				wait();
			}
			catch (InterruptedException e);
		}
	}
	public void turnTo(float heading, boolean wait) {
		turnTo(heading, wait, null);
	}
	public void turnTo(float heading, NavigatorListener listener) {
		turnTo(heading, false, listener);
	}
	public synchronized void go(float heading) {
		mTargetHeading = normalizeAngle(heading);
		mState = GO;
		updateListener(false, null);
	}
	public synchronized void stop() {
		mLeftMotor.setPower(Motor.STOP);
		mRightMotor.setPower(Motor.STOP);
		mState = STOP;
		updateListener(false, null);
	}
	public class Pose {
		public final float x;
		public final float y;
		public final float heading;
		
		public Pose(float x, float y, float heading) {
			this.x = x;
			this.y = y;
			this.heading = heading;
		}
	}
	public class OdometricLocalizer extends Thread implements Localizer {
		private static final float PI = 3.14159f;
		private static final float TWO_PI = PI * 2.0f;
		private ShaftEncoder mLeftEncoder;
		private ShaftEncoder mRightEncoder;
		private float mDistancePerCount;
		private float mRadiansPerCount;
		private float mX = 0.0f;
		private float mY = 0.0f;
		private float mHeading = 0.0f;
		private int mPeriod;
		private int mPreviousLeftCounts;
		private int mPreviousRightCounts;
	
		public OdometricLocalizer(ShaftEncoder leftEncoder, ShaftEncoder rightEncoder, float wheelDiameter, 
			float trackWidth, int countsPerRevolution, int threadPriority, int period) {
			mLeftEncoder = leftEncoder;
			mRightEncoder = rightEncoder;
			mDistancePerCount = (PI * wheelDiameter)
			/ (float)countsPerRevolution;
			mRadiansPerCount = PI * (wheelDiameter / trackWidth)
			/ countsPerRevolution;
			mPeriod = period;
			mPreviousLeftCounts = leftEncoder.getCounts();
			mPreviousRightCounts = rightEncoder.getCounts();
			setDaemon(true);
			setPriority(threadPriority);
			start();
		}
		public synchronized void setPose(Pose pose) {
			mX = pose.x;
			mY = pose.y;
			mHeading = pose.heading;
		}
		public synchronized void setPose(float x, float y, float heading) {
			mX = x;
			mY = y;
			mHeading = heading;
		}
		public synchronized void setPosition(float x, float y) {
			mX = x;
			mY = y;
		}
		public synchronized void setHeading(float heading) {
			mHeading = heading;
		}
		public synchronized Pose getPose() {
			return new Pose(mX, mY, mHeading);
		}
	}
}

// =====================================================================
// ====================== Compass ============================
// =====================================================================

public class CMP2X extends Thread implements Compass {
	private  I2CMaster mMaster;
	private  I2CEEPROM mEEprom;
	private  int mSpeed;
	private  int mInitTime;
	private  int mHeading;
	CMP2X(int speed, int initTime) {
        try {
			synchronized(mEEprom) {
				mSpeed = speed;
				mInitTime = initTime;
				mMaster = IntelliBrain.getI2CMaster();
				mEEprom = new I2CEEPROM(mMaster, 0xE0, 12, 200); // may need to set lower and manage differently
			}
		}
		catch (Throwable t)
			t.printStackTrace();
	}
	CMP2X() {
		this.(0, 0);
	}

	private void setHeading {
		mEEprom.write(0, 'I');
		Thread.sleep(200);
		while (eeprom.readByte(0) == 0xFF) {
			Thread.sleep(50);
		}
		synchronized(mHeading)
			mHeading = mEEprom.readByte(2)<<8 + mEEprom.readByte(1);
	}
	
	public void run() {
		try
			while (true)
				synchronized(mEEprom)
					setHeading();
		catch (Throwable t)
			t.printStackTrace();
	}

	public int calibrate() { // need to figure out syncronized use of servos
		try {
			if (mSpeed == 0 || mInitTime == 0) return;
			Servo leftServo = IntelliBrain.getservo(1);
			synchronized(mEEprom) {
				mEEprom.write(0, 'C'); // Start Calibration
				leftServo.setPosition(mSpeed);
				Thread.sleep(mInitTime);
				leftServo.off();
				setHeading();
			}			
		}
		catch (Throwable t)
			t.printStackTrace();
	}
	
	public int heading() {
		return mHeading;
	}
}

// =====================================================================
// ====================== Encoder  ============================
// =====================================================================

public class AnalogShaftEncoder extends Thread implements ShaftEncoder, DirectionListener {
	private AnalogInput mInput;
	private int mLowThreshold;
	private int mHighThreshold;
	private boolean mIsForward;
	private int mCounts, mLastCount;
	private int mPeriod;
	private int mRatePeriods, mRateMax;
	private float mRate, mPerSec;
	
	public AnalogShaftEncoder(AnalogInput input, int lowThreshold, int highThreshold, int period, int threadPriority) {
		mInput = input;
		mLowThreshold = lowThreshold;
		mHighThreshold = highThreshold;
		mPeriod = period;
		mIsForward = true;
		mCounts = 0;
		mLastCount = 0;
		mRateMax = 1 + 1000/mPeriod;
		mRatePeriods = 0;
		mPerSec = 1000.0/(mRateMax*mPeriod)
		setPriority(threadPriority);
		setDaemon(true);
		start();
	}

	public void run() {
		try {
			// take initial sensor sample
			boolean wasHigh = mInput.sample() > mLowThreshold;
			while (true) {
				int value = mInput.sample();
				if ((wasHigh && (value < mLowThreshold)) || (!wasHigh && (value > mHighThreshold))) {
					mCounts += mIsForward ? 1 : -1;
					wasHigh = !wasHigh;
				}
				if (++mRatePeriods >= mRateMax) {
					mRate = mPerSec*(mCounts-mLastCount);
					mLastCount = mCounts;
					mRatePeriods = 0;
				}
				Thread.sleep(mPeriod);
			}
		}
		catch (Throwable t) {
			t.printStackTrace();
		}
	}

	public void updateDirection(boolean isForward) {
		mIsForward = isForward;
	}
	
	public float getRate() {
		return mRate;
	}
	public int getCounts() {
		return mCounts;
	}
}