/* ********************************************************
 歩数計
 3軸加速度のリアルタイムデータから、ベクトルのノルムを出し、
 閾値を超える時間、間隔、ノルムの大きさで閾値を切り替えるなど
********************************************************* */
#pragma mul
#pragma div
#pragma bcd

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

#include "accero.h"
#include "pedometer.h"

#include "pedo_lpf_coeff.h"
#include "pool.h"

// ========================================================
static void hosu_increment();



// ========================================================
u16 get_long_hour();
extern uni_pool pool;



// ========================================================
#define _use_my_sqrt_
#ifdef _use_my_sqrt_
unsigned long my_sqrt();
#endif



/*=========================================================
　歩数計
 ========================================================*/
void pedometer()
{
    static s16 th_H = 15000; // 閾値。暫定。動的変更とかしたい…ので変数
    static s16 th_L = 11000;
    static u16 acc_norm[3]; // 加速度の大きさのヒストリ。数字が大きい方が古い
    static u16 acc_norm_temp;
    static u8 interval_hh;  // 山-山間の時間。短過ぎたらはじく。
    static u8 time_l;       // 前回の極小からの経過時間
    static u16 peak_l;      // 谷の深さ
    static u16 norm_hist[TAP];

    static u8 hist_indx;

    signed long filterd;

    u8 i;

    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] );

    // ベクトルのノルム
#ifdef _mcu_
# ifndef _use_my_sqrt_
    norm_hist[ hist_indx & TAP-1 ] = sqrt( (long)sx16 * ( sx16 / 2 ) +
                                           (long)sy16 * ( sy16 / 2 ) +
                                           (long)sz16 * ( sz16 / 2 )
                                           );
# else
    norm_hist[ hist_indx & TAP-1 ] = my_sqrt( (long)sx16 * ( sx16 / 2 ) +
                                              (long)sy16 * ( sy16 / 2 ) +
                                              (long)sz16 * ( sz16 / 2 )
                                              );
# endif
#endif

#ifdef _pc_
    norm_hist[ hist_indx & TAP-1 ] = normh * 256 + norml;
#endif

    hist_indx += 1;

    // ヒストリにフィルタを掛けて、今回の値を求める
    filterd = 0;
//    for( i = 8; i != 55; i++ )  // 係数が０ばかりのため
    for( i = 0; i != 46; i++ )  // 係数テーブルをいじりました。パラメータ調整時注意
    {
        filterd += (signed long)norm_hist[ ( hist_indx + i ) & TAP-1 ] * lpf_coeff[ i ];
    }

    filterd += (4096)*512;
    acc_norm_temp = (s16)( filterd /1024 & 0xFFFF ); // ←FIL_COEFF_QUANTから正規化
/*
    if( acc_norm[0] < acc_norm_temp )
    {
        t_rise += 1;
        if( t_rise == 0 )
            t_rise == 254;
    }
    else
    {
        t_rise = 0;
    }
*/
    if( acc_norm[0] != acc_norm_temp )
    {
        acc_norm[2] = acc_norm[1];  // ヒストリ
        acc_norm[1] = acc_norm[0];
        acc_norm[0] = acc_norm_temp;
    }

    if( acc_norm[2] <= acc_norm[1] && acc_norm[1] > acc_norm[0]
        && acc_norm[0] > th_H )
        // 極大で、閾値を超えていた
    {
        if( 21 < interval_hh )
            // 前回の極大からの間隔がほどよい
        {
            if(( interval_hh < 160 ) && ( time_l < interval_hh ))
                // 谷を挟んでいる
            {
                if( acc_norm[0] - peak_l > 4200 ){
                    // 一歩増えました
                    hosu_increment();
                }
            }
            interval_hh = 0;
        }
        if( acc_norm[0] > 18000 )
        {
            th_L = acc_norm[0] - 10000;
        }
        else
        {
            th_L = 11000;
        }
    }
    else
    {
        interval_hh += ( interval_hh != 255 ) ? 1: 0; // 飽和加算って楽に書けたらいいのに
    }

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

#ifdef _DBG_PEDO_AUTO_ENABLE_
    {
        static u8 i = 0;

        vreg_ctr[ 0x50 ] = i++;

        vreg_ctr[ 0x51 ] = (u8)( acc_norm[0] / 256 & 0x00FF );
        vreg_ctr[ 0x52 ] = (u8)( acc_norm[0] & 0x00FF );

        vreg_ctr[ 0x53 ] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] / 256 & 0xFF );
        vreg_ctr[ 0x54 ] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] & 0xFF );

        vreg_ctr[ 0x55 ] = interval_hh;
        vreg_ctr[ 0x56 ] = time_l;

        vreg_ctr[ 0x57 ] = vreg_ctr[ VREG_C_ACC_HOSU_L ];
        vreg_ctr[ 0x58 ] = (u8)( peak_l / 256 & 0x00FF );
        vreg_ctr[ 0x59 ] = (u8)( peak_l & 0x00FF );
//        vreg_ctr[ 0x5A ] = (u8)( norm_avg[0] / 256 & 0x00FF );
//        vreg_ctr[ 0x5B ] = (u8)( norm_avg[0] & 0x00FF );
    }
#endif
}



/*=========================================================
　歩数＋１
　　累積をインクリメント
  　履歴を更新
 ========================================================*/
u8 p_record;
u8 last_hour = 0x23;                   // 履歴の最新は何時？
u8 last_day = 0x30;
u8 last_month = 0x12;
u8 last_year = 0x99;
u8 now_min;
u8 now_sec;
u8 log_year;


#define HOSU_NODATA 0xFFFF
#define HOSU_MAX    0xFFFE

