#pragma config(Sensor, dgtl1,  f1c,            sensorTouch)
#pragma config(Sensor, dgtl2,  f2c,            sensorTouch)
#pragma config(Sensor, dgtl3,  f3c,            sensorTouch)
#pragma config(Sensor, dgtl4, s,              sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  f1i,            sensorLEDtoVCC)
#pragma config(Sensor, dgtl8,  f2i,            sensorLEDtoVCC)
#pragma config(Sensor, dgtl9,  f3i,            sensorLEDtoVCC)
#pragma config(Motor,  port2,  d1,             tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// <configuration>
float K_P            = 5.6;   // Proportional coefficient
float K_I            = 0.001; // Integral coefficient
float K_D            = 0.3;   // Derivative coefficient
int   K_DT           = 250;   // Time difference between PID iterations
float ERROR_THRESH   = 1.1;   // Target threshold
int   FLOOR_HEIGHT   = 9;     // Height of a floor, in CM
int   FLOOR_1_OFFSET = 3;     // Distance of the first floor from the ultrasonic sensor
int   IDLE_TIME      = 10000; // Idle time threshold
// </configuration>

int integral = 0; // PID integral value
int lastError = 0; // Error value in the previous iteration

/**
 * Clamps a value inclusively between two bounds.
 */
int clamp(int n, int lower, int upper);

/**
 * Sets the motor output.
 */
void out(int power);

/**
 * Executes one iteration of the PID algorithm.
 */
void pid(float error);

/**
 * Calculates an error value for a setpoint and measured value.
 */
float error(int expected, int actual);

/**
 * Updates floor indicator LEDs.
 */
void ledUpdate(int floorNum);

/**
 * Instructs the elevator to travel to a level and blocks until the operation is finished.
 */
void goToFloor(int floorNum);

task main() {
	int idleSince = nSysTime; // Store current system time as time of last action
	while (true) {
		if (SensorValue[f1c] == 1) { // Check if floor buttons are pressed
			goToFloor(0); // And go to the respective floor
			idleSince = nSysTime; // Then reset the idle time
		} else if (SensorValue[f2c] == 1) { // Ditto
			goToFloor(1);
			idleSince = nSysTime;
		} if (SensorValue[f3c] == 1) { // Ditto
			goToFloor(2);
			idleSince = nSysTime;
		} else { // If no floor button is pressed
			if (nSysTime - idleSince > IDLE_TIME) // Check if idle time exceeds threshold
				goToFloor(0); // Then go back to the ground floor
		}
	}
}

int clamp(int n, int lower, int upper) {
	return n < lower ? lower : n > upper ? upper : n;
}

void out(int power) {
	if (power < 0) // Threshold the power level so the motor still drives at lower power levels
		power = clamp(power, -96, -18);
	else if (power > 0)
		power = clamp(power, 18, 96);
	motor[d1] = power; // Set motor d1's output to power
}

void pid(float error) {
	integral += error * K_DT; // Add the current error value to the integral cumulator
	float pTerm = K_P * error; // Calculate proportional term
	float iTerm = K_I * integral; // Calculate integral term
	float dTerm = K_D * (error - lastError) / K_DT; // Calculate derivative term
	out(pTerm + iTerm + dTerm); // Set output to clamped value of u(t)
	lastError = error; // Store error value to lastError for calculating derivative later
}

float error(int expected, int actual) {
	return actual - expected; // Difference between setpoint and measured value
}

void ledUpdate(int floorNum) {
	SensorValue[f1i] = SensorValue[f2i] = SensorValue[f3i] = 0; // Turn off all LEDs
	switch (floorNum) { // Switch on the new floor number
		case 0: // If the floor is appropriate
			SensorValue[f1i] = 1; // Turn on the corresponding LED
			break;
		case 1: // Ditto
			SensorValue[f2i] = 1;
			break;
		case 2: // Ditto
			SensorValue[f3i] = 1;
			break;
	}
}

void goToFloor(int floorNum) {
	integral = 0; // Reset integral and last error value variables
	lastError = 0;
	int setPoint = floorNum * FLOOR_HEIGHT + FLOOR_1_OFFSET; // Determine PID setpoint
	int errorValue; // Declare error value variable
	while (true) {
		errorValue = error(setPoint, SensorValue[s]); // Calculate error value
		if (abs(errorValue) <= ERROR_THRESH) // If within the error threshold...
			break; // ...break the PID loop
		pid(errorValue); // Execute PID iteration
		waitInMilliseconds(K_DT); // Wait for next PID iteration
	}
	out(0); // Stop the motors
	ledUpdate(floorNum); // Update the LEDs
}