/* routine to map shortest path to cans while avoiding obstacles. */

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

struct object
  {
    int distance;
    int type;
    int xcenter;
    int ycenter;
    float angle;
  };

struct object objects[10];

main()

  {
    int count = 3;
    int path,distance1,distance2;
    float angle1,angle2;

    objects[1].distance = 149.654266;
    objects[1].angle = -0.137222;
    objects[1].type = 2;

    objects[2].distance = 140.721756;
    objects[2].angle = 0.099938;
    objects[2].type = 2;

    objects[3].distance = 172.346603;
    objects[3].angle = -0.052539;
    objects[3].type = 1;  

    VIS$OPEN();
    VIS$INIT_FB();
    setcamera(1);
    VIS$INIT_LUTS();
    VIS$VIEW();
    VIS$FREEZE();

    mapobjects(count,&path,&distance1,&distance2,&angle1,&angle2);

    printf("%s\n","mapping complete");
    printf("%s%d\n","path = ",path);
    printf("%s%d\n","distance1 = ",distance1);
    printf("%s%d\n","distance2 = ",distance2);
    printf("%s%f\n","angle1 = ",angle1);
    printf("%s%f\n","angle2 = ",angle2);
  }

int mapobjects(numberobj,pathcnt,dist1,dist2,angle1,angle2)

  int numberobj,*pathcnt,*dist1,*dist2;
  float *angle1,*angle2;

  {
    int i,level,target,alphax,alphay,pix,leftdistance;
    int rightdistance,alphaleftlength,alpharightlength;
    int betaleftlength,betarightlength,alphadata[1145];
    int type1cnt = 0;

    float deltaleft,deltaright,alphaleft,alpharight,betaleft;
    float betaright,alphaleftdeg,alpharightdeg,betaleftdeg;
    float betarightdeg,leftangle,rightangle;

    alphaleftdeg = alpharightdeg = betaleftdeg = betarightdeg = 0;
    deltaright = (3.141592654/180.0);
    deltaleft = -(3.141592654/180.0);

    for (i=0;i<=480;i++)
      hlclear(200,i,240,0);
    hlclear(200,1,240,254);
    vlclear(200,1,480,254);
    vlclear(440,1,480,254);
    line(200,180,295,480,254);
    line(440,180,345,480,254);
    line(236,288,406,288,253);

    for (i=1;i<=numberobj;i++)
      {
        if (objects[i].type==1)
          {
             objects[i].ycenter = (int)(480.0-((float)(objects[i].distance+2)
                                  *cos(objects[i].angle)*2.0));
             objects[i].xcenter = (int)(320.0+((float)(objects[i].distance+2)
                                  *sin(objects[i].angle)*2.0));
             
             if (type1cnt==0)
               {
                 level = 252;
                 type1cnt++;
                 target = i;
                 line(200,objects[i].ycenter,440,objects[i].ycenter,1);
               }
             else
                 level = 255;
             circle(objects[i].xcenter,objects[i].ycenter,3,5,6,253);
             circle(objects[i].xcenter,objects[i].ycenter,4,5,6,253);
             circle(objects[i].xcenter,objects[i].ycenter,22,5,6,level);
             circle(objects[i].xcenter,objects[i].ycenter,23,5,6,level);
             circle(objects[i].xcenter,objects[i].ycenter,24,5,6,level);
           }
         else
           {
             objects[i].ycenter = (int)(480.0-((float)(objects[i].distance+3)
                                  *cos(objects[i].angle)*2.0));
             objects[i].xcenter = (int)(320.0+((float)(objects[i].distance+3)
                                  *sin(objects[i].angle)*2.0));
             hlclear(objects[i].xcenter-9,objects[i].ycenter-9,19,253);
             hlclear(objects[i].xcenter-9,objects[i].ycenter+9,19,253);
             vlclear(objects[i].xcenter-9,objects[i].ycenter-9,19,253);
             vlclear(objects[i].xcenter+9,objects[i].ycenter-9,19,253);
             hlclear(objects[i].xcenter-30,objects[i].ycenter-30,61,255);
             hlclear(objects[i].xcenter-30,objects[i].ycenter+30,61,255);
             vlclear(objects[i].xcenter-30,objects[i].ycenter-30,61,255);
             vlclear(objects[i].xcenter+30,objects[i].ycenter-30,61,255);
             hlclear(objects[i].xcenter-30,objects[i].ycenter-29,61,255);
             hlclear(objects[i].xcenter-30,objects[i].ycenter+29,61,255);
             vlclear(objects[i].xcenter-29,objects[i].ycenter-30,61,255);
             vlclear(objects[i].xcenter+29,objects[i].ycenter-30,61,255);
           }
      }

    alphaleft = alpharight = objects[target].angle;
    betaleft = betaright = -(objects[target].angle);
    if (alpha(&alphaleft,deltaleft,&alphaleftdeg,&alphax,&alphay,target,
               &alphaleftlength) && (!alphaleftdeg))
      {
        *pathcnt = 1;
        *dist1 = objects[target].distance - 10;
        *angle1 = objects[target].angle;
        pix = rline(320,480,alphax,alphay,alphadata);
        for (i=pix;i>=(pix-alphaleftlength);i--)
          alphadata[i] = 255;
        wline(320,480,alphax,alphay,alphadata);
        return(1);
      }

    if (beta(&betaleft,deltaleft,&betaleftdeg,target,&betaleftlength))
      {
        pix = rline(320,480,alphax,alphay,alphadata);
        for (i=pix;i>=0;i--)
          {
            if (alphadata[i]==255)
              {
                wline(318,480,alphax,alphay,alphadata);
                alphaleftlength = (int)(((float)(pix-i))/2.25);
                i = -1;
              }
            else
                alphadata[i] = 255;
          }
        leftangle = (alphaleftdeg + betaleftdeg);
        leftdistance = alphaleftlength + betaleftlength;
      }
    else
        leftdistance = 0;

    alpha(&alpharight,deltaright,&alpharightdeg,&alphax,&alphay,target,
               &alpharightlength);
    if (beta(&betaright,deltaright,&betarightdeg,target,&betarightlength))
      {
        pix = rline(320,480,alphax,alphay,alphadata);
        for (i=(pix-2);i>=0;i--)
          {
            if (alphadata[i]==255)
              {
                wline(320,480,alphax,alphay,alphadata);
                alpharightlength = (int)(((float)(pix-i))/2.25);
                i = -1;
              }
            else
                alphadata[i] = 255;
          }
        rightangle = (alpharightdeg + betarightdeg);
        rightdistance = alpharightlength + betarightlength;
      }
    else
        rightdistance = 0;

    if ((!leftdistance) && (!rightdistance))
      *pathcnt = 0;
    else if ((!leftdistance)||((rightdistance)&&(rightdistance<leftdistance)))
      {
        *pathcnt = 2;
        *dist1 = alpharightlength;
        *dist2 = betarightlength;
        *angle1 = alpharight;
        *angle2 = -(alpharightdeg + betarightdeg);
      }
    else
      {
        *pathcnt = 2;
        *dist1 = alphaleftlength;
        *dist2 = betaleftlength;
        *angle1 = alphaleft;
        *angle2 = -(alphaleftdeg + betaleftdeg);
      }
  }

