// This is the Geometric Clutch model version 3 as published in Cell Motility
// and the Cytoskeleton 52:242, 2002. It is the copy with the force-velocity
// relationship shown in figure 2d of the paper, that gave the best result
// with outer arm deactivation.
// It is the specific adaptation used to compose figure 3d and h.
// It uses both inner and outer arm force to calculate dynein adhesion.
// It was written in stages from 1998-2001 and is based on Version 2 published
// in 1994 in Cell Motil. Cytoskeleton 29:141 (also called ModelC.cpp).
// It is the intellectual property of Charles B. Lindemann and Oakland University
// Rochester, Mich. 48309.
// Developed with support from the National Science Foundation.

#include <iostream.h>
#include <graphics.h>
#include <fstream.h>
#include <stdlib.h>
#include <stdio.h>
#include <conio.h>
#include <ctype.h>
#include <math.h>
#include <time.h>

FILE *printer; // pointer for printer applications
// ****************************************************
// These are the declared global constants, variables and arrays
const float pi = 3.1415926;
int ROUTE=1;// intial and continuation routing
int PL, N, Q, EX, total=0;//plotting data from previous run
double sx[32];//position coordinates x and y
double sy[32];
double G[50] [31];//storage arrays for plotting a run
double K[50] [31];
double data[31][2];//storage for output of data
double ang [41];// angles for 30 segments 1 to 30
double sang[31];//shear angles
double cur[31];// curvature values
double param[16];//MODELING PARAMETERS
char comm1[20];//globally accessible identifier for print outs
int graphdriver = DETECT, graphmode;// setup for graphics

