/* ========================================================
　加速度センサ関係
・データ更新完了でデータを吸い上げ手レジスタを更新、CPUに割り込み
・フラグが立っていれば歩数カウント
・加速度センサ割り込みからタスクを登録して下さい。（I2Cの競合回避などがあるので）

 ======================================================== */
#pragma SFR
#pragma NOP
#pragma HALT
#pragma STOP
#pragma ROT
// rorb, rolb, rorw, rolw
#pragma MUL

#include "incs.h"
#include <math.h>


// ========================================================
// レジスタ名
#define ACC_REG_WHOAMI 0x0F
#define ACC_REG_CTRL1  0x20
#define ACC_REG_CTRL5  0x24
#define ACC_REG_X      0x28

// ビット位置
#define ACC_bP_PM0    5
#define ACC_bP_DR0    3

// ビット設定値
#define ACC_BITS_PM_PDN   0
#define ACC_BITS_PM_NORM  1
#define ACC_BITS_PM_LP0R5 2
#define ACC_BITS_PM_LP1   3
#define ACC_BITS_PM_LP2   4
#define ACC_BITS_PM_LP5   5
#define ACC_BITS_PM_LP10  6

#define ACC_BITS_DR_50Hz   0
#define ACC_BITS_DR_100Hz  1
#define ACC_BITS_DR_400Hz  2
#define ACC_BITS_DR_1000Hz 3

#define ACC_BITS_ALL_AXIS_ON 7


#define VREG_BITMASK_ACC_CONF_HOSU ( 1 << 1 )
#define VREG_BITMASK_ACC_CONF_ACQ  ( 1 << 0 )

// ========================================================
static u8 hyst_pedometer[256];


// ========================================================
task_interval tsk_soft_int(  );




/* ========================================================
　・割り込みを確認してデータを吸い上げ、レジスタに書き出します
  ・本当であればコールバック関数を登録しておけばいいじゃんとなるのですが、
  I2Cが使用中だったら？とか考えると私ではそこまでできないのです。
  ・自動歩数計とかでも結局
 ======================================================== */
task_status_immed tsk_cbk_accero(  )
{                               // （疑似）isrから登録されます

    if( system_status.pwr_state == ON )
    {
        // 加速度センサデータレジスタへの反映
        iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, &vreg_ctr[VREG_C_ACC_XL] );

        {
            // 歩数計 //
            unsigned int mean;          // 二乗平均はあまりにも処理が重いので(!) 絶対値の合計にします
            static u8 direction;

            static s16 th_H = 0x2380;   // 閾値。暫定。動的変更とかしたい…
            static s16 th_L = 0x2180;

            static u16 interval;        // 山と谷の間の時間。短すぎても長すぎてもはじく。


            s16 sx16 = vreg_ctr[VREG_C_ACC_XH] * 256 + vreg_ctr[VREG_C_ACC_XL];
            s16 sy16 = vreg_ctr[VREG_C_ACC_YH] * 256 + vreg_ctr[VREG_C_ACC_YL];
            s16 sz16 = vreg_ctr[VREG_C_ACC_ZH] * 256 + vreg_ctr[VREG_C_ACC_ZL];

            // そのうちローコストな方法を考え
            mean = sqrt( (long)abs( sx16 ) * abs( sx16 ) /4 +
                         (long)abs( sy16 ) * abs( sy16 ) /4 +
                         (long)abs( sz16 ) * abs( sz16 ) /4 );

            {
                static u8 count_H = 0;
                static u8 count_L = 0;

                if( direction == 0 ) // 前回に下限を下回っていて
                {
                    if( mean > th_H ) // 今回、上の閾値を上回った
                    {
                        if( count_H == 5 ){  // 突発的なノイズは省く...
                            direction = 1;
                            count_L = 0;
                        }
                        if( count_H <= 5 ){
                            count_H += 1;
                        }
                    }
                    else
                    {
                        count_H = 0;
                    }
                }

                if( count_H >= 5 ){
                    interval += 1;
                }
                if( 500 < interval ){
                    interval = 0;
                    direction = 0;
                    count_H = 0;
                    count_L = 0;
                }

                // 下の閾値を超えるのを待つ
                if( direction == 1 )
                {
                    if( mean < th_L )
                    {
                        if( count_L == 5 ){
                            direction = 0; // 次は上の閾値を待つ
                            if( count_H >= 5 ){
                                if( 70 < interval )
                                {
                                    vreg_ctr[ VREG_C_ACC_HOSU_L ] += 1; // 一歩加算
                                    hyst_pedometer[0] += 1;
                                }
                            }
                            count_H = 0;
                        }
                        if( count_L <= 5 ){
                            count_L += 1;
                        }
                    }
                    else
                    {
                        count_L = 0;
                    }
                }
            }

            // debug
            if( mean > th_H )
            {
                DBG_LED_WIFI_on;
                DBG_LED_WIFI_2_off;
            }

            if( mean < th_L )
            {
                DBG_LED_WIFI_off;
                DBG_LED_WIFI_2_on;
            }

        }


        if( ( vreg_ctr[VREG_C_ACC_CONFIG] &
              ( VREG_BITMASK_ACC_CONF_ACQ | VREG_BITMASK_ACC_CONF_HOSU ) ) != 0 )
        {
            set_irq( VREG_C_IRQ1, REG_BIT_ACC_DAT_RDY );
        }
    }

    // 歩数計 offでなければ、電源off中でも計測
    if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_HOSU ) != 0x00 )
    {
        if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0x00 )
        {
            // 歩数計アルゴリズム 100Hz版
        }
        else
        {
            // 同　省電力版
        }

    }
    return ( ERR_SUCCESS );
}





