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

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


#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 hosu_increment();
u16 get_long_hour();




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

    if( system_status.pwr_state == ON )
    {
        // 加速度センサデータレジスタへの反映
        if( iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, &vreg_ctr[VREG_C_ACC_XL] )
            == ERR_SUCCESS )
        {
            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 );
            }
        }
        else
        {
            // 加速度センサが異常になったので止める
            vreg_ctr[VREG_C_ACC_CONFIG] &= ~( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ );
            acc_hosu_set();
            return ( ERR_SUCCESS ); // タスクの削除は必要
        }
    }

    // 歩数計 /////////////////////////////////////////////
    // 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;
                    hosu_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 );
}



/*=========================================================
　歩数＋１
　　累積をインクリメント
  　履歴を更新
 ========================================================*/
#define MAGIC_PEDO_NODATA 65535
#define MAGIC_PEDO_MAX    65534

u8 p_record;

static void hosu_increment()
{
    static u16 last_hour;        // 履歴の最新は何時？
    static u8  last_year;

    // 累積の更新 //
    // いろいろ失敗した...
    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;
            }
        }
    }

    // 毎時履歴の更新
    {
        u16 now_hour;
        u8 now_year;

        // 時計を止める必要が有るので↓は一気に行って下さい
        DI();
        RWAIT = 1;
        while( !RWST ){;}
        now_year = bcdtob( YEAR );
        now_hour = get_long_hour();
        // RWAIT = 0;  ↑で行っています
        // EI();             〃

        // 歩数計が止まったいた時間を考慮して必要なら進める //
        if( last_year == now_year )
        {
            if( now_hour > last_hour )
            {
                fill_hosu_hist_hours( now_hour - last_hour );
            }
        }
        else if( last_year == ( now_year -1 ) )
        {
            // 年をまたいでいるとき
            u16 temp_hours = ( ( 365 + (( now_year & 0x03 ) == 1 )? 1: 0 ) * 24 ) - last_hour + now_hour;
            fill_hosu_hist_hours( temp_hours );
        }
        else if( last_year < now_year )
        {
            // 数年放置
            fill_hosu_hist_hours( 255 );
        }
        else
        {
            // 巻き戻るなど
            // 知らない…
        }

        last_year = now_year;
        last_hour = now_hour;

        // 実際にインクリメント
        *( hyst_pedometer + p_record ) += 1;
        if( *( hyst_pedometer + p_record ) > MAGIC_PEDO_MAX )
        {
            *( hyst_pedometer + p_record ) = MAGIC_PEDO_MAX;
        }

    }
}



void fill_hosu_hist_hours( u16 hours )
{
    if( hours > 255 )
    {
      hours = 255;
    }

    hours -= 1;

    // 空白の数時間の設定
    while( hours != 0 )
    {
        p_record += 1;
        *( hyst_pedometer + p_record ) = MAGIC_PEDO_NODATA;
        hours -= 1;
    }
    p_record += 1;
    *( hyst_pedometer + p_record ) = 0;
}






bit hosu_read_inited;
bit record_read_msb_lsb;

/* ========================================================
  歩数計ヒストリ読み出しの後処理(初期化)
  読み出しポインタのクリア
======================================================== */
void hosu_read_end(  )
{
    record_read_msb_lsb = 0;
    hosu_read_inited = 0;
}

/* ========================================================
  歩数計のヒストリを返す。
  １回呼ぶ度に、ヒストリの下位、上位、一時間遡って下位上位...
======================================================== */
u8 hosu_read(  )
{
    u16 temp;
    u8 rv;

    static u8 p_record_buffer;

    if( hosu_read_inited == 0 )
    {
        p_record_buffer = p_record;
        hosu_read_inited = 1;
    }

    temp = *( hyst_pedometer + p_record_buffer );
    if( record_read_msb_lsb == 0 )
    {
        rv = (u8)( ( temp & 0xFF00 ) >> 8 );
    }
    else
    {
        rv = (u8)( temp & 0x00FF );
        p_record_buffer -= 1;
    }
    record_read_msb_lsb += 1;

    return( rv );
}


void hosu_init()
{
    u8 i = 0;

    hyst_pedometer = (u16*)pool;
    do
    {
        *( hyst_pedometer + i ) = MAGIC_PEDO_NODATA;
        i += 1;
    }while( i != 0 );
}





/*=========================================================
　加速度センサ透過アクセス　リード
 ========================================================*/
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 );
            }
        }
    }
}



/* ========================================================
  今年の元旦からの経過時間(hour)を返す。
  引数  無し
  返値  u16 long_hour
======================================================== */
static const u16 DAYS_FROM_HNY[] = {
    0,
    0, 31, 31+28, 59+31, 90+30,
    120+31, 151+30, 181+31, 212+31, 243+30,
    273+31, 304+30 };

u16 get_long_hour()
{
    u16 long_hour;
    u8  year_hex;
    u8  month_hex;
    u8  day_hex;
    u8  hour_hex;

    year_hex  = bcdtob( YEAR );
    month_hex = bcdtob( MONTH );
    day_hex   = bcdtob( DAY );
    hour_hex  = bcdtob( HOUR );

    RWAIT = 0;
    EI();

    // まず日数の部分
    long_hour = DAYS_FROM_HNY[ month_hex ];
    if(( ( year_hex & 0x03 ) == 0 ) && ( ( 3 <= month_hex )
                                         || (( 2 == month_hex ) && ( day_hex == 29 )) ))
    {
        // 閏年で、閏日より後
        long_hour += 1;
    }
    long_hour += day_hex;
    long_hour *= 24; // 日数→時間

    long_hour += hour_hex;

    return( long_hour );
}