/* 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.
 */

Config cfg;
Gyroscope gyroscope;
Accelerometer accelerometer;
Compass compass;
Barometer barometer;
MotorController motorCtrl;
Receiver receiver;
CopterModel model;
FlightControl flightCtrl;
Kinematics kinematics;

bool loop() {
	try {
		/* get sensor data */
		double gx, gy, gz, ax, ay, az;
		int16 mx, my, mz;
		gyroscope.get_data(out gx, out gy, out gz);
		accelerometer.get_data(out ax, out ay, out az);
		compass.get_data(out mx, out my, out mz);

		/* calculate kinematics (TODO: calculate time diff) */
		kinematics.update(gx, gy, gz, ax, ay, az, mx, my, mz, 0.1);

		/* update motors */
		flightCtrl.process();
	} catch(Error e) {
		stderr.printf("%s\n", e.message);
	}

	return true;
}

int main(string[] args) {
	Environment.set_prgname("microcopterd");

	try {
		cfg = new Config();
		Log.init();

		gyroscope     = cfg.get_gyroscope();
		accelerometer = cfg.get_accelerometer();
		compass       = cfg.get_compass();
		barometer     = cfg.get_barometer();
		motorCtrl     = cfg.get_motor_controller();
		model         = cfg.get_model();
		receiver      = cfg.get_receiver();

		motorCtrl.enable();

		receiver.set_speed.connect((id, speed) => {
			motorCtrl.set_single(id, speed);
		});

		kinematics   = new Kinematics();
		flightCtrl   = new FlightControl();

		Timeout.add(10, loop, Priority.HIGH);
	} catch(Error e) {
		stderr.printf("%s\n", e.message);
		return 1;
	}

	new MainLoop(null, false).run();
	return 0;
}