/*
 *  Copyright (c) 1994, Riley Rainey,  riley@netcon.com
 *
 *  Permission to use, copy, modify and distribute (without charge) this
 *  software, documentation, images, etc. is granted, provided that this 
 *  comment and the author's name is retained.
 *
 *  This software is provided by the author as is, and without any expressed
 *  or implied warranties, including, but not limited to, the implied
 *  warranties of merchantability and fitness for a particular purpose.  In no
 *  event shall the author be liable for any direct, indirect, incidental, or
 *  consequential damages arising in any way out of the use of this software.
 */

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

#define balance_IMPORT
#include "balance.h"

void
balance (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;

}

#ifdef notdef
main()
{

	struct balance_data s;

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

	VSetPoint (s.rn, 14.0, 0, 6.5);
	VSetPoint (s.rm, -1.0, 0, 6.5);

/*
 *  Gross weight
 */

	s.weight = 24326.0;

/*
 *  Maximum oleo extension lengths
 */

	s.cnMax = 1.5;
	s.cmMax = 1.5;

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

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

	s.cm = 1.0;
	s.cn = 1.0;

	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);
}
#endif

