//gcc -o pb pb.c -lm

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define pi 3.141592

//the index is the ratio of Vd/Vq
//column 1 is the difference already done
//column 0                  colunmn 1
// cos(atan(0))*32768       ([0][0]-[1][0])
// cos(atan(1/16*45))*32768 ([1][0]-[2][0])
// cos(atan(2/16*45))*32768 ([2][0]-[3][0])
// ...
//  this continues until 45 degrees
// .....
// cos(atan(17/16*45))*32768([16][0]-[17][0])


int tancos[18][2]={
{	32768	,	64	},
{	32704	,	189	},
{	32515	,	308	},
{	32207	,	417	},
{	31790	,	514	},
{	31276	,	594	},
{	30682	,	661	},
{	30021	,	712	},
{	29309	,	749	},
{	28560	,	773	},
{	27787	,	785	},
{	27002	,	788	},
{	26214	,	782	},
{	25432	,	772	},
{	24660	,	755	},
{	23905	,	735	},
{	23170	,	712	},
{	22458	,	22458	},
};


int getratio(int vd, int vq)
{
 long tanl;
 int tani;
 long magl,hypl;
 long ratiox1024;
 int i;

 if (vq<0) vq=-vq; //do not care which quaderant
 if (vd<0) vd=-vd;

 if(vq>=vd) //0-45 then vd/vq
  {
   tanl=vd; //make vd long
   tanl<<=15;//multiply by 32768
   tanl/=vq; //tanl now has the ratio*32768
   magl=vq;  //make vq long for later
   magl<<=15; //and multiply it up
 }
 else //90-45 then vq/vd
  {
   tanl=vq;  //same as above by vq and vd swapped
   tanl<<=15;
   tanl/=vd;
   magl=vd;
   magl<<=15;
  }
  
  i=tanl;  //tanl must be less than 32768
  i>>=11;  //divide by 2048 to see which 16th to use
  hypl=i;  //promote long
  tanl=tanl-hypl*2048;  //find how many counts off of index
  tanl=tanl*tancos[i][1]/2048;  //this is the linear interpolation
  hypl=tancos[i][0]-tanl;    //put in the estimate of the cos
  magl/=hypl;  //with the cos calculate the hypotenus (x/cos)
   if(magl>32767) //to big, reduce it
       {
        ratiox1024=32767*1024; //get it in long form
        ratiox1024/=magl;    //divide it by the to big hypot
        return (ratiox1024);  //this is the ratio to return
       }

    return(1024);  //no need to reduce so return 1
}

void main()
{
double vd, vq, mag, angle, ratio, magld, perror, degree;
int vdi, vqi, ratiox1024;
long vdl, vql, magl;

 printf("Enter Mag:");   //try this mag and angle
 fscanf(stdin,"%lf",&mag);
 printf("Enter Angle:");
 fscanf(stdin,"%lf",&angle);
 vq=mag*cos(angle*pi/180); //calc in float actual values
 vd=mag*sin(angle*pi/180);
 vdi=vd+0.5;  //convert to integer
 vqi=vq+0.5;

 printf("Vd=%lf and Vq=%lf\n\r",vd,vq);   
 printf("Vdi=%d and Vqi=%d\n\r",vdi,vqi);
 
 if(mag>32767.0)  //in float what should it look like
  {
   printf("Mag to big reducing size\n\r");
   ratio = 32767.0/mag;
   vd *= ratio;
   vq *= ratio;
   printf("Vd=%lf and Vq=%lf\n\r",vd,vq);
  }
else printf("Nothing to do\n\r");

ratiox1024=getratio(vdi,vqi); //get the reduciton amount

vdl=vdi;  //do this in long
vql=vqi;

vdl=vdl*ratiox1024/1024;  //reduce it
vql=vql*ratiox1024/1024;

vdi=vdl;   //put it back into int
vqi=vql;
magl = sqrt(pow(vdl,2)+pow(vql,2));  //what is the mag for comparison

 printf("Vdi=%d and Vqi=%d and mag=%ld\n\r",vdi,vqi,magl);

magld=magl;  //to do comparison with

//check the numbers and percent error
 mag = sqrt(pow(vd,2)+pow(vq,2));
 angle = atan(vd/vq)*180/3.141592;
perror=(magld-mag)/mag*100.0;

 printf("Error = %lf\n\r",perror);
 printf("Magnitude = %lf\n\r",mag);
 printf("Angle= %lf\n\r",angle);
 
return;
}