/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
 * Copyright 2012, Ted Carancho
 *
 * Ported from AeroQuad
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

public class FlightControl {
	MotorController motor;
	FlightMode mode;
	PID[] pid;
	Kinematics k;

	public FlightControl() {
		motor = new AtmostripeMotorController();
		pid = new PID[PIDEntry.length];
		k = new Kinematics(0, 0); // TODO: read compass instead of using 0,0
	}

	/**
	 * calculateFlightError
	 *
	 * Calculate roll/pitch axis error with gyro/accel data to
	 * compute motor command thrust so used command are executed
	 */
	void calculateFlightError(out double roll, out double pitch) {
		roll = 0; pitch = 0;

		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
		}
#if 0
		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
			// 0.0 -> (receiverCommand[AXIS.X] - receiverZero[AXIS.X]) * ATTITUDE_SCALING
			double rollAttitudeCmd   = pid[PIDEntry.ATTITUDE_XAXIS].update(0.1, k.kinematicsAngle[AXIS.X]);
			// 0.0 -> (receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING
			double pitchAttitudeCmd  = pid[PIDEntry.ATTITUDE_YAXIS].update(0.1, k.kinematicsAngle[AXIS.Y]);

			roll   = pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd, gyroRate[AXIS.X]*1.2);
			pitch  = pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroRate[AXIS.Y]*1.2);
		}
#endif
#if 0
			double pitchAttitudeCmd = updatePID((receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING, -kinematicsAngle[AXIS.Y], &PID[ATTITUDE_YAXIS_PID_IDX]);
			roll   = updatePID(rollAttitudeCmd, gyroRate[XAXIS]*1.2, &PID[ATTITUDE_GYRO_XAXIS_PID_IDX]);
			pitch  = updatePID(pitchAttitudeCmd, -gyroRate[YAXIS]*1.2, &PID[ATTITUDE_GYRO_YAXIS_PID_IDX]);
		}
		else {
			roll  = updatePID(getReceiverSIData(XAXIS), gyroRate[XAXIS]*0.8, &PID[RATE_XAXIS_PID_IDX]);
			pitch = updatePID(getReceiverSIData(YAXIS), -gyroRate[YAXIS]*0.8, &PID[RATE_YAXIS_PID_IDX]);
		}
#endif
	}

#if 0
	/**
	 * processThrottleCorrection
	 *
	 * This function will add some throttle imput if the craft is angled
	 * this prevent the craft to loose altitude when angled.
	 * it also add the battery throttle correction in case
	 * of we are in auto-descent.
	 *
	 * Special thank to Ziojo for this.
	 */
	void processThrottleCorrection() {
		int throttleAsjust = throttle / (cos (radians (kinematicsAngle[XAXIS])) * cos (radians (kinematicsAngle[YAXIS])));
		throttleAsjust = constrain ((throttleAsjust - throttle), 0, 160); //compensate max  +/- 25 deg XAXIS or YAXIS or  +/- 18 ( 18(XAXIS) + 18(YAXIS))
		throttle = throttle + throttleAsjust + (int)batteyMonitorThrottleCorrection;

		throttle = constrain(throttle,MINCOMMAND,MAXCOMMAND-150);  // limmit throttle to leave some space for motor correction in max throttle manuever
	}
#endif

	/**
	 * processHeading
	 *
	 * This function will calculate the craft heading correction depending
	 * of the users command. Heading correction is process with the gyro
	 * or a magnetometer
	 */
	private void processHeading() {
		// TODO
	}

	/**
	 * processFlightControl
	 *
	 * Main flight control processos function
	 */
	public void processFlightControl() {
		double roll, pitch;
		calculateFlightError(out roll, out pitch);
		processHeading();

#if 0
		/* 50Hz tasks */
		processAltitudeHold();
		processThrottleCorrection();

		/* if running */
		applyMotorCommand();

		processMinMaxCommand();

		/* Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers */
		//processHardManuevers();

		/* If throttle in minimum position, don't apply yaw */
		if (receiverCommand[THROTTLE] < MINCHECK) {
			for (byte motor = 0; motor < LASTMOTOR; motor++) {
				motorMaxCommand[motor] = minArmedThrottle;
			}
		}

		/* Apply limits to motor commands */
		for (byte motor = 0; motor < LASTMOTOR; motor++) {
			motorCommand[motor] = constrain(motorCommand[motor], motorMinCommand[motor], motorMaxCommand[motor]);
		}

		/* ESC Calibration */
		if (motorArmed == OFF) {
			processCalibrateESC();
		}

		if (motorArmed == ON && safetyCheck == ON) {
			writeMotors();
		}
#endif
	}
}