#include <math.h>
#include <stdio.h>
#include <Vlib.h>

#undef printf

/*
 *   ----------------  z = 0 in body coordinates
 *      ^
 *      |	r{m,n}.z	(constant)
 *      v
 *      o	gear attachment point
 *      ^
 *      |	c{m,n}.z	strut extension  (0 - c{m,n}Max)
 *      v
 *      ^
 *      |	G{m,n}		strut + tire length  (constant)
 *      v
 *   ----------------  ground
 *
 */


struct balance_data {
	double	weight;	/* weight for this test */
	VPoint	rm;	/* rest main gear ground contact point (input),
			   rest main gear attachment point (output) */
	VPoint	rn;	/* rest nose gear ground contact point (input),
			   rest nose gear attachment point (output) */
	double	cm, cn;	/* rest extension values of each strut */
	double	cmMax, cnMax;	/* maximum extension values of each strut */
	double	Gm, Gn; /* strut + tire lengths */
	double	Km, Kn;	/* string constants (output) */
	double  Gpz;	/* the old "grounding point" Z value */
	};

void
balance (s)
struct balance_data *s;
{

	double	theta, cosTheta, sinTheta;
	double	Fmz, Fnz;

/*
 *  Determine the rest pitch angle of the aircraft body
 */

	theta = - atan2 (s->rn.z - s->rm.z, s->rn.x - s->rm.x);
	cosTheta = cos(theta);
	sinTheta = sin(theta);

	printf ("Theta = %f degrees (positive down)\n", theta * 180.0 / M_PI);

/*
 *  Determine correct rm/rn values
 */
	s->rn.z = s->rn.z - s->Gn - s->cn;
	s->rm.z = s->rm.z - s->Gm - s->cm;

/*
 *  Determine spring constants
 */

	Fmz = (s->weight * s->rn.x) / (s->rn.x - s->rm.x);
	Fnz = s->weight - Fmz;

	s->Km = Fmz / (s->cmMax - s->cm);
	s->Kn = Fnz / (s->cnMax - s->cn);

/*
 *  Determine the initial grounding point
 */

	s->Gpz = s->rm.x * sinTheta + (s->rm.z + s->Gm + s->cm) * cosTheta;

}

main()
{

	struct balance_data s;

/*
 *  Wheel contact locations for the aircraft fully loaded at rest.
 */

	VSetPoint (s.rn, 4.1165, 0.0, 2.029);
	VSetPoint (s.rm, -0.3489, 4.0118, 2.0299);

/*
 *  Gross weight
 */

	s.weight = 1450 + 300;

/*
 *  Maximum oleo extension lengths
 */

	s.cnMax = 0.5;
	s.cmMax = 0.5;

/*
 *  The length of the wheel and lower landing gear strut
 */
	s.Gm = 1.0;
	s.Gn = 1.0;

/*
 *  Rest oleo extension; must be less than cnMax or cmMax; usually about
 *  half the max value.
 */

	s.cm = 0.20;
	s.cn = 0.25;

	printf ("Input:\n");
	printf ("nose   contact = %lf  %lf  %lf\n", s.rn.x, s.rn.y, s.rn.z);
	printf ("main's contact = %lf  %lf  %lf\n", s.rm.x, s.rm.y, s.rm.z);
	printf ("Weight = %lf\n", s.weight);

	balance(&s);

	printf ("\nOutput:\n");
	printf ("rm = %lf,  %lf,  %lf\n", s.rm.x, s.rm.y, s.rm.z);
	printf ("rn = %lf,  %lf,  %lf\n", s.rn.x, s.rn.y, s.rn.z);
	printf ("Km = %lf\n", s.Km);
	printf ("Kn = %lf\n", s.Kn);
	printf ("Grounding point (z) = %lf\n", s.Gpz);
	printf ("\n\"inventory\" form:\n\n");
	printf ("\tRm\t\t{%lg,  %lg,  %lg}\n", s.rm.x, s.rm.y, s.rm.z);
	printf ("\tRn\t\t{%lg,  %lg,  %lg}\n", s.rn.x, s.rn.y, s.rn.z);
	printf ("\tKm\t\t%lg\n", s.Km);
	printf ("\tKn\t\t%lg\n", s.Kn);
	printf ("\tGm\t\t%lg\n", s.Gm);
	printf ("\tGn\t\t%lg\n", s.Gn);
	printf ("\tCmMax\t\t%lg\n", s.cmMax);
	printf ("\tCnMax\t\t%lg\n", s.cnMax);
	printf ("\tGroundingPoint\t{0.0, 0.0, %lg}\n", s.Gpz);

	exit (0);
}