Subversion Repositories shark

Rev

Blame | Last modification | View Log | RSS feed

#include <kernel/kern.h>   //include the SHARK kernel header
#include <drivers/glib.h>  //include the graphics library
#include <drivers/keyb.h>  //include the keyboard library
#include <semaphore.h>     //include the semaphore library
#include <stdlib.h>        //include the standard library
#include <math.h>          //include the math library for random
#include <string.h>

#ifndef _ATCSIM_HEADER_
#include "atcsim.h"
#endif

#ifndef _SIM_AREA_HEADER_
#include "sim_area.h"
#endif

#ifndef _INPUT_
#include "input.h"
#endif

#ifndef _CONST_
#include "constants.h"
#endif

extern sem_t mut_planeinzone[MAX_ATCS][MAX_PLANES];
char *plane_cab_ptr1[MAX_PLANES];
aeroplane cabmsg_aeroplanes1[MAX_PLANES];
PID tid[MAX_ATCS];

extern int STARTSIM;
extern int get_i();
extern aeroplane aeroplanes[MAX_PLANES];

extern airport airports[NUM_OF_AIRPORTS];
extern atc atcs[MAX_ATCS];
extern mutex_t mut_number_of_planes;

aeroplane cabmsg_aeroplanes2[MAX_PLANES];
atcplanecab cabmsg_atc[MAX_PLANES];

int planeinzone[MAX_ATCS][MAX_PLANES];