int alpha(theta,degree,deltadeg,alphx,alphy,target,length)

  float *theta,degree,*deltadeg;
  int *alphx,*alphy,target,*length;

  {
    int i,times,pix,test,linedata[1145];
float psi;
    times = 0;
    while (times < 90)
      {
        *alphx = (int)(320.0+(((float)(objects[target].distance+2))*
                 tan(*theta)*2.0));
        *alphy = objects[target].ycenter;
        pix = rline(320,480,*alphx,*alphy,linedata);
        for (i=pix;i>=0;i--)
          {
            test = linedata[i];
            linedata[i] = 50;
            switch (test)
              {
                case 1   : printf("%s\n","can line reached");
                case 254 : wline(320,480,*alphx,*alphy,linedata);
                           wline(319,480,(*alphx-1),*alphy,linedata);
                           return(1);
                           break;
                case 252 : line(320,480,*alphx,*alphy,50);
                           line(319,480,(*alphx-1),*alphy,50);
                           *length = pix - i;
                           return(1);
                           break;
                case 255 : i = -1;
                           printf("%s\n","obstacle, try new angle");
                           break;
              }
          }
        *deltadeg += degree;
        *theta += degree;
        times++;
 psi = *theta * (180.0/3.141592654);
 printf("%s%d%s%f%s\n","alpha try ",times," is ",psi," degrees");
      }
    return(0);
  }