// ****************************************************
// this programs specific prototypes are as follows:
void cls(void);
void enter_angles(void);
void load_angles(void);
void init_param(void);
void load_cilium(void);
void print_param(void);
double find_angle(double, double, double);
double find_force(int, double);
void run_model(void);
void data_output(void);
void graph_output(void);
int experiment_mod(void);
// ****************************************************
main()
{
char menu;
char deflt;
start : ;
cls();
cout << "Welcome to the C version of the Geometric Clutch model \n";
cout << "It is based on version draft 8.0 in Qbasic.\n";
cout << "and also has the NEWHEART clutch routine\n\n\n";
cout << "A) INITIALIZE MODELING PARAMETERS (FRESH START).\n";
cout << "B) LOAD STARTING POSITION.\n";
cout << "C) RUN MODEL.\n";
cout << "D) ENTER STARTING ANGLE FILE. \n";
cout << "E) OUTPUT DATA TO PRINTER.\n";
cout << "F) PRINT OUT MODELING PARAMETERS.\n";
cout << "G) PLOT LAST MODEL RUN.\n";
cout << "Q) EXIT FROM PROGRAM.\n\n\n";
cout << "\t Press the letter of the desired selection: ";
menu = toupper(getch());
cout << '\n';

switch (menu)
{
	case 'A':goto first;
	case 'B':goto second;
	case 'C':goto third;
	case 'D':goto fourth;
	case 'E':goto fifth;
	case 'F':goto sixth;
	case 'G':goto seventh;
	case 'Q': return 0;
	default :goto start;
}

first  : cls();
ROUTE = 1;
total=PL=N=Q=0;
cout << "Do you want to use the cilium default settings? (if yes type Y): ";
deflt=toupper(getch());
if (deflt=='Y') { load_cilium(); goto start; }
init_param();
goto start;
second : cls();
load_angles();
goto start;
third  : cls();
run_model();
goto start;
fourth : cls();
enter_angles();
cout << "angle file stored" << endl;
getch();
goto start;
fifth  : cls();
data_output();
cout << "SENT TO PRINTER:"<< endl;
getch();
goto start;
sixth  : cls();
print_param();
cout << "SENT TO PRINTER:"<< endl;
getch();
goto start;
seventh : cls();
graph_output();
goto start;
}
//clear screen function ************************
void cls(void)
{
// the next four lines actually clear the screen and return the cursor
int i=1;
for (i=1; i<26; i++)
{ cout << '\n';
}
gotoxy (1,1);// A function from conio.h that works dandy
return;
}
//**********************************************
//enter a startup angle file function
void enter_angles(void)
{
float angles[31];// define local variables
char fname[12];
cout << "This creates an initial position file of 30 angles in radians.\n";
cout << "Give the file a name: ";
cin >> fname;

for (int i=1; i<31; i++)
	{
	cout << '\n' << i << ": ";
	cin >> angles[i];
	}

ofstream outAngleFile(fname, ios::out);
if (!outAngleFile) {
	cerr << "File could not be opened" << endl;
	exit (1); //for this you must have stdlib.h
	}

for (int j=1; j<31; j++)
	outAngleFile << angles[j] << endl;
outAngleFile.close();
}
//***********************************************
// Load angles from storage and compute starting position
void load_angles(void)
{
 double Z = param[10];
 char fname[12];
cout << "This subroutine loads initial angles from file storage.\n";
cout << "What is the name of the file: ";
cin >> fname;
ifstream inAngleFile(fname, ios::in);
if (!inAngleFile) {
	cerr << "File could not be opened." << endl;
	exit(1);
	}
for (int i=1; i<31; i++) {
	inAngleFile >> ang[i];
	cout << i << " "<< ang[i] << endl;
	}
inAngleFile.close();
// process angles into tail curve
// segment length Z is a modeling parameter
sx[1]=0.0;
sy[1]=0.0;
sang[1]=0.0;
for (int j=1; j<31; j++) {
	sx[j+1] = sx[j] +(Z * cos(ang[j]));
	sy[j+1] = sy[j] +(Z * sin(ang[j]));
	cout << sx[j] <<"  " << sy[j] << "  " << ang[j] << endl;
	}
for ( i=2 ; i<31 ; i++){
	cur[i] = (ang[i]-ang[i-1]) / Z;//calculates the curvatures
	sang[i]=ang[i] - ang[1];
	cout << i << "  " << cur[i] << endl;
	}
getch();

}// end of load angles function
 //***************************************************
 //subroutine function to initialize parameters or
 // to change parameters as needed
 void init_param (void)
{
int i;
char choice;
char menu;

ifstream inParamFile("BROK.DAT2", ios::in);
if (!inParamFile) {
	cerr << "File could not be opened." << endl;
	cout << "YOU MUST CREATE A FILE TO RUN THE PROGRAM" << endl;
	cout << "press any key to continue:" << endl;
	getch();
	goto cont;
	}
for ( i=1; i<16; i++)
	inParamFile >> param[i];
cont :inParamFile.close();
start :cls();

cout << "\tMODELING PARAMETERS" << endl;
cout << "\n";
cout << "A) COUPLING= "<< param[1] << endl;
cout << "B) BRIDGEFORCE= "<< param[2] << endl;
cout << "C) SNAP= "<< param[3] << endl;
cout << "D) SWITCH.P=" << param[4] << endl;
cout << "E) SWITCH.R=" << param[5] << endl;
cout << "F) HISTER.P=" << param[6] << endl;
cout << "G) HISTER.R=" << param[7] << endl;
cout << "H) IE= "<< param[8] << endl;
cout << "I) DIAM= " << param[9] << endl;
cout << "J) Z= " << param[10] << endl;
cout << "K) DRAG= " << param[11] << endl;
cout << "L) GAIN= " << param[12] << endl;
cout << "M) ITS= " << param[13] << endl;
cout << "N) STATE= " << param[14] << endl;
cout << "O) BASE= " << param[15] << endl;
cout << "\nTO RETURN TO THE MAIN MENU PRESS X and (ENTER)" << endl;
cout << "\nWHICH PARAMETER DO YOU WANT TO ENTER(type the letter): " ;
cin >> choice;
menu = toupper(choice);
switch (menu)
	{
	case 'A':cout << "enter COUPLING(zero to 1):" ;
	cin >> param[1]; goto start;
	case 'B':cout << "enter BRIDGFORCE(must be pos.):" ;
	cin >> param[2]; goto start;
	case 'C':cout << "enter NEXIN ELASTICITY SNAP(must be pos.):";
	cin >> param[3]; goto start;
	case 'D':cout << "enter P BRIDGE SWITCPOINT(must be positve):";
	cin >> param[4]; goto start;
	case 'E':cout << "enter R BRIDGE SWITCHPOINT(must be pos.):";
	cin >> param[5]; goto start;
	case 'F':cout << "enter COEFFICIENT OF HISTERESIS(must be pos.):";
	cin >> param[6]; goto start;
	case 'G':cout << "enter COEFFICIENT OF HISTERESIS.R(must be pos.):";
	cin >> param[7]; goto start;
	case 'H':cout << "enter STIFFNESS IE(must be pos.):";
	cin >> param[8]; goto start;
	case 'I':cout << "enter WORKING DIAMETER(must be positve):";
	cin >> param[9]; goto start;
	case 'J':cout << "enter SEGMENT LENGTH Z(must be pos.):";
	cin >> param[10]; goto start;
	case 'K':cout << "enter DRAG COEFICIENT D(must be pos.):";
	cin >> param[11]; goto start;
	case 'L':cout << "enter GAIN(must be pos.):";
	cin >> param[12]; goto start;
	case 'M':cout << "enter ITERATION TIME in SECONDS (ITS):";
	cin >> param[13]; goto start;
	case 'N':cout << "enter STATE(must be 1=active or 0=passive):";
	cin >> param[14]; goto start;
	case 'O':cout << "enter BASE DRAG(between 0=pivoting and 1=fixed):";
	cin >> param[15]; goto start;

	default :break;
	}
ofstream outParamFile("BROK.DAT2", ios::out);
if (!outParamFile) {
	cerr << "File could not be opened" << endl;
	exit (1); //for this you must have stdlib.h
	}
for (int j=1; j<16; j++)
	outParamFile << param[j] << endl;
outParamFile.close();
}
//***********************************************
// Load default settings for a ten micron cilium
void load_cilium(void)
{
cout << "\nTHESE ARE THE DEFAULT SETTINGS FOR A 10 micron CILIUM: " << endl;
param[1]=.2;
param[2]=1.0e-07;
param[3]=.14;
param[4]=.06;
param[5]=.005;
param[6]=26000.;
param[7]=26000.;
param[8]=1.3e-13;
param[9]=1.0e-05;
param[10]=3.3e-05;
param[11]=0.028;
param[12]=9000.;
param[13]=.0001;
param[14]=1.0;
param[15]=1.0;
cout << "\tMODELING PARAMETERS" << endl;
cout << "\n";
cout << "A) COUPLING= "<< param[1] << endl;
cout << "B) BRIDGEFORCE= "<< param[2] << endl;
cout << "C) SNAP= "<< param[3] << endl;
cout << "D) SWITCH.P=" << param[4] << endl;
cout << "E) SWITCH.R=" << param[5] << endl;
cout << "F) HISTER.P=" << param[6] << endl;
cout << "G) HISTER.R=" << param[7] << endl;
cout << "H) IE= "<< param[8] << endl;
cout << "I) DIAM= " << param[9] << endl;
cout << "J) Z= " << param[10] << endl;
cout << "K) DRAG= " << param[11] << endl;
cout << "L) GAIN= " << param[12] << endl;
cout << "M) ITS= " << param[13] << endl;
cout << "N) STATE= " << param[14] << endl;
cout << "O) BASE= " << param[15] << endl;
getch();
}
//***********************************************
//Print model parameters function
void print_param(void)
{
// local variables
int i;
char date[10];
char comm2[20];
cout << "ENTER THE DATE: ";
cin >> date;
cout << "OTHER INFORMATION:";
cin >> comm2;
printer =fopen ("prn", "w");
if (printer==0){
	 printf("printer was not located\n");
	 goto end;
	 }
fputs("\n\t ADVANCED MODEL version BRKAW-F2,4,6\n", printer);
fputs("\t   THE GEOMETRIC CLUTCH \n\n", printer);
fputs("   CGS UNITS and complex torque balance\n\n", printer);
fprintf(printer,"  MODELING PARAMETERS     DATED: %s\n", date);
fprintf(printer,"  IDENTIFYING COMMENT: %s\n", comm1);
fprintf(printer,"  IDENTIFYING COMMENT: %s\n\n\n", comm2);
fprintf(printer,"SIDE TO SIDE COUPLING = %.2f",param[1]);
fprintf(printer,"\nFORCE PER DYNEIN BRIDGE IN DYNES(tug)= %.2e", param[2]);
fprintf(printer,"\nELASTIC CONST. OF PASSIVE ELASTIC NEXIN(snap)= %.2f", param[3]);
fprintf(printer,"\nSWITCH PROBABILITY P-BRIDGE = %.3f", param[4]);
fprintf(printer,"\nSWITCH PROBABILITY R-BRIDGE = %.3f", param[5]);
fprintf(printer,"\nCOEFFICIENT OF HISTERESIS(P) = %.1f", param[6]);
fprintf(printer,"\nCOEFFICIENT OF HISTERESIS(R) = %.1f", param[7]);
fprintf(printer,"\nTHE LOCAL ELASTIC MODULUS(IE) = %.2e",param[8]);
fprintf(printer,"\nTHE WORKING DIAMETER(diam) = %.2e",param[9]);
fprintf(printer,"\nTHE SEGMENT LENGTH(Z) = %.2e",param[10]);
fprintf(printer,"\nTHE DRAG COEFFICIENT(D) = %.4f", param[11]);
fprintf(printer,"\nTHE T-FORCE TO PROBABILITY SCALING(gain) = %.1f", param[12]);
fprintf(printer,"\nTHE ITERATION TIME INTERVAL(its) = %.5f",param[13]);
fputs("\n\n\tCURRENT MODE OF OPERATION", printer);
fprintf(printer,"\nTHE STATE OF THE MODEL(ACTIVE=1) = %.1f",param[14]);
fprintf(printer,"\nTHE BASE DRAG(1 highest; 0 lowest) = %.1f",param[15]);
fputs("\n\n\tTHIS IS PRINTED FROM THE C++ VERSION OF THE MODEL\n", printer);
fputc('\014', printer);

fclose(printer);
end  : ;
}//end of printout parameter function
//************************************
//output data to printer
void data_output(void)
{
int i;
char date[10];
char comm2[20];
char Y;
double startpoint=-5.0e-4;
cout << "do you want the current force velocity profiles?(if yes type Y):";
cin >> Y;
Y= toupper(Y);
if (Y=='Y') {
for (i=1; i<41; i++){
	data[i][0]= 2.4 * find_force(2, startpoint);
	data[i][1]= 5.0 * find_force(1, startpoint);
	startpoint+= 5.0e-05;
	}
	}
cout << "ENTER THE DATE: ";
cin >> date;
cout << "OTHER INFORMATION:";
cin >> comm2;
printer =fopen ("prn", "w");
if (printer==0){
	 printf("printer was not located\n");
	 goto end;
	 }
fputs("\n\t ADVANCED MODEL version CHIMERA/BROKAW\n", printer);
fputs("\t   THE GEOMETRIC CLUTCH \n\n", printer);
fputs("   CGS UNITS and complex torque balance\n\n", printer);
fputs("the sliding velocity increment is 5x10-5 cm per second\n", printer);
fprintf(printer,"  DATA OUTPUT     DATED: %s\n", date);
fprintf(printer,"  IDENTIFYING COMMENT: at iteration# %d\n",total);
fprintf(printer,"  IDENTIFYING COMMENT: %s\n\n\n", comm2);
for (i=1; i<41; i++){
fprintf(printer," %d) %e, %e \n",i,data[i][0], data[i][1]);

}
fputc('\014', printer);

fclose(printer);
end  : ;
}
//*****************************************
// find angles from slopes
double find_angle(double x, double y, double slope)
{
 double angle;
 if (slope==0.0) angle=0.0;
 else {
 if ( x>0 && y>0 )  angle=atan(slope);
 if ( x<0 && y>0 )  angle=atan(slope) + pi;
 if ( x<0 && y<0 )  angle=atan(slope) + pi;
 if ( x>0 && y<0 )  angle=atan(slope) + 2*pi;
      }
 return angle;
}
//**************************************************
// function which assigns force from force velocity curves
double find_force(int arm,double velocity)
{
// local variables
double e = 2.7182818;
double force;
if (arm==0) force = 0.0;
if (arm==1) {
force= pow(e,-(velocity*velocity)*4.0e07);
if (velocity<0.0) force=100.0*velocity + 1.0;
}
if (arm==2) {
force= pow(e,-(velocity-3.0e-04)*(velocity-3.0e-04)*4.0e06);
if(velocity<2.0e-04)force=pow(e,-(velocity-2.0e-04)*(velocity-2.0e-04)*5.0e07);
}

return force;
}
//**************************************************
// function which graphs output on laser printer
void graph_output(void)
{
// local variables
int i,I,H,l;
double Z = param[10];
int LN = ceil(300000.0 * Z);
char choice;
char menu;
char date[10];
cout << "ENTER THE DATE; ";
cin >> date;
cout << "Assign an identifying code :"<< endl;
cout << "based on the simulated structure" << endl;
cout << "IDENTIFYING CODE:";
cin >> comm1;
printer =fopen ("prn", "w");
if (printer==0){
	 printf("printer was not located\n");
	 goto end;
	 }
fputs("\n\t ADVANCED MODEL in C version CHIMERA/BROKAW\n", printer);
fputs("\t   THE GEOMETRIC CLUTCH \n\n", printer);
fprintf(printer,"  OUTPUT DATED: %s    LENGTH IS: %d microns\n", date, LN);
fprintf(printer,"  IDENTIFYING CODE: %s\n", comm1);
fprintf(printer,"Plotted every %d iterations from %d to %d \n",Q,total-N,total);
printer =fopen ("prn", "wb");

fprintf(printer,"\33%0B");// graphic mode command
fprintf(printer,"IN");// initialize graphics Mode
fprintf(printer,"SP1;SC-700,700,-870,870;");
for(l=1; l<(PL+1); l++){
  fprintf(printer,"PA0,0");
for (i=1; i<31; i++){
	I = floor(G[l] [i]*(12/Z));
	H = floor(K[l] [i]*(12/Z));
	fprintf(printer,"PD%d,%d;",I,H);
	}
	if(l==1) fprintf(printer, "DT*,1;LB_s*;");
	if (l==PL) fprintf(printer,"LB_f*;");
	if (l==EX ) fprintf(printer,"LB_i*;");
	if ( EX>0 && l==(EX+1)) fprintf(printer,"LB_i+*;");
fprintf(printer,"PU"); // pen up
}
fprintf(printer,"SP0");
fprintf(printer,"\33%0A");// back to normal Mode
fprintf(printer,"\33E"); // reset & eject page
fclose(printer);
cout << "sent to printer" << endl;
getch();
end  : ;
}
//************************************
//EXPERIMENT MODULE
int experiment_mod(void)
{
int i,j,R;
double change;
double Z=param[10];
char menu;
char Q;
cls();
cout << "EXPERIMENT MENU\n\n" << endl;
cout << "A) MOVE TO A STORED ANGLE FILE" << endl;
cout << "B) RETARD OR ADVANCE ANGLES" << endl;
cout << "C) CHANGE MODELING PARAMETER"<< endl;
cout << "D) DISABLE THE OUTER ARMS"<< endl;
cout << "enter your choice: ";
Q=getche();
menu=toupper(Q);
switch (menu)
{
	case 'A':goto first;
	case 'B':goto second;
	case 'C':goto third;
	case 'D':goto fourth;
	default :goto finis;
}
first :load_angles();
R=1;
goto finis;
second: ;
cout << "\n enter angle increment: ";
cin >> change ;
for (i=2; i<31; i++){
ang[i]+=change;
}
sx[1]=0;
sy[1]=0;
for ( j=1; j<31; j++) {
	sx[j+1] = sx[j] +(Z * cos(ang[j]));
	sy[j+1] = sy[j] +(Z * sin(ang[j]));
	cout << sx[j] <<"  " << sy[j] << "  " << ang[j] << endl;
	}
for ( i=2 ; i<31 ; i++){
	cur[i] = (ang[i]-ang[i-1]) / Z;//calculates the curvatures
	sang[i]=ang[i] - ang[1];
	cout << i << "  " << cur[i] << endl;
	}
getch();
R=1;
goto finis;
third : cls();
init_param();
R=2;
goto finis;
fourth : R=3;
finis :cls();
return R;
}
//************************************
// MAIN MODELING SUBROUTINE
void run_model(void)
{
// This begins the modelling portion of the Geometric Clutch program
// define local variables*********************
srand(time(NULL));
char C,c;
char Y,y;
int xval=0;
int yval=0;
int BRIDGE=0;
int COUNTER=0;
int innerarm= 1;
int outerarm= 2;
float oad_conv;
int i,j,k,m,n,l,A=0;
int RPTS=0;
const double DECAY_C=20000000000.0;
float R, BRVAL=0.0;
double STALLFORCE_P, STALLFORCE_R = 0.0;
double PBI1,PBO1,PBI2,PBO2,RBI1,RBO1,RBI2,RBO2=0.0;
double SMDRAG,bk,DIFFERENCE,TRACKER,FIXER,CUTOFF, ACURV = 0.0;
double TOTFORCE_R = .1;
double OLFORCE = 1.0;
double PANGL, LANGL, MAGN, XLEG, TORQUE, SUM_BP, SUM_BR, ADJUST=0.0;
double AVEBCORR, TOTCOR, DRAGTORQUE, DYDX, ARM, VECTX, VECTY, PREVIOUS=0.0;
double HYPOT, DR, TF, PSI, SUMF, OLDDECAY,LONG_FORC_P, LONG_FORC_R, DECAY=0.0;
double TVFR, DC, TVFP, TOTFORCE_P, NEWDECAY, CHOICE, FEEDBK, PUSH=0.0;
double TRIGGER_P, TRIGGER_R, TRIGGEROOT_P, TRIGGEROOT_R, ADHESN_P, ADHESN_R=0.0;

double tfa[4] [31]={ {0},{0} };//t-force arrays 0 and 1 are long. and transverse comp.
//2 and 3 are P and R t-force at each segment
double adh[2] [32]={ {0} , {0} };// adhesion force storage
double forc[6] [31]={ {0} , {0} }; //longitudinal forces on the doublets
double dec[31]={0};// DECAY values
double tcur[31]={0};//Temporary curvatures for torque balance adjustment
double dkc[31] = {0};// Decay constants for each segment
double osx[32]={0};// old position data from previous iteration
double osy[32]={0};
double drag[31]={0}; // dragtorque values
double torq[31]={0};// total torque passive and active
double equ[31]={0};// equilibrium curvatures
double adj[31]={0};// angle adjustments for feedback between iterations
double oshear[31]={0};//old shear angles
double vel[31]={0};// sliding velocities
double smvel[31]={0}; // smoothed sliding velocity
PL = 0;
for (i=1;i<31;i++) dkc[i] =DECAY_C /(.0333 * (30.0-(i-1)));
// load parameters from global param array
reset :double coupl=param[1];
double tug=param[2];
double SNAP=param[3];
double SWITCH_P=param[4];
double SWITCH_R=param[5];
double HISTER_P=param[6];
double HISTER_R=param[7];
double IE=param[8];
double DIAM=param[9];
double Z=param[10];
double D=param[11];
double GAIN=param[12];
double ITS=param[13];
double STATE=param[14];
double BASE=param[15];
//end of local variable list
oad_conv = 7.29e05*5.0 /(1.25e06*2.4*find_force(2,0.0) + 7.29e05*5.0);
HISTER_P=HISTER_P * oad_conv;
HISTER_R=HISTER_R * oad_conv;

cls();
repeat : A=1;
cout << "\t\t---THE GEOMETRIC CLUTCH MODEL version CHIMERA---" ;
cout << "\n\n\n\n         Enter the number of iterations: ";
cin >> N;
cout << "Enter the number of iterations per plot: ";
cin >> Q;
cout << "                       To cancel type C: ";
C = getche();
c = toupper(C);
if (c=='C') goto last;
initgraph(&graphdriver, &graphmode, "c:\\tc\\bgi");
if (graphresult() < 0)
	exit(1);
setlinestyle (DASHED_LINE,0,NORM_WIDTH);
moveto (300,0);
lineto (300,460);
moveto (0,240);
lineto (640,240);
setlinestyle (SOLID_LINE,0,THICK_WIDTH);
for ( n=1; n<(N+1) ; n++)//MAIN ITERATION LOOP FOR MODELING
{
if (PL<49) {
 if (n==1 || n%Q==0){ // Storage of position data for plotting
	PL+= 1;
	for ( j=1; j<31; j++){
	G[PL] [j]=sx[j];
	K[PL] [j]=sy[j];
	}
}
 }
if (n%Q==0 || n==1){
moveto (300,240);
for ( i=1; i<31; i++)//PLOT CURRENT POSITION
	{
	xval= 300 + floor(sx[i] * 7.92/Z);
	yval= 240 - floor(sy[i] * 7.92/Z);
	lineto (xval,yval);
	}
   }
// FIND T-FORCE
sang[1]=0;
for (j=1;j<31;j++){
	HYPOT =sqrt(((sang[j]*DIAM)*(sang[j]* DIAM))+pow(.0000033, 2));
	DR=HYPOT - .0000033;
	TF = SNAP * DR;
	if (sang[j]==0) sang[j]=.000001;
	PSI=fabs(atan(.0000033/(sang[j]*DIAM)));
	tfa[0] [j]=TF * cos(PSI) * (sang[j]/fabs(sang[j]));
	tfa[1] [j]=TF * sin(PSI);
	}
for ( l=1; l<31; l++) {//summation loop for passive force
	SUMF=0;
	for ( k=l; k<31; k++) {
		SUMF += tfa[0] [k];
		}
	forc[3] [l]=SUMF;
	}
if (STATE == 0) goto passive;
// FIND ACTIVE FORCES USING THE T-FORCE MECHANISM
for (l=1; l<31; l++) { // loop for bridge control
TVFP =tfa[1] [l] + Z*cur[l+1] * (.454 * forc[3] [l] + forc[4] [l]);

TVFR =tfa[1] [l] + Z*cur[l+1] * (.454 * forc[3] [l] + forc[5] [l]);

tfa[2] [l] = TVFP;
tfa[3] [l] = TVFR;
TRIGGEROOT_P = SWITCH_P + (TVFP * GAIN);
TRIGGEROOT_R = SWITCH_R + (TVFR * GAIN);

TRIGGER_R = TRIGGEROOT_R + adh[1] [l] +.5 * adh[1] [l-1]+ .5 * adh[1] [l+1];
if (TRIGGER_R<0.0) TRIGGER_R = 0.0;
TRIGGER_P = TRIGGEROOT_P + adh[0] [l] +.5 * adh[0] [l-1]+ .5 * adh[0] [l+1];
if (TRIGGER_P<0.0) TRIGGER_P = 0.0;


CHOICE = fabs(TOTFORCE_R);
if (fabs(TOTFORCE_P)>=fabs(TOTFORCE_R)) CHOICE=fabs(TOTFORCE_P);
//skip line
OLFORCE = 1.0;
COUNTER = 0;
	while (fabs((CHOICE/OLFORCE)-1)>.1){
	COUNTER++;
	if (COUNTER>150) break;
	OLFORCE=CHOICE;
	TOTFORCE_P = PBI1 = PBI2 = PBO1 = PBO2 = 0.0;
	TOTFORCE_R = RBI1 = RBI2 = RBO1 = RBO2 = 0.0;
	  for (BRIDGE=1; BRIDGE<floor(729166.0 * Z); BRIDGE++){
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_P){
	   PBI1+= 5.0 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_P){
	   PBI2+= 5.0 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_R){
	   RBI1-= 5.0 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_R){
	   RBI2-= 5.0 * tug;
	   }

	  }
if (outerarm==2) {
	  for (BRIDGE=1; BRIDGE<floor(1250000.0 * Z); BRIDGE++){
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_P){
	   PBO1+= 2.4 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_P){
	   PBO2+= 2.4 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_R){
	   RBO1-= 2.4 * tug;
	   }
	  R = rand();
	  BRVAL = R/RAND_MAX;
	  if (BRVAL<TRIGGER_R){
	   RBO2-= 2.4 * tug;
	   }
	  }
	  }// if loop for outer arm extracted

	STALLFORCE_P = (PBI1 + PBI2 + PBO1*find_force(2,0.0) + PBO2*find_force(2,0.0));
	STALLFORCE_R = (RBI1 + RBI2 + RBO1*find_force(2,0.0) + RBO2*find_force(2,0.0));

	PBI1=PBI1* find_force(innerarm,-smvel[l]);
	PBI2=PBI2* find_force(innerarm,-smvel[l]);
	RBI1=RBI1* find_force(innerarm,smvel[l]);
	RBI2=RBI2* find_force(innerarm,smvel[l]);
	PBO1=PBO1* find_force(outerarm,-smvel[l]);
	PBO2=PBO2* find_force(outerarm,-smvel[l]);
	RBO1=RBO1* find_force(outerarm,smvel[l]);
	RBO2=RBO2* find_force(outerarm,smvel[l]);
	TOTFORCE_P = (PBI1 + PBI2 + PBO1 + PBO2)/2.0;
	TOTFORCE_R = (RBI1 + RBI2 + RBO1 + RBO2)/2.0;
	ADHESN_P = fabs(STALLFORCE_P) * ((1.0 - TRIGGEROOT_P) * HISTER_P);
	ADHESN_R = fabs(STALLFORCE_R) * ((1.0 - TRIGGEROOT_R) * HISTER_R);
	if (outerarm==0){ADHESN_P=ADHESN_P/oad_conv;
	ADHESN_R=ADHESN_R/oad_conv;}
	adh[0] [l] = ADHESN_P - coupl * ADHESN_R;
	adh[1] [l] = ADHESN_R - coupl * ADHESN_P;

TRIGGER_R = TRIGGEROOT_R + adh[1] [l] +.5 * adh[1] [l-1]+ .5 * adh[1] [l+1];
TRIGGER_P = TRIGGEROOT_P + adh[0] [l] +.5 * adh[0] [l-1]+ .5 * adh[0] [l+1];

	forc[1] [l] = TOTFORCE_P;
	forc[2] [l] = TOTFORCE_R;
	CHOICE = fabs(TOTFORCE_R);
	if (fabs(TOTFORCE_P)>=fabs(TOTFORCE_R)) CHOICE=fabs(TOTFORCE_P);
	if (OLFORCE == 0.0) OLFORCE=.0000001;
	if (CHOICE == 0) CHOICE = OLFORCE;
	}
}// end of bridge control loop

