/* =================================================================================
File name:        SPEED_PR.C
===================================================================================*/


#include "speed_pr.h"

long StatorRpmSum = 0;
long StatorArr[8] = {0,0,0,0,0,0,0,0};//,0,0,0,0,0,0,0,0};//,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,};
void SPEED_PR_MACRO(SPEED_MEAS_CAP *v,unsigned int hall,unsigned int hall_last,unsigned int stall_status)	{
    static int j = 0;


    long StatorRpm = 0;
   if (v->InputSelect == 0)
   {
     v->OldTimeStamp = v->NewTimeStamp;
     v->NewTimeStamp = v->TimeStamp;
    // v->EventPeriod = v->NewTimeStamp - v->OldTimeStamp;

    //  	if (v->EventPeriod < 0)
  	 //   {
  	 //   v->EventPeriod += 32767; }  // 0x7FFF = 32767

     StatorRpm  = v->NewTimeStamp - v->OldTimeStamp;

     if (StatorRpm < 0)
         StatorRpm += 32767;
    }

          StatorArr[j] = StatorRpm;
          StatorRpmSum += StatorArr[j];
          v->EventPeriod = StatorRpmSum >> 3;

          j++;
          if (j >= 8)
          {
               j = 0;
           }
          StatorRpmSum -= StatorArr[j];

   if(stall_status == 1){
          v->SpeedRpm = 0;
          v->Speed = 0;
         /* StatorRpmSum = 0;
          for(i=0;i<32;i++){
              StatorArr[i] = 0;
              j = 0;
          }*/

   }else{
       if(hall_last == 4 && hall == 5 )
              {
                  v->Speed = _IQdiv(v->SpeedScaler,v->EventPeriod)/10;
                  //StatorRpmQ = _IQdiv(v->SpeedScaler,v->EventPeriod*6);
              }
              else if(hall_last == 1 && hall == 5)
              {
                  v->Speed = -_IQdiv(v->SpeedScaler,v->EventPeriod)/10;
                  //StatorRpmQ = -_IQdiv(v->SpeedScaler,v->EventPeriod*6);
              }
      /* if(hall_last == 2 && hall == 6 || hall_last == 6 && hall == 4 ||hall_last == 4 && hall == 5 ||hall_last == 5 && hall == 1||hall_last == 1 && hall == 3||hall_last == 3 && hall == 2)
       {
           v->Speed = _IQdiv(v->SpeedScaler,v->EventPeriod)/10;
           //StatorRpmQ = _IQdiv(v->SpeedScaler,v->EventPeriod*6);
       }
       else if(hall_last == 4 && hall == 6 || hall_last == 6 && hall == 2 ||hall_last == 2 && hall == 3 ||hall_last == 3 && hall == 1||hall_last == 1 && hall == 5|| hall_last == 5 && hall == 4)
       {
           v->Speed = -_IQdiv(v->SpeedScaler,v->EventPeriod)/10;
           //StatorRpmQ = -_IQdiv(v->SpeedScaler,v->EventPeriod*6);
       }*/

    /*   StatorRpm = _IQmpy(v->BaseRpm,StatorRpmQ);

       StatorArr[j] = StatorRpm;
       StatorRpmSum += StatorArr[j];
       v->SpeedRpm = StatorRpmSum >> 5;
       v->Speed = _IQdiv(v->SpeedRpm,v->BaseRpm);
       j++;
       if (j >= 32)
       {
            j = 0;
        }
       StatorRpmSum -= StatorArr[j];*/

    /* Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q*/
       v->SpeedRpm = _IQmpy(v->BaseRpm,v->Speed);

  }
}

