#include "header.h"

double CosVect[32]={0.87758256189037275873943144688383, 0.96891242171064473343022882545483, 0.99219766722932900560039115589461, 0.99804751070009911639857591580949, 0.99951175848513640342929420512519, 0.99987793217100662257479370964575, 0.99996948257709505902823821088532, 0.99999237061516998981147708036588, 0.99999809265197348029374779798673, 0.99999952316287965548013971783803, 0.99999988079071278068710171282873, 0.99999997019767772332698996251565, 0.99999999254941940307617187500000, 0.99999999813735485076904296875000, 0.99999999953433871269226074218750, 0.99999999988358467817306518554688, 0.99999999997089616954326629638672, 0.99999999999272404238581657409668, 0.99999999999818101059645414352417, 0.99999999999954525264911353588104, 0.99999999999988631316227838397026, 0.99999999999997157829056959599257, 0.99999999999999289457264239899814, 0.99999999999999822364316059974954, 0.99999999999999955591079014993738, 0.99999999999999988897769753748435, 1.00000000000000000000000000000000, 1.00000000000000000000000000000000, 1.00000000000000000000000000000000, 1.00000000000000000000000000000000, 1.00000000000000000000000000000000, 1.00000000000000000000000000000000};
 double SinVect[32]={0.47942553860420300537725779577158, 0.24740395925452293712787366075645, 0.12467473338522769288339020476997, 0.06245931784238020062494101125594, 0.03124491398532608030236268348290, 0.01562436422488337230130017729834, 0.00781242052738283145746356339600, 0.00390624006590011658632266566826, 0.00195312375823680404139659483320, 0.00097656234477957830613509271345, 0.00048828123059744659708800162967, 0.00024414062257468080430720946961, 0.00012207031219683510392653297272, 0.00006103515621210438968488251610, 0.00003051757812026304955764326177, 0.00001525878906190788161822188135, 0.00000762939453117598541403597198, 0.00000381469726561574828263361490, 0.00000190734863281134358826876107, 0.00000095367431640610547500337474, 0.00000047683715820310694761031164, 0.00000023841857910156025006873386, 0.00000011920928955078097206731418, 0.00000005960464477539059191277550, 0.00000002980232238769530919127755, 0.00000001490116119384765625000000, 0.00000000745058059692382812500000, 0.00000000372529029846191406250000, 0.00000000186264514923095703125000, 0.00000000093132257461547851562500, 0.00000000046566128730773925781250, 0.00000000023283064365386962890625};

double cordic(void)
{ 
  int i=0, Pos1=0, Pos2=0;
  double CosBin, SinBin;


  while(BinVect[i]==0)				//finds the first two places with 1's in BinVect
  {
    i++;
  }
  Pos1=i;
  i++;
  while(BinVect[i]==0)
  {
    i++;   
    if(i==32)
    {
      return SinVect[Pos1];			//breaks if there is no second 1 and returns the Sin
    }
  } 
  Pos2=i;
  i++;
  SinBin=SinVect[Pos1]*CosVect[Pos2]+CosVect[Pos1]*SinVect[Pos2]; //computes the initial sin and cos values after the first two 1's have been found.
  CosBin=CosVect[Pos1]*CosVect[Pos2]+SinVect[Pos1]*SinVect[Pos2];
  


  for(i=i;i<32;i++)				//continues to check each vector for a 1, if a 1 is found, new values for sin and cos will be made
  {
    if(BinVect[i]==1)
    {
      SinBin=SinBin*CosVect[i]+CosBin*SinVect[i];
      CosBin=CosBin*CosVect[i]+CosBin*SinVect[i];
    }
  }

  return SinBin;
}