for ( i=1; i<31; i++){ // sum up forces from current position to tip
	SUM_BP=0;
	SUM_BR=0;
		for( j=i; j<31 ; j++){
		SUM_BP += forc[1] [j];
		SUM_BR += forc[2] [j];
		}
	forc[4] [i] = SUM_BP;
	forc[5] [i] = SUM_BR;
	} // END OF ACTIVE FORCE CALCULATION

passive  : ;
// Drag routine for rotating base
dragloop : ;
if (BASE<1.0 && n>1) {
AVEBCORR = TOTCOR = 0.0;
RPTS = 0;
drag[1] = .000001;
ADJUST = pow((Z/.0001), 3);
while ((fabs(drag[1]))>(.00000001*ADJUST)){
 PREVIOUS = drag[1];
 RPTS+=1;

 if (RPTS>100) exit(1);
 l=1;
	DRAGTORQUE=0.0;
	for (k = l+1; k<32; k++){
	ARM = sqrt(pow((sx[l]-sx[k]),2) + pow((sy[l]-sy[k]),2));
	VECTX = sx[k]-osx[k];
	VECTY = sy[k]-osy[k];
	if (VECTX==0.0) VECTX =.000000001;
	DYDX = VECTY/VECTX;
	PANGL = find_angle(VECTX, VECTY, DYDX);
	MAGN = sqrt(pow(VECTX, 2)+pow(VECTY,2));
	XLEG = sx[k]-sx[l];
	if (XLEG==0.0)  XLEG=.000000001;
	DYDX = (sy[k]-sy[l])/XLEG;
	LANGL= find_angle(XLEG, sy[k]-sy[l], DYDX);
	if (ARM==0.0) ARM = .000000001;
	TORQUE = ARM * D * Z *((MAGN * sin(PANGL-LANGL))/ITS);// individual contributions
	DRAGTORQUE += TORQUE; // running sum of individual contrib.
	   }
 drag[1]=DRAGTORQUE; // current value of drag at segment 1 the base

 FEEDBK = 4000/(D*ADJUST);
 PUSH = drag[1]*FEEDBK;
 AVEBCORR = -PUSH;
 TOTCOR+= AVEBCORR;
		for (j=1; j<31; j++) {
		 ang[j]+= AVEBCORR;
		 sx[j+1] = sx[j] + Z*cos(ang[j]);
		 sy[j+1] = sy[j] + Z*sin(ang[j]);
		 }
 if (RPTS>1 && (drag[1]*PREVIOUS)<0 ) drag[1]=0.0;
 }
	for (j=1; j<31; j++){
	ang[j]-=TOTCOR*BASE;
	sx[j+1]=sx[j]+Z*cos(ang[j]);
	sy[j+1]=sy[j]+Z*sin(ang[j]);
	}
}// END OF BASE ADJUSTMENT LOOP
// TORQUE BALANCE ROUTINE FOR ADVANCED MODEL
for ( l=1; l<31; l++) { //calculation of torques
	forc[0] [l] = 2*forc[3] [l] + forc[4] [l] + forc[5] [l];
	torq[l]= forc[0] [l] * DIAM;
	}