void DoesCollide(int myid,int no_of_planes)
{
        int id,id2;
        int currx1,curry1,destx1,desty1,speed1,currx2,curry2,destx2,desty2,speed2,srcx2,srcy2,srcx1,srcy1;

        int src_apid1,dest_apid2,tempx1,tempx2,tempy1,tempy2,resx,resy;
        int gxmin,gymin,gxmax,gymax;

        int cx1mmd,cx1pmd,cx2mmd,cx2pmd,cy1mmd,cy1pmd,cy2mmd,cy2pmd;
        float pdistance,a1,a2,b1,b2,c1,c2,d1,d2,delta;
        char *plane_cab_ptr2[MAX_PLANES];
//      char *atc_cab_ptr1,*atc_cab_ptr2;

        char temp[100];

        int finalx1=-1,finalx2=-1,finaly1=-1,finaly2=-1;


        for(id=0;id<no_of_planes;id++)
        {
                        plane_cab_ptr2[id]=cab_getmes(plane_atc_cab_id[id]);
                        memcpy(&cabmsg_aeroplanes2[id],plane_cab_ptr2[id],sizeof(aeroplane));
                        cab_unget(plane_atc_cab_id[id],plane_cab_ptr2[id]);
        }

        //Getting the starting and ending co-ordinates of the 2 planes
        for(id=0;id<no_of_planes-1;id++)
        {
                sem_wait(&mut_planeinzone[myid][id]);
                if (planeinzone[myid][id]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id]].is_flying == 1)
                {
                        currx1=cabmsg_aeroplanes2[planeinzone[myid][id]].currx;
                        curry1=cabmsg_aeroplanes2[planeinzone[myid][id]].curry;

                        tempx1=cabmsg_aeroplanes2[planeinzone[myid][id]].xtemp;
                        tempy1=cabmsg_aeroplanes2[planeinzone[myid][id]].ytemp;

                        destx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].x;
                        desty1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].y;

                        srcx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].x;
                        srcy1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].y;

                        speed1=cabmsg_aeroplanes2[planeinzone[myid][id]].curr_speed;

                        src_apid1  = cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id;

                        sem_post(&mut_planeinzone[myid][id]);

                        cx1mmd = currx1-MIN_DIST/2;
                        if (cx1mmd < XMIN) cx1mmd = XMIN+10;

                        cx1pmd = currx1+MIN_DIST/2;
                        if (cx1pmd > XMAX) cx1pmd = XMAX-10;

                        cy1mmd = curry1-MIN_DIST/2;
                        if (cy1mmd < YMIN) cy1mmd = YMIN+10;

                        cy1pmd = curry1+MIN_DIST/2;
                        if (cy1pmd > YMAX) cy1pmd = YMAX-10;

                        for( id2 = id+1 ; id2 < no_of_planes; id2++ )
                        {
                                sem_wait(&mut_planeinzone[myid][id2]);
                                if (planeinzone[myid][id2]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id2]].is_flying == 1)
                                {
                                        dest_apid2 = cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id;

                                        currx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].currx;
                                        curry2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curry;

                                        tempx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].xtemp;
                                        tempy2=cabmsg_aeroplanes2[planeinzone[myid][id2]].ytemp;

                                        destx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].x;
                                        desty2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].y;

                                        srcx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].x;
                                        srcy2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].y;

                                        speed2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curr_speed;
                                        sem_post(&mut_planeinzone[myid][id2]);
                                        cx2mmd = currx2-MIN_DIST/2;
                                        if (cx2mmd < XMIN) cx2mmd = XMIN+10;

                                        cx2pmd = currx2+MIN_DIST/2;
                                        if (cx2pmd > XMAX) cx2pmd = XMAX-10;

                                        cy2mmd = curry2-MIN_DIST/2;
                                        if (cy2mmd < YMIN) cy2mmd = YMIN+10;

                                        cy2pmd = curry2+MIN_DIST/2;
                                        if (cy2pmd > YMAX) cy2pmd = YMAX-10;

                                        pdistance=sqrt((currx2-currx1)*(currx2-currx1) + (curry2-curry1)*(curry2-curry1));
                                        if ( pdistance < MIN_DIST )
                                        {
                                                if((currx1-destx1)!=0)  a1 = (curry1-desty1)/(currx1-destx1);
                                                else    a1 = 0.0;
                                                b1=-1;c1=-curry1;
                                                if ((currx2-destx2)!=0) a2=(curry2-desty2)/(currx2-destx2);
                                                        else    a2=0.0;
                                                b2=-1;c2=-curry2;
                                                delta=a1*b2-a2*b1;//The determinant used for division
                                                resx=0;resy=0;  //The result of solving the two equations.
                                                if(delta)
                                                {
                                                        resx=(int)((c1*b2-c2*b1)/delta);
                                                        resy=(int)((a1*c2-a2*c1)/delta);

                                                        gxmin = atcs[myid].x-GRID_SIZE/2;
                                                        gxmax = atcs[myid].x+GRID_SIZE/2;
                                                        gymin = atcs[myid].y-GRID_SIZE/2;
                                                        gymax = atcs[myid].y+GRID_SIZE/2;

                                                        //Now to check if the lines really collide within the simulation area
                                                        if ((resx > gxmin)  && (resx < gxmax) && (resy > gymin) && (resy < gymax))
                                                        {
                                                                //There is definitely an intersection within the area but need not be a collision
                                                                //So calculate the distance of the two planes from the colliding point
                                                                //And based on their speeds get whether they reach there at the same time
                                                                        d1=sqrt((resx-currx1)*(resx-currx1) + (resy-curry1)*(resy-curry1));
                                                                        d2=sqrt((resx-currx2)*(resx-currx2) + (resy-curry2)*(resy-curry2));
                                                                //These are the 2 distances of the 2 planes from the intersecting point.
                                                                //if ((abs((d1*SCALE_DIST/speed1)*60-(d2*SCALE_DIST/speed2)*60))<5)
                                                                if (floor(d1/speed1) == floor(d2/speed2))
                                                                {
                                                                        //Condition for no collision
                                                                        if(((curry1<curry2)&&(tempy1<tempy2))||((currx1<currx2)&&(tempx1<tempx2))){}
                                                                        else if(currx1<tempx1)//&&curry1>tempy1)
                                                                                finalx1=cx1pmd;
                                                                        else if(curry1<tempy1)//&&curry1>tempy1)
                                                                                finaly1=cy1pmd;

                                                                }
                                                        }
                                                        //else The planes collide outside our area;
                                                }
                                                else
                                                {
                                                        if (srcx1 == tempx2 && srcy1 == tempy2)
                                                        {
                                                                finalx1=cx1pmd+30;
                                                                //finaly1=cy1pmd;
                                                                //finalx2=cx2mmd;
                                                                //finaly2=cy2pmd;
                                                        }
                                                        else if (srcx1 == tempx2 && tempy1 > tempy2)
                                                        {
                                                                finalx1=cx1pmd;
                                                                finaly1=cy1pmd;
                                                                finalx2=cx2mmd;
                                                                finaly2=cy2mmd;
                                                        }
                                                        else if (srcx1 == tempx2 && tempy1 < tempy2)
                                                        {
                                                                finalx1=cx1mmd;
                                                                finaly1=cy1mmd;
                                                                finalx2=cx2pmd;
                                                                finaly2=cy2pmd;
                                                        }
                                                        else if (srcy1 == tempy2 && tempx1 > tempx2)
                                                        {
                                                                finalx1=cx1pmd;
                                                                finaly1=cy1mmd;
                                                                finalx2=cx2mmd;
                                                                finaly2=cy2pmd;
                                                        }
                                                        else if (srcy1 == tempy2 && tempx1 < tempx2)
                                                        {
                                                                finalx1=cx1mmd;
                                                                finaly1=cy1pmd;
                                                                finalx2=cx2pmd;
                                                                finaly2=cy2mmd;
                                                        }
                                                        else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 > tempy2))
                                                        {
                                                                finalx1=cx1pmd;
                                                                finaly1=cy1pmd;
                                                                finalx2=cx2mmd;
                                                                finaly2=cy2mmd;

                                                        }
                                                        else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 < tempy2))
                                                        {
                                                                finalx1=cx1mmd;
                                                                finaly1=cy1mmd;
                                                                finalx2=cx2pmd;
                                                                finaly2=cy2pmd;
                                                        }
                                                        else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 > tempx2))
                                                        {
                                                                finalx1=cx1pmd;
                                                                finaly1=cy1mmd;
                                                                finalx2=cx2mmd;
                                                                finaly2=cy2pmd;
                                                        }
                                                        else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 < tempx2))
                                                        {
                                                                finalx1=cx1mmd;
                                                                finaly1=cy1pmd;
                                                                finalx2=cx2pmd;
                                                                finaly2=cy2mmd;
                                                        }

                                                }
                                                //keep mutex for the aeroplane
                                                sem_wait(&mut_planeinzone[myid][id]);
                                                if (finalx1!=-1)
                                                        aeroplanes[planeinzone[myid][id]].xtemp=finalx1;
                                                if (finaly1!=-1)
                                                        aeroplanes[planeinzone[myid][id]].ytemp=finaly1;
                                                sem_post(&mut_planeinzone[myid][id]);
                                                sem_wait(&mut_planeinzone[myid][id2]);
                                                if (finalx2!=-1)
                                                        aeroplanes[planeinzone[myid][id2]].xtemp=finalx2;
                                                if (finaly2!=-1)
                                                        aeroplanes[planeinzone[myid][id2]].ytemp=finaly2;
                                                sem_post(&mut_planeinzone[myid][id2]);

                                                if ((finalx1 != -1 || finaly1 != -1) || (finalx2 != -1 || finaly2 != -1))
                                                {
                                        //              cabmsg_atc[id].written='Y';
                                        //              cabmsg_atc[id2].written='Y';
                                        //              cabmsg_atc[id].xtemp=cx1mmd;
                                                //      cabmsg_atc[id].ytemp=cy1pmd;
                                        //              cabmsg_atc[id2].xtemp=cx2pmd;
                                                //      cabmsg_atc[id2].ytemp=cy2mmd;

                                                //      atc_cab_ptr1=cab_reserve(atc_plane_cab_id[id]);
                                                //      memcpy(atc_cab_ptr1,&cabmsg_atc[id],sizeof(atcplanecab));
                                                //      cab_putmes(atc_plane_cab_id[id],atc_cab_ptr1);

                                                        //atc_cab_ptr2=cab_reserve(atc_plane_cab_id[id2]);
                                                        //memcpy(atc_cab_ptr2,&cabmsg_atc[id2],sizeof(atcplanecab));
                                                        //cab_putmes(atc_plane_cab_id[id2],atc_cab_ptr2);

                                                        sprintf(temp,"ATC %d detected collision between %d and %d     ",myid,id,id2);
                                                        grx_text(temp,XMIN,YMIN-10,red,black);
                                                }
                                        }//end of if loop inside the second for
                                }
                                else
                                {
                                        sem_post(&mut_planeinzone[myid][id2]);
                                }
                        }
                }
                else
                {
                        sem_post(&mut_planeinzone[myid][id]);
                }
        }
}  // End of DoesCollide

