/***********************************************************************/
/* */
/* FILE :sh_zenka_hilbert.c */
/* DATE :Tue, Feb 03, 2004 */
/* DESCRIPTION :Main Program */
/* CPU TYPE :SH7047 */
/* */
/* This file is generated by Renesas Project Generator (Ver.3.0). */
/* */
/***********************************************************************/
#include "start.h"

#define N 81
#define K 15

short fir[N]={
// 次数＝80　タップ数＝81 Q15
// サンプリング＝　26.93KHz
// 帯域端周波数 = 0.3 KHz - 13.165KHz


-260,-403,101,-255,11,-251,-11,-287,-16,-336,
-17,-394,-17,-462,-17,-539,-16,-629,-16,-734,
-15,-859,-14,-1010,-13,-1196,-12,-1434,-10,-1750,
-9,-2197,-7,-2885,-5,-4104,-4,-6913,-2,-20847,
0,20847,2,6913,4,4104,5,2885,7,2197,
9,1750,10,1434,12,1196,13,1010,14,859,
15,734,16,629,16,539,17,462,17,394,
17,336,16,287,11,251,-11,255,-101,403,
260
};

short fir2[K]={
// 次数＝14　タップ数＝15 Q15
// サンプリング＝　26.93KHz
// 帯域端周波数 = 1 KHz - 13.93KHz


-2811,1,-3323,1,-6417,0,-20677,0,20677,0,
6417,-1,3323,-1,2811

};

short data_up[N]={0};
short data_low[N]={0};

short sin_up[K]={0};
short sin_low[K]={0};

short data=0;
short out=0;
short moto_data=0;
short real_y=0;
short imag_y=0;

int output=0;
int data_chukan=0;

inline void kakunou(unsigned char j,short data3,unsigned char k)
{
    data_up[k-1-j]=data3; //下から格納する
    data_low[k-1-j]=data3; //同じ
}

inline void sin_kakunou(unsigned char j,short data3,unsigned char k)
{
    sin_up[k-1-j]=data3;
    sin_low[k-1-j]=data3;
}


inline int macw( short *a,short *b )
{
    int c;
    asm("clrmac");
    asm("mov.l %0,r2\n"
    "mov.l %1,r3"::"m" (a),"m" (b) );
    asm(".rept 81 \n" //ここのNだけは、手動で変える事
    "mac.w @r2+,@r3+\n"
    ".endr");
    asm("sts MACL,r4\n" //積和MACの下位だけr4に移す。
    "mov.l r4,%0":"=m" (c));
    return c; //int Cを返す
}
inline int macw2( short *a,short *b )
{
    int c;
    asm("clrmac");
    asm("mov.l %0,r2\n"
    "mov.l %1,r3"::"m" (a),"m" (b) );
    asm(".rept 15 \n" //ここのNだけは、手動で変える事
    "mac.w @r2+,@r3+\n"
    ".endr");
    asm("sts MACL,r4\n" //積和MACの下位だけr4に移す。
    "mov.l r4,%0":"=m" (c));
    return c; //int Cを返す
}
//************************************************************

int sinw=0 ; //Q15 a1=sinw　2KHz sampling 26.93Hz

int const1_cosd=0; //Q17 b0=1-cosd

//************************************************************

int main(void)
{

    InitSH();
    unsigned int i=0;
    unsigned int j=0;
    sinw=14742/120 ;
    const1_cosd=14013;
//initialize zenkashiki
    asm("mov.l %0,r9"::"m" (const1_cosd) ); //r9=1-cosd 2^17
    asm("mov.l %0,r10"::"m" (sinw) ); //r10=sinw
    asm("mov #0,r11" ); //r11=sin(w-deltaw)=0
    asm("mov.l r9, @-r15\n" //PUSH r9 [1-cosd]
    "mov.l r10, @-r15\n" //PUSH r10 [sinw]
    "mov.l r11, @-r15" ); //PUSH r11 [sin(w-d)]

    while(1)
    {

        if (AD0_READY)
        {

            data=getAD();
            data -=512;//無入力時のAD0の値


//AD変換の結果をhilbert変換器に通す---------------------------------------------------------------------
            kakunou(i,data,N);
            data_chukan=macw( (data_up+N-1-i),fir);

            data=data_chukan>>15;

            moto_data=data_up[N-1-i+(N-1)/2];//N-1-iから40ポイント前の点＝中点
   
//sin波形の、漸化式による発生
//*****************************************************************
            asm("mov.l @r15+,r11"); //POP r11 [sin(w-d)]
            asm("mov.l @r15+,r10"); //POP r10 [sinw]
            asm("mov.l @r15+,r9"); //POP r9 [1-cosd]

            asm("mov r9,r1"); //r1=r9=1-cosd r9 保存される
            asm("mov r10,r0"); //r0=r10=sinw

            asm("muls r1,r0"); //(1-cosd)*sinw --to MACL 2^32
            asm("shll16 r0"); // r0=r0*2^16 sinw 2^31
            asm("shll r0"); // r0=r0*2 sinw 2^32

            asm("sts MACL,r1"); //積をr1に移す r1=(1-cosd)*sinw
                                         //r0=sinw(2^32)
            asm( "sub r1,r0"); //r0=sinw-(1-cosd)*sinw=cosd*sinw
            asm("shlr16 r0"); //上の結果をシフト　2^32--2^16=2^16 = 2*(cosd*sinw) 2^15
            asm("exts.w r0,r1"); //r1=r0この結果を、符号拡張してr1に保存
            asm("sub r11,r1"); //2cosd*sinw-sin(w-deltaw)

            asm("mov r10,r11"); //r11=new sin(w-deltaw)
            asm("mov r1,r10" ); //r10=new sinw
            asm("mov.l r9,@-r15"); //PUSH r9 [1-cos]
            asm("mov.l r10,@-r15"); //PUSH r10 [sinw]
            asm("mov.l r11, @-r15");//PUSH r11 [sin(w-d)]
            asm("mov.w r10,%0":"=m" (out) );

            sin_kakunou(j,out,K); //一旦、配列に格納する

//作成したsin波を、hilbert変換器に通し、虚部を求める

            data_chukan=macw2( (sin_up+K-1-j),fir2);

            imag_y=data_chukan>>15;
            real_y=sin_up[K-1-j+(K-1)/2];//K-1-iから、(K-1)/2 ポイント前の点＝中点

            output=moto_data*real_y - data*imag_y;

            output=output>>8; //これは実測により決定する


            // output= (i%2) ? moto_data : data; //AD変換後の入力の、直交確認

            // output=(j%2) ? real_y : imag_y; //漸化式sinの直交確認

            output +=512;
            putDA(output);
            i++;
            j++;
            if( i==N) i=0;
            if( j==K) j=0;
         }

    }
return 1;
}