CUTOFF = 2.0E-11;
bk = .002*(.00003/Z);
for ( m=1; m<8; m++){
SMDRAG = drag[1];
for ( l=1; l<30; l++){
	SMDRAG = (SMDRAG + drag[l])/2.0;
	equ[l+1]=-torq[l]/IE;
	if (NEWDECAY>.4) equ[l+1] = -(torq[l] + (.01 * drag[l]))/IE;//Correction for wind
	OLDDECAY = dec[l];
	NEWDECAY = OLDDECAY;
	if (fabs(torq[l]+ SMDRAG +(IE*cur[l+1])) > CUTOFF){ //TOTAL TORQUE LIMIT
	if (fabs(torq[l]+ SMDRAG +(IE*cur[l+1])) > 1.0E-9) bk = .0001*(.00003/Z);
	FIXER = bk * (fabs(torq[l] + SMDRAG + (IE * cur[l+1])) * dkc[l]);//feedback adjustment
	if (SMDRAG * (torq[l] + (IE*cur[l+1])) >0.0) NEWDECAY = OLDDECAY + FIXER;
	if (SMDRAG * (torq[l] + (IE*cur[l+1])) <0.0) {
	if (fabs(SMDRAG) > fabs(torq[l]+(IE*cur[l+1])) ) NEWDECAY=OLDDECAY-FIXER;
	if (fabs(SMDRAG) < fabs(torq[l]+(IE*cur[l+1])) ) NEWDECAY=OLDDECAY+FIXER;
	if (fabs(drag[l]) > fabs(torq[l] * 1.4)) NEWDECAY = OLDDECAY * .6;
	}
	if (NEWDECAY > .6 ) NEWDECAY = .6; //LIMITS ON DECAY COSTANT
	if (NEWDECAY < -.2) NEWDECAY = -.2;

	if (n==1 && ROUTE==1) NEWDECAY=.025; // LAUNCH RAMP ON STARTUP ONLY
	if (n==2 && ROUTE==1) NEWDECAY=.01;
	if (n==3 && ROUTE==1) NEWDECAY=.005;

	DIFFERENCE = NEWDECAY - OLDDECAY;
	TRACKER = OLDDECAY + (.5 * DIFFERENCE);
	if (fabs(drag[l]) > fabs(SMDRAG * 1.6)) TRACKER = 0.0;
	if (drag[l]*SMDRAG<0.0) TRACKER = 0.0;
	DECAY = (OLDDECAY+TRACKER)/2.0;
	dec[l]= DECAY;
	adj[l+1]= (cur[l+1]-equ[l+1])*DECAY;
	}
  }
for ( i=2; i<31; i++){ // feedback point for position correction
	tcur[i]=cur[i] - adj[i];
	ang[i]=ang[i-1] + tcur[i]*Z;
	}
for ( j=1; j<31; j++){ // recalculate positions
	if (m==1){
	osx[j+1]=sx[j+1];
	osy[j+1]=sy[j+1];
	}
	sx[j+1]=sx[j]+Z*cos(ang[j]);
	sy[j+1]=sy[j]+Z*sin(ang[j]);
	}                             // position reset loops
if (total>1){
if (m==1 && BASE<1){
	for (j=1; j<31; j++){
	ang[j]+=TOTCOR * (1.0-BASE);
	sx[j+1]=sx[j]+Z*cos(ang[j]);
	sy[j+1]=sy[j]+Z*sin(ang[j]);
	}
}
}

//drag recalculation routine
for (l=1; l<31; l++){
	DRAGTORQUE=0.0;
	for (k = l+1; k<32; k++){
	ARM = sqrt(pow((sx[l]-sx[k]),2) + pow((sy[l]-sy[k]),2));
	VECTX = sx[k]-osx[k];
	VECTY = sy[k]-osy[k];
	if (VECTX==0) VECTX =.00000000001;
	DYDX = VECTY/VECTX;
	PANGL = find_angle(VECTX, VECTY, DYDX);
	MAGN = sqrt(pow(VECTX, 2)+pow(VECTY,2));
	XLEG = sx[k]-sx[l];
	if (XLEG==0.0)  XLEG=.00000000001;
	DYDX = (sy[k]-sy[l])/XLEG;
	LANGL= find_angle(XLEG, sy[k]-sy[l], DYDX);
	if (ARM==0.0) ARM = .00000000001;
	DC = .5*D+(.5 * D * fabs(sin(PANGL-ang[k-1])));
	TORQUE = ARM * DC * Z *((MAGN * sin(PANGL-LANGL))/ITS);// individual contributions
	DRAGTORQUE += TORQUE; // running sum of individual contrib.
	   }
	drag[l]=DRAGTORQUE; // store them in the drag array
	}//end of drag loop
if (n<=3 && ROUTE ==1) break;
}
for ( j=2; j<30; j++){ // smoothing loop
	ACURV = (cur[j] +cur[j+1])/2;
	cur[j]= (cur[j] + ACURV)/2;
	cur[j+1]=(cur[j+1] + ACURV)/2;
	}