TASK recv_reg(void *arg)
{
        int atc_id = (int)(arg);
        int plane_id;
        int count=0;
        while(1)
        {
                for(plane_id=0;plane_id<get_i();++plane_id)
                {
                        sem_wait(&mut_planeinzone[atc_id][plane_id]);
                        if (planeinzone[atc_id][plane_id]==plane_id)
                                ++count;
                        sem_post(&mut_planeinzone[atc_id][plane_id]);
                }

                if(count>1)
                {
                        DoesCollide(atc_id,plane_id); //check if all the planes in my zone collide
                }
                count=0;
                task_endcycle();
        }
}

TASK manager_ATC(void *arg)
{
        int plane_id,atc_id;
        while(1)
        {
                for (atc_id=0;atc_id<MAX_ATCS;atc_id++)
                {
                        for(plane_id=0;plane_id<get_i();++plane_id)
                        {
                                //check if the plane is actually flying
                                plane_cab_ptr1[plane_id]=cab_getmes(plane_atc_cab_id[plane_id]);
                                memcpy(&cabmsg_aeroplanes1[plane_id],plane_cab_ptr1[plane_id],sizeof(aeroplane));
                                cab_unget(plane_atc_cab_id[plane_id],plane_cab_ptr1[plane_id]);

                                if (cabmsg_aeroplanes1[plane_id].is_flying == 1)  //this value will be read from the CAB
                                {
                                        if (cabmsg_aeroplanes1[plane_id].zone_id == atc_id)  //this zone id will be compared by the manager ATC thread and that will activate the ATC of that zone
                                        {
                                                sem_wait(&mut_planeinzone[atc_id][plane_id]);
                                                planeinzone[atc_id][plane_id]=plane_id;
                                                sem_post(&mut_planeinzone[atc_id][plane_id]);
                                                task_activate(tid[atc_id]);
                                        }
                                        else
                                        {
                                                sem_wait(&mut_planeinzone[atc_id][plane_id]);
                                                planeinzone[atc_id][plane_id]=NOTMYZONE;  //since plane is not flying
                                                sem_post(&mut_planeinzone[atc_id][plane_id]);
                                                task_disable(tid[atc_id]);
                                        }
                                }
                                else
                                {
                                        sem_wait(&mut_planeinzone[atc_id][plane_id]);
                                        planeinzone[atc_id][plane_id]=NOTMYZONE;
                                        sem_post(&mut_planeinzone[atc_id][plane_id]);
                                        task_disable(tid[atc_id]);
                                }
                        }
                }
                task_endcycle();
        }
}


