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

#pragma mul
#pragma div
#pragma bcd

#endif

#include "incs.h"

#ifndef _WIN32
#include <math.h>
#endif

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

#include "pool.h"

// ========================================================
// 履歴の最終記録時刻
//  この順番はログ読み出しの順番でもあるのでいじらないでね
//  順番にアドレスの若いのから確保されるのを期待してます...
typedef struct{
    u8 hour_bcd;
    u8 day_bcd;
	u8 month_bcd;
	u8 year_bcd;
	u8 min_bcd;
	u8 sec_bcd;
}st_calender;



// ========================================================
static u16 get_long_hour();
static u16 calc_hours_spend( u8 );



// ========================================================
bit pedolog_overflow;           // 192時間記録済みフラグ(i2cで読める)
extern uni_pool pool;           // 歩数ログはこの構造体の中
static u8 p_record;             // ログの書き込み位置
static st_calender cal_log_latest; // 最後に歩数を更新した時刻
static u16 last_hour_fny;       // fny:from new year
static st_calender cal_temp;
static u16 now_longhour;






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

// 今年は閏年？
#define is_leapyear( y )  (( y & 0x03 ) == 0 )
// 「去年」は閏年？
#define is_firstyear( y ) (( y & 0x03 ) == 1 )


// 加速度センサ値をFIR-LPFに通す。それの係数
extern const s8 lpf_coeff[];
#define TAP 64
#define FIL_COEFF_QUANT 10


/********************************************//**
　歩数計
 ***********************************************/
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 = 0xFF;  // 山-山間の時間。短過ぎたらはじく。
    static u8 time_l;       // 前回の極小からの経過時間
    static u16 peak_l;      // 谷の深さ
    static u16 norm_hist[TAP];

    static u8 hist_indx;

    signed long filterd;

    u8 i;

    u16 sx16,sy16,sz16;

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

    // ベクトルのノルム
#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 ++;

    // ヒストリにフィルタ(fir)を掛けて、今回の値を求める //
    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;      // DC分加算...だったと思う
    acc_norm_temp = (s16)( filterd /1024 & 0xFFFF ); // ←FIL_COEFF_QUANTから正規化
/*
    if( acc_norm[0] < acc_norm_temp )
    {
        t_rise ++;
        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_if_necessary();
                }
            }
            interval_hh = 0;
        }
        // なんちゃって閾値の動的変更
        if( acc_norm[0] > 18000 )
        {
            th_L = acc_norm[0] - 10000;
        }
        else
        {
            th_L = 11000;
        }
    }
    else
    {
		if( interval_hh != 255 )	 // 飽和加算って楽に書けたらいいのに
		{
			interval_hh ++;
		}
    }

    // (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
    {
		if( time_l != 255 )
		{
			time_l ++;
		}
    }
}


/********************************************//**
　歩数＋１

 - 累積をインクリメント
 - 履歴を更新

   *2011/01/20

	仕様変更　ログがいっぱいになったらそこで止める
 ***********************************************/
#define HOSU_NODATA 0xFFFF
#define HOSU_MAX    0xFFFE

