#include <graphics.h>
#include <dos.h>
#include <math.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>

char *Fonts[] = {
  "DefaultFont","SmallFont",
  "SansSerifFont"};

#include "VGAMOUSE.j"
#include "GRAPfunc.j"

double dist(int tarx, int tary, int recx, int recy);

double pi=3.1415927;

void main()
	{

	int tx=20,ty=400,r1x=300,r1y=359,r2x=300,r2y=361,t,key;
	int incr=10,new_graph=1,frame=128;
	double distance1,distance2,ti1,ti2,delay,f=301,fs=4000,phase;
	float signal1[260],signal2[260],beam[260],maximum;
	char buffer[20];

	vgamode();
	setcolor(15);
	rectangle(0,242,320,479);
while(key != 13)
	{
if(new_graph==1)
{
	circle(tx,ty,5);
   	line(r1x,r1y,r2x,r2y);
	distance1=dist(tx,ty,r1x,r1y);
   	distance2=dist(tx,ty,r2x,r2y);
	ti1=distance1*(1.0/344.0);
	ti2=distance2*(1.0/344.0);
	delay=(ti2-ti1);
	for(t=0;t<=(frame-1);t++)
		{
		signal1[t]=sin(2*pi*(f/fs)*((double)t-(ti1*fs)));
		signal1[t]=signal1[t]+sin(2*pi*(205/fs)*((double)t-(ti1*fs)));
        signal1[t]=signal1[t]+sin(2*pi*(443/fs)*((double)t-(ti1*fs))); 

	    signal2[t]=sin(2*pi*(f/fs)*((double)t-(ti2*fs)));  
	    signal2[t]=signal2[t]+sin(2*pi*(205/fs)*((double)t-(ti2*fs)));  
		signal2[t]=signal2[t]+sin(2*pi*(443/fs)*((double)t-(ti2*fs)));
		}
	signal1[t]=999;
	signal2[t]=999;
	setcolor(9);
	graph(1,10,639,240,signal1,2);
	setcolor(7);
    graph(1,10,639,240,signal2,0);  
}
	key=getch();

	setcolor(0);
	circle(tx,ty,5);
	line(r1x,r1y,r2x,r2y);
	if(key==56)
		ty=ty-incr;
	if(key==50)
		ty=ty+incr;
	if(key==54)
		tx=tx+incr;
	if(key==52)
		tx=tx-incr;
	if(key==43)
		{                /* + */
		r1y=r1y-incr;
		r2y=r2y+incr;
        }
	if(key==45)            /* - */
    	{
		r1y=r1y+incr;
		r2y=r2y-incr;
		}
	if(key==49)
		incr=10;
	if(key==48)
		incr=1;
    if(key==71)
		new_graph=1;
    if(key==103)
		new_graph=0;
    if(key==66)
		{
        for(t=0;t<=(frame/2)-1;t++)
			{
			beam[t]=0;
            }
		maximum=10;
		for(t=0;t<=(frame/2)-1;t++)
			{
			for(key=0;key<=(frame/2)-1;key++)
            			{
				delay=(signal1[key+((frame/4)-1)]-signal2[t+key]);
			 	delay=pow((double)delay,2.0);    
				beam[t]=beam[t]+delay;
				}
			beam[t]=beam[t]/(frame/2);
			if(beam[t]<maximum)
                {
				maximum=beam[t];
				phase=(double)(t-((frame/4)-1));
                }
            }
		beam[(frame/2)-1]=999.;
		setcolor(15);
		graph(1,10,639,240,beam,1);
        new_graph=0;
        phase=phase*(1.0/fs);
		phase=phase*344.0;
        if(phase != 0)
			phase=asin(phase/2.0);
		phase=phase*(180.0/pi);
                if(phase >0)
			phase=phase+((pow(phase,2.0)*.0018208)-(phase*.0440229));
                if(phase <0)
			phase=phase-((pow(phase,2.0)*.0018208)+(phase*.0440229));

		}

	setcolor(15);
    setfillstyle (SOLID_FILL,0);  /* draw grey center of button */
	bar(322,242,630,479);

	sprintf(buffer,"Dis. to target: %f",dist(tx,ty,300,360));
	outtextxy(332,252,buffer);
	sprintf(buffer,"Dis. between arrays: %d",(r2y-r1y));
	outtextxy(332,262,buffer);
    sprintf(buffer,"Step size: %d",incr);
	outtextxy(332,272,buffer);
    sprintf(buffer,"Graphing status: %d",new_graph);
	outtextxy(332,282,buffer);
    sprintf(buffer,"Calculated angle to target: %f",phase);
	outtextxy(332,292,buffer);
	sprintf(buffer,"Actual angle to target: %f",(180.0/pi)*(atan((ty-360.0001)/(tx-300.001))));
	outtextxy(332,302,buffer);

	circle(tx,ty,5);
	line(r1x,r1y,r2x,r2y);

        }
    closegraph();
	}
double dist(int tarx, int tary, int recx, int recy)
	{
	double temp1,temp2;
	temp1=tarx-recx;
	temp2=tary-recy;
	temp1=pow(temp1,2);
    temp2=pow(temp2,2);
	temp1=temp1+temp2;
	temp2=sqrt(temp1);
	return temp2;
    }