TASK display_ATC_info(void *arg)  //this shud also display which planes are colliding
{
        int i,j;
        char temp[100];
        int printed[MAX_ATCS];
        int no_of_planes_in_zone[MAX_ATCS];

        char temp_p_in[3];
        char p_in_zone[100];
        strcpy(p_in_zone," ");
        char clear[30];
        sem_wait(&graphics_mutex);
        grx_text("ATC     PLANES IN ZONE",XMAX+10,(int)(YMIN+YMAX/1.5),green,black);
        grx_text("----------------------",XMAX+10,(int)(10+YMIN+YMAX/1.5),green,black);
        for (i=0;i<MAX_ATCS;i++)
        {
                sprintf(temp," %d  : ",i);
                grx_text(temp,XMAX+10,(int)((i*10)+YMIN+YMAX/1.5+20),green,black);
                printed[i]=0;
                no_of_planes_in_zone[i]=0;
        }
        sem_post(&graphics_mutex);
        while(1)
        {
                for (i=0;i<MAX_ATCS;i++)
                {
                        for (j=0;j<MAX_PLANES;j++)
                        {
                                sem_wait(&mut_planeinzone[i][j]);
                                if (cabmsg_aeroplanes1[j].is_flying==1 && planeinzone[i][j]==j)
                                {
                                        sem_post(&mut_planeinzone[i][j]);
                                        sprintf(temp_p_in,"%d ",j);
                                        strcat(p_in_zone,temp_p_in);
                                        ++no_of_planes_in_zone[i];
                                }
                                else
                                {
                                        sem_post(&mut_planeinzone[i][j]);
                                }

                                if (no_of_planes_in_zone[i] < printed[i])
                                {
                                        strcpy(clear,"                  ");
                                        sem_wait(&graphics_mutex);
                                        grx_text(clear,XMAX+65,(int)((i*10)+YMIN+YMAX/1.5+20),black,black);
                                        sem_post(&graphics_mutex);
                                        printed[i]=0;
                                        strcpy(p_in_zone," ");
                                }
                                else
                                {
                                        printed[i]=no_of_planes_in_zone[i];
                                        sem_wait(&graphics_mutex);
                                        grx_text(p_in_zone,XMAX+65,(int)((i*10)+YMIN+(YMAX/1.5)+20),white,black);
                                        sem_post(&graphics_mutex);
                                }
                        }
                        no_of_planes_in_zone[i]=0;
                        strcpy(p_in_zone," ");
                }
                task_endcycle();
        }
}

