/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

public class Hexacopter : CopterModel {
	int[] min = new int[6];
	int[] max = new int[6];
	int[] cmd = new int[6];

	const int YAW_DIRECTION = 1;

	private enum Position {
		FRONT,
		FRONT_RIGHT,
		REAR_RIGHT,
		REAR,
		REAR_LEFT,
		FRONT_LEFT,
	}

	public override uint8 get_motor_amount() {
		return 6;
	}

	public override void set_max(uint8 motor, int val) {
		max[motor] = val;
	}

	public override int get_max(uint8 motor) {
		return max[motor];
	}

	public override void set_min(uint8 motor, int val) {
		min[motor] = val;
	}

	public override int get_min(uint8 motor) {
		return min[motor];
	}

	public override int[] get_motor_data(int throttle, int motorAxisCommandYaw, int motorAxisCommandRoll, int motorAxisCommandPitch) {
		int throttleCorrection = (motorAxisCommandYaw*3/6).abs();

		cmd[Position.FRONT]       = (throttle - throttleCorrection) + motorAxisCommandRoll/2 - motorAxisCommandPitch/2 - (YAW_DIRECTION * motorAxisCommandYaw);
		cmd[Position.FRONT_RIGHT] = (throttle - throttleCorrection) - motorAxisCommandRoll/2 + motorAxisCommandPitch/2 + (YAW_DIRECTION * motorAxisCommandYaw);
		cmd[Position.REAR_RIGHT]  = (throttle - throttleCorrection) - motorAxisCommandRoll/2 - motorAxisCommandPitch/2 - (YAW_DIRECTION * motorAxisCommandYaw);
		cmd[Position.REAR]        = (throttle - throttleCorrection) + motorAxisCommandRoll/2 + motorAxisCommandPitch/2 + (YAW_DIRECTION * motorAxisCommandYaw);
		cmd[Position.REAR_LEFT]   = (throttle - throttleCorrection)                          - motorAxisCommandPitch   + (YAW_DIRECTION * motorAxisCommandYaw);
		cmd[Position.FRONT_LEFT]  = (throttle - throttleCorrection)                          + motorAxisCommandPitch   - (YAW_DIRECTION * motorAxisCommandYaw);

		return cmd;
	}
}