for ( i=2; i<31; i++) oshear[i] = sang[i] * DIAM;
for ( i=2; i<31; i++){ // Major feedback point
	cur[i]=cur[i] - adj[i];
	ang[i]=ang[i-1] + cur[i]*Z;
	sang[i]=ang[i] - ang[1];
	vel[i]=.5*(((sang[i] * DIAM)- oshear[i])/ITS);
	}
for (i=2; i<31; i++){ //smoothing block for rolling average sliding velocity
	smvel[i]=(smvel[i-1]+smvel[i])/2.0;
	smvel[i]=(9.0*smvel[i]+vel[i])/10.0;
	}
for ( j=1; j<31; j++){ // recalculate positions
	sx[j+1]=sx[j]+Z*cos(ang[j]);
	sy[j+1]=sy[j]+Z*sin(ang[j]);
	}                             // position reset loops
  //end position reset loops

total+= 1;
}//END OF MAIN ITERATION LOOP
moveto(10, 10);
outtext("The run is finished:");
getch();
closegraph();
cout << "The model is now at iteration # " << total << endl;
cout << "Do you wish to continue(if yes type y): " << endl;
Y = getche();
y = toupper(Y);
if (y=='Y') {
	ROUTE = 2;
	cls();
	cout << "Do you want data printed out? if yes type y:"<< endl;
	Y = getche();
	y = toupper(Y);
	if (y=='Y'){
	for (i=1;i<31;i++) data[i][0]=torq[i];
	data_output();
	cout << "torque values sent to printer"<< endl;
	getch();
	}
	cout << "Do you wish to experiment? if yes type y: " ;
	Y = getche();
	y = toupper(Y);
	if (y=='Y') A=experiment_mod();
	else PL=0;
	if (A==1) goto repeat;
	if (A==2) goto reset;
	if (A==3){ outerarm=0; goto repeat; }
	}


last : ;
ROUTE = 2;
}// END OF THE MODELING FUNCTION
// ********************************************************
// ************************* END OF PROGRAM ****************