/*=========================================================
　加速度センサ透過アクセス　リード
 ========================================================*/
task_status_immed acc_read(  )
{
    vreg_ctr[VREG_C_ACC_W_BUF] = iic_mcu_read_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_R_ADRS] );
//  vreg_ctr[ VREG_C_ACC_R_BUF ] = iic_mcu_read_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_R_ADRS] );
    vreg_ctr[VREG_C_IRQ1] |= REG_BIT_ACC_ACK;
    if( ( vreg_ctr[VREG_C_IRQ_MASK1] & REG_BIT_ACC_ACK ) == 0 )
    {
        IRQ0_ast;
    }
    return ( ERR_SUCCESS );
}



/*=========================================================
　加速度センサ透過アクセス　ライト
 ========================================================*/
task_status_immed acc_write(  )
{
    iic_mcu_write_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_W_ADRS], vreg_ctr[VREG_C_ACC_W_BUF] );
    vreg_ctr[VREG_C_IRQ1] |= REG_BIT_ACC_ACK;
    if( ( vreg_ctr[VREG_C_IRQ_MASK1] & REG_BIT_ACC_ACK ) == 0 )
    {
        IRQ0_ast;
    }
    return ( ERR_SUCCESS );
}



/*=========================================================
　自動歩数カウントモードにセット
  todo 他のモードだったら止めたり、復帰させたり
  割り込みルーチンなどでカウント判定が必要
 ========================================================*/
task_status_immed acc_hosu_set(  )
{
    u8      str_send_buf[4];

    iic_mcu_read_a_byte( IIC_SLA_ACCEL, ACC_REG_WHOAMI );
    if( iic_mcu_bus_status == ERR_NOSLAVE )
    {
        return ( ERR_SUCCESS ); // とりあえず、タスクは削除しなくてはならない
    }

    str_send_buf[1] = 0x00;     // ctrl2  HPF:normal, filterd, HPF for IRQ : dis/dis, HPF coeff:norm
#ifdef _MODEL_WM0_
    str_send_buf[2] = 0x10;     //     3  IRQ pol :Active HI, Drive:Pushpull,
                                ///       IRQ2flg latch: auto clear after read, IRQ2 conf: IRQ( fall,shock,...)
                                ///          1         : auto clear after read,      conf: data ready
#else
    str_send_buf[2] = 0x02;     //     3  IRQ pol :Active HI, Drive:Pushpull,
                                ///       IRQ2flg latch: auto clear after read, IRQ2 conf: IRQ( fall,shock,...)
                                ///          1         : auto clear after read,      conf: data ready
#endif
    str_send_buf[3] = 0x80;     // ctrl3  block update:enable, MSB first, scale: +-2G(default), selftest: dis

    if( ( vreg_ctr[VREG_C_ACC_CONFIG] &
          ( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ ) ) == 0 )
    {
#ifdef _MCU_BSR_
	    PMK23 = 1;
#endif
        // 完全停止
        str_send_buf[0] =
            ( ACC_BITS_PM_PDN << ACC_bP_PM0 | 0 << ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
    }
    else
    {
#ifdef _MCU_BSR_
	    PMK23 = 0;
#endif
        if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0x00 )
        {
            // 100Hz 自動取り込み
            str_send_buf[0] =
                ( ACC_BITS_PM_NORM << ACC_bP_PM0 | ACC_BITS_DR_100Hz <<
                  ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
        }
        else
        {
            // 100Hz 自動取り込み
            str_send_buf[0] =
                ( ACC_BITS_PM_NORM << ACC_bP_PM0 | ACC_BITS_DR_100Hz <<
                  ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
//        // 10Hz 自動取り込み(歩数計向け省電力モード)
//        str_send_buf[0] = ( ACC_BITS_PM_LP10 << ACC_bP_PM0 | ACC_BITS_ALL_AXIS_ON );
        }
    }
    iic_mcu_write( IIC_SLA_ACCEL, ( ACC_REG_CTRL1 | 0x80 ), 4, str_send_buf );
    if( ACC_VALID == 1 )
    {
        if( system_status.pwr_state == ON )
        {
            renge_task_immed_add( tsk_cbk_accero );
        }
    }

    return ( ERR_SUCCESS );
}



/* ========================================================
 加速度センサ割り込み
  I2Cが使用中かもしれないので、読み出しタスクの登録を行うのみ
 ======================================================== */
__interrupt void intp23_ACC_ready(  )
{
    if( ( vreg_ctr[VREG_C_ACC_CONFIG] & 0x03 ) != 0x00 )
    {
        if( ( system_status.pwr_state == ON ) || ( system_status.pwr_state == SLEEP ) )
        {
            if( ACC_VALID )
            {
                renge_task_immed_add( tsk_cbk_accero );
            }
        }
    }
}