static void hosu_increment()
{
    static u16 last_hour_fny;

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

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

        // 履歴読み出し時に使用。BCDのままでよい
        last_hour = HOUR;
        last_day = DAY;
        last_month = MONTH;
        now_min = MIN;
        now_sec = SEC;
        now_longhour = get_long_hour();

        // RWAIT = 0;  ↑で行っています
        // EI();             〃

        // 元旦零時台で昨日扱いになった場合の帳尻合わせ
        if( now_longhour == 65535 )
        {
            now_longhour = ( ( 365 + (( now_year & 0x03 ) == 1 ? 1: 0 )) * 24 ) -1;
            now_year   -= 1;
        }

        // 歩数計が止まっていた時間を考慮して必要なら進める //
        if( last_year == now_year )
        {
            if( now_longhour > last_hour_fny )
            {
                fill_hosu_hist_hours( now_longhour - last_hour_fny );
            }
        }
        else if( last_year == ( now_year -1 ) )
        {
            // 年をまたいでいるとき
            fill_hosu_hist_hours( ( ( 365 + (( now_year & 0x03 ) == 1 ? 1: 0 )) * 24 ) - last_hour_fny + now_longhour );
        }
        else if( last_year < now_year )
        {
            // 数年放置
            fill_hosu_hist_hours( 0 );
        }
        else
        {
            // カレンダーが巻き戻るなど
            // ノーケアでよい
        }
        last_year = now_year;
        last_hour_fny = now_longhour;

        // 実際にインクリメント
        {
            u16* p_pedo_data = &pool.vreg_c_ext.pedo_log[ p_record ];

            if( *p_pedo_data == HOSU_NODATA )
            {
                *p_pedo_data = 1;
            }
            else if( *p_pedo_data != HOSU_MAX )
            {
                *p_pedo_data += 1;
            }
        }
    }
}



/* ========================================================
空白の時間を適切に0にして、
今を含む１時間のデータを書く位置にポインタ？を進める
======================================================== */
void fill_hosu_hist_hours( u16 hours )
{
    if( hours > PEDOMETER_LOG_SIZE )
    {
      hours = PEDOMETER_LOG_SIZE;
    }

    // 空白の数時間の設定
    do
    {
        p_record += 1;
        if( p_record >= PEDOMETER_LOG_SIZE )
        {
            p_record = 0;
        }
        pool.vreg_c_ext.pedo_log[ p_record ] = 0;
        hours -= 1;
    }
    while( hours != 0 );
}



/* ========================================================
空白の時間を適切に0にして、
今を含む１時間のデータを書く位置にポインタ？を進める
======================================================== */
void clear_hosu_hist()
{
    u8 hours = PEDOMETER_LOG_SIZE;
    do
    {
        hours -= 1;
        pool.vreg_c_ext.pedo_log[ hours ] = 0xFFFF;
    }
    while( hours != 0 );
    vreg_ctr[ VREG_C_ACC_HOSU_L ] = 0;
    vreg_ctr[ VREG_C_ACC_HOSU_M ] = 0;
    vreg_ctr[ VREG_C_ACC_HOSU_H ] = 0;
    p_record = 0;
}


extern u8 iic_burst_state;
bit record_read_msb_lsb;

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


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

    switch( iic_burst_state ){
    case( 0 ):
        p_record_buffer = p_record;
        iic_burst_state += 1;
        return( last_hour );

    case( 1 ):
        iic_burst_state += 1;
        return( last_day );

    case( 2 ):
        iic_burst_state += 1;
        return( last_month );

    case( 3 ):
        iic_burst_state += 1;
        return( btobcd( log_year ) );

    case( 4 ):
        iic_burst_state += 1;
        return( now_min ); // reserved dummy

    case( 5 ):
        iic_burst_state += 1;
        return( now_sec ); // reserved. dummy

    default:
        temp = pool.vreg_c_ext.pedo_log[ p_record_buffer ];
        if( record_read_msb_lsb == 0 )
        {
            dat = (u8)( temp & 0x00FF );
        }
        else
        {
            dat = (u8)(( temp >> 8 ) & 0x00FF );
            if( p_record_buffer == 0 )
            {
                p_record_buffer = PEDOMETER_LOG_SIZE-1;
            }
            else
            {
                p_record_buffer -= 1;
            }
        }
        record_read_msb_lsb += 1;
        return( dat );
    }
}



/* ========================================================
  今年の元旦からの経過時間(hour)を返す。
  引数  無し
  返値  u16 long_hour
======================================================== */
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;

    u8 min;
    u8 sec;

    // RWAIT = 1 を確認してから↓に進んで下さい
    year_hex  = YEAR;
    month_hex = MONTH;
    day_hex   = DAY;
    hour_hex  = HOUR;
    min       = MIN;
    sec       = SEC;

    RWAIT = 0;
    EI();

    year_hex  = bcdtob( year_hex );
    month_hex = bcdtob( month_hex );
    day_hex   = bcdtob( day_hex );
    hour_hex  = bcdtob( hour_hex );

    // まず日数の部分
    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 - 1;
    long_hour *= 24; // 日数→時間

    long_hour += hour_hex;

    if( ( min > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
        || ( ( min >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
                && ( sec > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY_SEC ] ))
        )
    {
        return( long_hour );
    }
    else
    {
        return( long_hour -1 );  // 1時間前に含める 注意：元旦の0時
    }
}


// 拝借もと
// ttp://www001.upp.so-net.ne.jp/y_yutaka/labo/math_algo/math_algo.html
unsigned long my_sqrt(unsigned long x)
{
    unsigned long s, t;

    if (x <= 0) return 0;

    s = 1;
    t = x;
    while (s < t)
    {
        s <<= 1;
        t >>= 1;
    }
    do
    {
        t = s;
        s = (x / s + s) >> 1;
    } while (s < t);

  return t;
}