/* ========================================================
　加速度センサ関係
・データ更新完了でデータを吸い上げ手レジスタを更新、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 )

// ========================================================
extern u8 pool[];
u16* hyst_pedometer;


// ========================================================
task_interval tsk_soft_int(  );
static void pedo_increment();





/* ========================================================
　・割り込みを確認してデータを吸い上げ、レジスタに書き出します
  ・本当であればコールバック関数を登録しておけばいいじゃんとなるのですが、
  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] );

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

        // ゴミデータのカラ読み
        if( ACC_VALID == 1 )
        {
            u8 temp[6];
            iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, temp );
        }
    }

    // 歩数計 /////////////////////////////////////////////
    // offでなければ、電源off中でも計測
//    if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_HOSU ) != 0x00 )
    {
        static s16 th_H = 0x3500; // 閾値。暫定。動的変更とかしたい…ので変数
        static s16 th_L = 0x2A00;
        static u16 acc_norm[3]; // 加速度の大きさのヒストリ。数字が大きい方が古い
        static u16 interval_hh; // 山-山間の時間。短過ぎたらはじく。
        static u16 time_l;       // 前回の極小からの経過時間


        u16 sx16 = abs( (u16)vreg_ctr[VREG_C_ACC_XH] * 256 + vreg_ctr[VREG_C_ACC_XL] );
        u16 sy16 = abs( (u16)vreg_ctr[VREG_C_ACC_YH] * 256 + vreg_ctr[VREG_C_ACC_YL] );
        u16 sz16 = abs( (u16)vreg_ctr[VREG_C_ACC_ZH] * 256 + vreg_ctr[VREG_C_ACC_ZL] );

        acc_norm[2] = acc_norm[1];
        acc_norm[1] = acc_norm[0];
            // そのうちローコストな方法を考える、かも
        acc_norm[0] = sqrt( (long)sx16 * ( sx16 / 2 ) +
                            (long)sy16 * ( sy16 / 2 ) +
                            (long)sz16 * ( sz16 / 2 ) );

        if( acc_norm[2] <= acc_norm[1] && acc_norm[1] > acc_norm[0]
            && acc_norm[0] > th_H )
        // 極大で、閾値を超えていた
        {
            if(( 25 < interval_hh ) && ( interval_hh < 200 ))
            // 前回の極大からの間隔がほどよい
            {
                DBG_LED_WIFI_on;
                if( time_l < interval_hh )
                // 谷を挟んでいる
                {
                    // 一歩増えました
                    DBG_LED_WIFI_2_on;
                    pedo_increment();
                }
            }
            interval_hh = 0;
        }
        else
        {
            interval_hh += ( interval_hh != 65535 ) ? 1: 0; // 飽和加算って楽に書けたらいいのに
        }
        DBG_LED_WIFI_off;
        DBG_LED_WIFI_2_off;

        // (2) 直近の極小からの時間
        if( acc_norm[2] >= acc_norm[1] && acc_norm[1] < acc_norm[0]
            && acc_norm[0] < th_L )
        {
            time_l = 0;
        }
        else
        {
            time_l += ( time_l != 65535 ) ? 1: 0;
        }
    }
    return ( ERR_SUCCESS );
}



/*=========================================================
　歩数＋１
　　累積をインクリメント
  　履歴を更新
 ========================================================*/
static void pedo_increment()
{
    static u8 last_hour;        // 履歴の最新は何時？
    static u8 last_day;
    static u8 last_month;
    static u8 last_year;
    static u8 p_record;

    // 累積の更新 //
    // いろいろ失敗した...
    if( ++vreg_ctr[ VREG_C_ACC_HOSU_L ] == 0 )
    {
        if( ++vreg_ctr[ VREG_C_ACC_HOSU_M ] == 0 )
        {
            if( ++vreg_ctr[ VREG_C_ACC_HOSU_H ] == 0 ){
                vreg_ctr[ VREG_C_ACC_HOSU_L ] = 255; // カンスト orz
                vreg_ctr[ VREG_C_ACC_HOSU_M ] = 255;
                vreg_ctr[ VREG_C_ACC_HOSU_H ] = 255;
            }
        }
    }

    // 毎時履歴の更新
    DI();
    RWAIT = 1;
    while( !RWST ){;}

    if(( last_hour != HOUR ) || ( last_day != DAY ) || ( last_month != MONTH ) || ( last_year != YEAR ))
    {
        last_hour = HOUR;
        last_day = DAY;
        last_month = MONTH;
        last_year = YEAR;
        p_record += 1;
        *(hyst_pedometer + p_record) = 0;
    }
    RWAIT = 0;
    EI();

    *(hyst_pedometer + p_record) += 1;
    if( *( hyst_pedometer + p_record ) == 65535 )  // 65535はマジックナンバーで
    {
        *hyst_pedometer = 65534;
    }
}




/*=========================================================
　加速度センサ透過アクセス　リード
 ========================================================*/
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 )
    {
        vreg_ctr[ VREG_C_STATUS_1 ] |= REG_BIT_ACCERO_ERR;
#ifdef _MCU_BSR_
//	    PMK23 = 1;
#endif
        return ( ERR_SUCCESS ); // とりあえず、タスクは削除しなくてはならない
    }else{
        vreg_ctr[ VREG_C_STATUS_1 ] &= ~REG_BIT_ACCERO_ERR;
    }


    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
        // 100Hz 自動取り込み
        str_send_buf[0] =
            ( ACC_BITS_PM_NORM << ACC_bP_PM0
              | ACC_BITS_DR_100Hz << ACC_bP_DR0
              | 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 )
        {
            u8 temp[6];
            iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, temp );
        }
    }
    return ( ERR_SUCCESS );
}



/* ========================================================
 加速度センサ割り込み
  I2Cが使用中かもしれないので、読み出しタスクの登録を行うのみ
 ======================================================== */
__interrupt void intp23_ACC_ready(  )
{
    EI();
    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 );
            }
        }
    }
}