void hosu_increment_if_necessary()
{
    u8 year_compd;               // hour境界補正済み現在年。comp(ensation -ed)

	// 現在時刻取得
    DI_wt_chk();
    RWAIT = 1;
    while( !RWST ){;}

    cal_temp.hour_bcd  = HOUR;
    cal_temp.day_bcd   = DAY;
    cal_temp.month_bcd = MONTH;
    cal_temp.year_bcd  = YEAR;
    cal_temp.min_bcd   = MIN;
    cal_temp.sec_bcd   = SEC;
    
    RWAIT = 0;
    EI();
    
    year_compd = bcdtob( cal_temp.year_bcd );

    now_longhour = get_long_hour();

	// 書き込みポインタの更新
    if( ! ( vreg_ctr[ VREG_C_ACC_HOSU_L ] == 0 &&  // 歩数計on後、最初の一歩までは前回からの経過時間を計算しない
            vreg_ctr[ VREG_C_ACC_HOSU_M ] == 0 &&
            vreg_ctr[ VREG_C_ACC_HOSU_H ] == 0 ))  //. 全ビットORでゼロ判定するのはデジタル回路屋の方言みたい
	{
		// 歩数計が止まっていた時間を考慮して必要なら進める
        // 補正計算　元旦零時台で昨日扱いになった場合、大晦日の23時台に上書き
        if( now_longhour == (u16)-1 ) // 昨年大晦日の23時台扱いのとき、計算結果が -1 になってる
        {
            now_longhour = 365 * 24 -1;
			if( is_firstyear(year_compd) )
			{
				now_longhour += 24;
			}
            year_compd --;
        }
        fill_hosu_hist_hours( calc_hours_spend( year_compd ) );  // ■書き込みポインタの更新も行う

		// ログあふれで記録停止？
		if( pedolog_overflow )
		{
			return;
            // おしまい。ログの更新もなし。
		}
	}

	// インクリメントして良い
    cal_log_latest = cal_temp;  // ■ログ時刻更新
    last_hour_fny  = now_longhour;

	// 毎時ログ　インクリメント
	if( pool.vreg_c_ext.pedo_log[ p_record ] == HOSU_MAX )
	{
		// 何もしないでおしまい
		//. 小計の合計と累計があわなくなるのを避けるためだろうけど、どうなの？
		return;
	}
	else if( pool.vreg_c_ext.pedo_log[ p_record ] == HOSU_NODATA )   // その時間帯最初のカウントの時
    {															//  これしないと1歩足りない
        pool.vreg_c_ext.pedo_log[ p_record ] = 1;
    }
    else 
	{
		// 通常パス
        pool.vreg_c_ext.pedo_log[ p_record ] ++;
	}

	// 累積の更新 //
	DI_wt_chk();
	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; //. いろいろ失敗だったね...
				vreg_ctr[ VREG_C_ACC_HOSU_M ] = 255;
				vreg_ctr[ VREG_C_ACC_HOSU_H ] = 255;
			}
		}
    }
	EI();
}


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

	// 空白の数時間の設定
    while( hours != 0 )
    {
		// 新仕様　いっぱいで停止
	    p_record ++;
#if 1	// debug
		if( p_record >= PEDOMETER_LOG_SIZE )
        {
			pedolog_overflow = true;
//			dbg_nop();  // ここに来るようだとバグ
			break;
        }
		else
#endif
		{
	        pool.vreg_c_ext.pedo_log[ p_record ] = 0;
		}
        hours --;
    }

	return;
}



/********************************************//**
歩数をクリア、履歴を「データ無し」に初期化
 ***********************************************/
void clear_hosu_hist()
{
    u8 hours = PEDOMETER_LOG_SIZE;
    do
    {
        hours --;
        pool.vreg_c_ext.pedo_log[ hours ] = 0xFFFF;
    }
    while( hours != 0 );

	DI_wt_chk();
	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;
	pedolog_overflow = false;
	EI();
}


extern u8 iic_burst_state;
bit pedolog_read_msb;
/********************************************//**
  歩数計のヒストリを返す。

  １回呼ぶ度に、ヒストリの下位、上位、一時間遡って下位上位...
 ***********************************************/
u8 hosu_read(  )
{
    u8 rv;
    static u8 p_record_buffer;
    static st_calender cal_buff;	// 一応、アトミック処理に

    if( iic_burst_state == 0 )
    {
		p_record_buffer = p_record;
		DI_wt_chk();
		cal_buff = cal_log_latest;
		EI();
    }

    if( iic_burst_state <= 5 )
    {
        rv = *( (u8*)&cal_buff + iic_burst_state );  // あうあう
        iic_burst_state ++;
        return( rv );
    }
    else
    {
        u16 temp;
        // 16ビットで記録してあるのでばらして送る /// もっと楽する方法があるんじゃ
        temp = pool.vreg_c_ext.pedo_log[ p_record_buffer ];
        if( !pedolog_read_msb  )
        {
            rv = (u8)( temp & 0x00FF );
        }
        else
        {
            rv = (u8)(( temp >> 8 ) & 0x00FF );
            if( p_record_buffer == 0 )
            {
                p_record_buffer = PEDOMETER_LOG_SIZE-1;
            }
            else
            {
                p_record_buffer --;
            }
        }
        pedolog_read_msb ^= 1;
        return( rv );
    }

}