int beta(theta,degree,deltadeg,target,length)

  float *theta,degree,*deltadeg;
  int target,*length;

  { 
    int i,j,times,pix,test,betax,betay,linedata[1145];
float si;
    times = 0;
    while ((*theta < 90.0) && (*theta > -90.0))
      {
        betax = (int)(((float)(objects[target].xcenter))+
                (((float)(objects[target].ycenter))*sin(*theta)*2.0));
        betay = (int)(((float)(objects[target].ycenter))+
                (((float)(objects[target].ycenter))*cos(*theta)*2.0));
        pix = rline(objects[target].xcenter,objects[target].ycenter,
                    betax,betay,linedata);
      if((linedata[0]==1)&&(linedata[1]==0)&&((linedata[2]==253)||
         (linedata[3]==253)||(linedata[4]==253)))
       {
        j = 0;
        while ((linedata[j]!=252) && (linedata[j]!=50))
          {
            j++;
            if (linedata[j]==50)
              {
                linedata[j-1] = 255;
                linedata[j] = 255;
                linedata[j+1] = 255;
                wline(objects[target].xcenter,
                      objects[target].ycenter,betax,betay,linedata);
                *length = (int)((float)(i-j)/2.25);
                return(1);
              } 
          }
        j++;
        j++;

        for (i=j;i<=pix;i++)
          {
            test = linedata[i];
            linedata[i] = 255;
            switch (test)
              {
                case 50   : linedata[i+1] = 255;
                            wline(objects[target].xcenter,
                                 objects[target].ycenter,betax,betay,linedata);
                           *length = (int)((float)(i-j)/2.25);
                           return(1);
                           break;
                case 254 : return(0);
                           break;
                case 255 : i = pix+1;
                           break;
              }
          }
       }
      else
       {
        j = pix - 1;
        while ((linedata[j]!=252) && (linedata[j]!=50))
          {
            j--;
            if (linedata[j]==50)
              {
                linedata[j-1] = 255;
                linedata[j] = 255;
                linedata[j+1] = 255;
                wline(objects[target].xcenter,
                      objects[target].ycenter,betax,betay,linedata);
                *length = (int)((float)(j-i)/2.25);
                return(1);
              }
          }
        j--;
        j--;
        for (i=j;i>=0;i--)
          {
            test = linedata[i];
            linedata[i] = 255;
            switch (test)
              {
                case 50   : linedata[i-1] = 255;
                            wline(objects[target].xcenter,
                                 objects[target].ycenter,betax,betay,linedata);
                           *length = (int)((float)(j-i)/2.25);
                           return(1);
                           break;
                case 254 : return(0);
                           break;
                case 255 : i = -1;
                           break;
              }
          }
       }
        *deltadeg += degree;
        *theta += degree;
        times++;
 si = *theta * (180.0/3.141592654);
 printf("%s%d%s%f%s\n","beta try ",times," is ",si," degrees");
      }
    return(0);
  }