void init_atcs()
{
        PID tid2,m_tid;
        int n_atcs;
        int i,j;
        SOFT_TASK_MODEL m;
        SOFT_TASK_MODEL manager_atc;
        SOFT_TASK_MODEL disp_atc;

        for (i=0;i<MAX_ATCS;i++)
                for (j=0;j<MAX_PLANES;j++)
                {
                        sem_wait(&mut_planeinzone[i][j]);
                        planeinzone[i][j]=NOTMYZONE;
                        sem_post(&mut_planeinzone[i][j]);
                }

        soft_task_default_model(manager_atc);
        soft_task_def_level(manager_atc,1);
        soft_task_def_ctrl_jet (manager_atc);
        //soft_task_def_arg (manager_atc, (void *)n_atcs);
        soft_task_def_met (manager_atc, MANAGERATCWCET);
        soft_task_def_group (manager_atc, ATCGROUP);  //modify this later
        soft_task_def_period (manager_atc, MANAGERATCPERIOD);
        soft_task_def_usemath (manager_atc);
        m_tid = task_create("MgrATC", manager_ATC, &manager_atc, NULL);
        if (m_tid == NIL)
        {
                grx_close();
                cprintf("Could not create task <MgrATC>");
                sys_end();
        }
        task_activate(m_tid); //manager task will activate and deactivate the other ATC tasks as planes go in their zone



        for (n_atcs=0;n_atcs<MAX_ATCS;n_atcs++)  //create all the ATC tasks but do not activate them
        //the manager thread will activate and deactivate these tasks
        {
                //activate the receive message TASK
                soft_task_default_model(m);
                soft_task_def_level(m,1);
                soft_task_def_ctrl_jet (m);
                soft_task_def_arg (m, (void *)n_atcs);
                soft_task_def_met (m, RECVMSGWCET);
                soft_task_def_group (m, ATCGROUP);  //modify this later
                soft_task_def_period (m, RECVMSGPERIOD);
                soft_task_def_usemath (m);
                tid[n_atcs] = task_create("recv_reg", recv_reg, &m, NULL);
                if (tid[n_atcs] == NIL)
                {
                        grx_close();
                        cprintf("Could not create task <Recv_reg>");
                        sys_end();
                }
        }

        //activate the display ATC info TASK
        soft_task_default_model(disp_atc);
        soft_task_def_level(disp_atc,1);
        soft_task_def_ctrl_jet (disp_atc);
        soft_task_def_arg (disp_atc, (void *)n_atcs);
        soft_task_def_met (disp_atc, DISPATCWCET);
        soft_task_def_group (disp_atc, ATCGROUP);  //modify this later
        soft_task_def_period (disp_atc, DISPATCPERIOD);
        soft_task_def_usemath (disp_atc);
        tid2 = task_create("DispATC", display_ATC_info, &disp_atc, NULL);
        if (tid2 == NIL)
        {
                grx_close();
                cprintf("Could not create task <DispATC>");
                sys_end();
        }
        task_activate(tid2);
}