/********************************************//**
  今年の元旦からの経過時間(hour)を返す。

  引数  無し

  返値  u16 long_hour
 ***********************************************/
const u16 DAYS_FROM_HNY[] = {
    0,
    31,
    31+28,  // =59。　…３月０日は１月５９日
    31+28+31,
    31+28+31+30,
    31+28+31+30+31,
    31+28+31+30+31+30,
    31+28+31+30+31+30+31,
    31+28+31+30+31+30+31+31,
    31+28+31+30+31+30+31+31+30,
    31+28+31+30+31+30+31+31+30+31,
    31+28+31+30+31+30+31+31+30+31+30
    };

static u16 get_long_hour()
{
    u8 year    = bcdtob( cal_temp.year_bcd );
    u8 month   = bcdtob( cal_temp.month_bcd );
    u8 day     = bcdtob( cal_temp.day_bcd );
    u8 hour    = bcdtob( cal_temp.hour_bcd );
    u8 min_bcd = cal_temp.min_bcd; // 大小比較しかしないのでbcdのままでよい
    u8 sec_bcd = cal_temp.sec_bcd;
    u16 long_hour;

    // まず日数の部分
    long_hour = DAYS_FROM_HNY[ month -1 ]; // -1はインデックス合わせ
    if( is_leapyear(year) && ( 3 <= month ))
    {
        // 閏年で、閏日より後
        long_hour ++;
    }
    long_hour += day - 1;
    long_hour *= 24; // 日数→時間

    long_hour += hour;

    // 時・分境界の前？後？
    if( ( min_bcd > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
        || ( ( min_bcd >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
                && ( sec_bcd >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY_SEC ] ))
        )
    {
        return( long_hour );
    }
    else
    {
        return( long_hour -1 );  // 1時間前に含める 注意：元旦で昨年扱いにするとき。-1 になる
    }
}



/********************************************//**
　軽量平方根。

　必要十分な精度で打ち切る

   pc上でシミュレーションして大丈夫そう
 ***********************************************/
static 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;
}




/********************************************//**
　二つの 前回呼ばれた時刻と、現在時刻の差分を求める。返るのはfill_hosu_hist_hours にそのまま渡せる
 //. 引数がyear_bcd なのがちょっといやだけど...
 ***********************************************/
static u16 calc_hours_spend( u8 year )
{
	u8 cal_log_latest_year = bcdtob( cal_log_latest.year_bcd );

	// 同じ年の内
    if( cal_log_latest_year == year )
    {
        if( now_longhour > last_hour_fny )
        {
            return( now_longhour - last_hour_fny );
        }
		else if( now_longhour == 0 && last_hour_fny != 0 )
		{
			// 年明けたばかりで、境界を越えた一回目。これやらないと昨年最後に加算してしまう。
			return( 1 );
		}
		else
		{
			return( 0 );  // 同じ時間帯(と、巻き戻り。　どうなっても知らない)
		}
    }
    else if( cal_log_latest_year == ( year -1 ) )
    {
        // 年をまたいでいるとき
		u16 temp = 365 * 24 - last_hour_fny + now_longhour;
		if( is_firstyear(year) )
		{
			temp += 24;
		}
		return( temp );
    }
    else if( cal_log_latest_year < year )
    {
        // 数年放置
        return( PEDOMETER_LOG_SIZE +1 );
    }
    else
    {
        // カレンダーが巻き戻るなど
        // ノーケアでよい…が、不定値というわけにもいかない
        return( 0 );
    }
}