import lejos.nxt.*;
import lejos.util.*;
import java.lang.*;

public class segway {
    private final boolean DEBUG = true;
    
	// shared variables for interthread communication
    private volatile int angular_velocity = 0;
    private volatile double angle = 0;
    
    public class MotorThread extends Thread {
		// PID control scheme constants
		double kp = .6;
		double ki = 0.;
		double kd = 0.1;


        @Override
        public void run() {
            setDaemon(true);

			double old_error = 0., int_error = 0.;

            System.out.println("Starting MotorThread");

			// the angle, at which the robot is stable
			final double setpoint = -0.047;

            while (true) {
				// multiplier for the angle and angular velocity
				double c_angular_velocity = -3.14;
				double c_angle = -44.25 * (180./3.14);

				// only the angle of the robot is used in this version
                double error = angle * c_angle - setpoint;
					//c_angular_velocity * angular_velocity;

				// compute the derivative of the error
				double error_dt = error - old_error;
				old_error = error;

				// update the integral of the error term
				int_error += error;  // suspectible to integral windup

				double new_speed = error * kp + error_dt * kd + int_error * ki;
				
                move((int) new_speed);

                Delay.msDelay(30);
            }
        }

        private void move(int speed) {
            Motor.A.setSpeed(Math.abs(speed));
            Motor.C.setSpeed(Math.abs(speed));

            if (speed > 0) {
                Motor.A.forward();
                Motor.C.forward();
            } else {
                Motor.A.backward();
                Motor.C.backward();
            }
        }
    }

    public class SensorThread extends Thread {
        private void init_adxl312(I2CSensor adxl312) {
            byte[] buf = new byte[10];

            // initialization according the adxl312 Quick start guide, page 3
            buf[0] = 0x0b;  // +-12g, 13-bit mode
            adxl312.sendData(0x31, buf, 1);

            buf[0] = 0x08;  // start measurement
            adxl312.sendData(0x2d, buf, 1);

            buf[0] = (byte) 0x80;  // enable drdy init
            adxl312.sendData(0x2e, buf, 1);

        }

        @Override
        public void run() {
            setDaemon(true);

            System.out.println("Starting SensorThread");

            byte[] buf = new byte[10];

            short accel_ax = 0, accel_ay = 0, accel_bx = 0, accel_by = 0;
            int ret_a = 0, ret_b = 0;

            int retries = 0;

            // the top sensor is 'a'
            I2CSensor a = new I2CSensor(SensorPort.S1, 0x53 << 1,
						SensorConstants.MODE_RAW, SensorConstants.TYPE_HISPEED);
            I2CSensor b = new I2CSensor(SensorPort.S1, 0x1d << 1, 
						SensorConstants.MODE_RAW, SensorConstants.TYPE_HISPEED);

            init_adxl312(a);
            init_adxl312(b);

            double angle_a = 0., angle_b = 0.;
            
            while (true) {
                Delay.msDelay(20);

                retries = 0;

                while (ret_a != 0 || ret_b != 0 || retries == 0) {

                    ret_a = a.getData(0x32, buf, 4);
                    accel_ax = (short) ((buf[0] & 0xff) | (buf[1] << 8));
                    accel_ay = (short) ((buf[2] & 0xff) | (buf[3] << 8));
                    
                    
                    ret_b = b.getData(0x32, buf, 4);
                    accel_bx = (short) ((buf[0] & 0xff) | (buf[1] << 8));
                    accel_by = (short) ((buf[2] & 0xff) | (buf[3] << 8));
                    
                    // try to reinitialize the sensor when the communication
					// fails
                    if (retries++ > 10) {
                        System.out.println("Reinitializing sensors");

                        init_adxl312(a);
                        init_adxl312(b);

                        Delay.msDelay(2);
                    }
                }
                
                angle_a = Math.atan2(accel_ax, -accel_ay);
                angle_b = Math.atan2(accel_bx, -accel_by);
                
                setAngle(angle_b);
                setAngularVelocity(accel_ax - accel_bx);

                if(DEBUG) {
                    System.out.println("ax: " + Integer.toString(accel_ax));
                    System.out.println("ay: " + Integer.toString(accel_ay));
                    System.out.println("angle a " + Double.toString(angle_a));
                    System.out.println("angle b " + Double.toString(angle_b));
                }
            }
        }
        
		// TODO try to filter the data
        private void setAngle(double a) {
            angle = a;
        }
        
        private void setAngularVelocity(int v) {
            angular_velocity = v;
        }
    }

    public void run() {
        System.out.println("Press enter to start");

        Button.ENTER.waitForPressAndRelease();

        SensorThread st = new SensorThread();
        MotorThread mt = new MotorThread();

        st.start();

        Delay.msDelay(50);  // wait for the sensors to settle

        mt.start();

        Button.ENTER.waitForPressAndRelease();

        System.out.println("Exiting");
    }

    public static void main(String[] args) {
        segway s = new segway();
        s.run();
    }
}