/*******************************************************************************
 * $B%o%s%3%$%s%G%3!<%@#6(B  - Ver 2.0 - for PIC12F1822
 * Copyright (C) 2007-2026  Nucky
 * $B;HMQ(BPIC$B%^%$%3%s(B PIC12F1822$B!!!!FbItH/?6(B16MHz$B%b!<%I(B
 *  
 * PIC12F1822 DCC $B%b!<%?!<!u%U%!%s%/%7%g%s(B $B<V:\%G%3!<%@(B
 * 
 * 2026.03.21 Ver2.0  $B%=!<%9%3!<%I$r(BC$B8@8l5-=R$KJQ99!J5!G=6/2=!K(B
 * 
 * $B!Z%G%3!<%@$N35MW![(B
 * DCC$B%3%^%s%I%9%F!<%7%g%s$+$i$N%Q%1%C%H$r<u?.$7!"<VN>$N%b!<%?!<$*$h$S%i%$%H$r(B
 * $B@)8f$9$kB?5!G=%b%P%$%k%G%3!<%@$G$9!#(BPAGE$B%b!<%I(B/DIRECT$B%b!<%I(B(Bit$BBP1~(B)$B$G$N(B
 * $B3N<B$J(BCV$BFI$_=q$-$KBP1~$7!"$"$i$f$k%3%^%s%I%9%F!<%7%g%s$G0BDjF0:n$7$^$9!#(B
 *
 * $B!Z<g$JEk:\5!G=![(B
 * 1. $BL52;(B/$B9b%H%k%/6nF0(B: ECCP$B%O!<%I%&%'%"(BPWM$B$r;HMQ(B (CV9$B$G(B 245Hz$B!A(B31.4kHz$B2DJQ(B)
 * 2. 3$BE@@^$l@~B.EY%+!<%V(B: CV2($B;OF0(B), CV5($B:GBg(B), CV6($BCf4V(B) $B$K$h$k=@Fp$JB.EYD4@0(B
 * 3. $B%b%a%s%?%`(B($B47@-(B): CV3($B2CB.(B), CV4($B8:B.(B) $B$K$h$k=ENL46$N$"$k3j$i$+$J2C8:B.(B
 * 4. $B%-%C%/%9%?!<%H(B: CV64($B;~4V(B), CV65($B6/EY(B) $B$K$h$k@E;_K`;$(B($B%9%F%#%/%7%g%s(B)$BBGGK(B
 * 5. $BN>6K@-(BFx$B%b!<%I(B: CV30$B$K$h$j!"%b!<%?!<C<;R$r;THN<V8~$1$N%i%$%H=PNO2=(B
 * 6. $B=VCGBP:v(B: $B%l!<%k$N1x$l$K$h$kEE8;CG;~$b!"%i%$%H>uBV$H?J9TJ}8~$r5-21!&I|85(B
 * 
 * $B!Z%T%sG[CV(B (PIC12F1822)$B![(B
 * 1 (VDD): 5V$BEE8;(B (+)
 * 2 (RA5): $B%b!<%?!<=PNO(B 2 (ECCP P1A / $B%b!<%?!<%I%i%$%P$X(B)
 * 3 (RA4): $B%b!<%?!<=PNO(B 1 (ECCP P1B / $B%b!<%?!<%I%i%$%P$X(B)
 * 4 (RA3): MCLR ($BL$;HMQ!&F~NO@lMQ(B)
 * 5 (RA2): DCC$B?.9fF~NO(B
 * 6 (RA1): $B%i%$%H=PNO(B 1 ($BHFMQ(BFx / $B=i4|CM(B=$BA0?J%X%C%I%i%$%H(B)
 * 7 (RA0): $B%i%$%H=PNO(B 2 ($BHFMQ(BFx / $B=i4|CM(B=$B8e?J%F!<%k%i%$%H(B)
 * 8 (VSS): GND (-)
 *******************************************************************************/
 
/*
 *$B!Z%=!<%9%3!<%I$N%i%$%;%s%9$*$h$SMxMQ>r7o$K$D$$$F![(B
 * $BK\%=!<%9%3!<%I$NCx:n8"$O$9$Y$F(BNucky$B$K5"B0$7$^$9!#(B
 * $BK\%=%U%H%&%'%"$O!"0J2<$N>r7o$K4p$E$$$FDs6!$5$l$^$9!#(B
 *
 * 1. [$B8D?M!&Hs1DMxMxMQ(B]
 * $B8D?M$NE4F;LO7?$NHO0O$G$NMxMQ!"8&5f!"2~JQ$O<+M3$+$DL5=~$G9T$($^$9!#(B
 * $BK\%3!<%I!"$^$?$O2~JQ$7$?%3!<%I$r8x3+!&6&M-$9$k$3$H$b2DG=$G$9$,!"(B
 * $B$=$N:]$bI,$:Hs1DMx$G$N8x3+$H$7!"K\%i%$%;%s%9I=5-$r;D$7$F$/$@$5$$!#(B
 *
 * 2. [$B>&MQMxMQ!JHNGdEy!K(B]
 * $BK\%=%U%H%&%'%"!J2~JQ$7$?$b$N$r4^$`!K$r=q$-9~$s$@%^%$%3%s$d%G%3!<%@4pHD$NHNGd!"(B
 * $B@=IJ2=!"$=$NB>$$$+$J$k7A$G$"$C$F$bMx1W$rF@$kL\E*!J>&MQ!K$GMxMQ$9$k>l9g$O!"(B
 * $BI,$:;vA0$K$4O"Mm$/$@$5$$!#JLES!"M-=~$N>&MQ%i%$%;%s%9$r$47@Ls$$$?$@$-$^$9!#(B
 *
 * --- English Notice ---
 * Copyright (c) Nucky. All rights reserved.
 * 
 * 1. Personal/Non-Commercial Use: Free to use, modify, and share strictly 
 *    for personal, non-commercial hobby purposes.
 * 2. Commercial Use: ANY commercial use, including manufacturing, selling 
 *    products, or integrating this software into items for sale, is STRICTLY 
 *    PROHIBITED without prior permission. A paid commercial license is required. 
 *    Please contact the author via the website.
 *
 * --- $BLH@U;v9`(B / Disclaimer ---
 * $BK\%W%m%0%i%`$NMxMQ$K$h$C$F@8$8$?%G%3!<%@!"<VN>!"$=$NB>$N5!:`$NGKB;!"IT6q9g!"(B
 * $B$*$h$S$$$+$J$kB;32$K$D$$$F$b!":n<T$O0l@Z$N@UG$$rIi$$$+$M$^$9!#(B
 * This software is provided "AS IS", WITHOUT ANY WARRANTY. Use at your own risk.
*/

#include <xc.h>

// $BFbIt%/%m%C%/<~GH?t(B(16MHz)$B$NDj5A!#(B
// XC8$B%3%s%Q%$%i$,(B __delay_ms() $BEy$NBT5!;~4V$r@53N$K7W;;$9$k$?$a$KI,?\$G$9!#(B
#define _XTAL_FREQ 16000000 

// =========================================================
// 1. $BDj5A(B: $B%b!<%?!<%I%i%$%P$N%O!<%I%&%'%";EMMA*Br(B
// =========================================================
// $BEk:\$9$k%b!<%?!<%I%i%$%P(BIC(H$B%V%j%C%82sO)(B)$B$K9g$o$;$F%3%a%s%H$r@Z$jBX$($^$9!#(B
//#define BD6231      // BD6231F-E2 $BEy$r;HMQ$N>l9g(B
#define TB67H450    // TB67H450FNG $BEy$r;HMQ$N>l9g(B

#if defined(BD6231)
    // BD6231$B;EMM(B: 
    // PWM$B$r=PNO$7$F$$$J$$B&$N%T%s$r>o$K(B LOW(0V) $B$K8GDj!#(B
    // PWM$B$O@5O@M}(B(Active-High: 1$B$N;~$K(BON)$B$G6nF0$7$^$9!#(B
    #define MOTOR_FIXED_LVL 0
    #define PWM_MODE_VAL    0x0C 
#elif defined(TB67H450)
    // TB67H450$B;EMM(B: 
    // PWM$B$r=PNO$7$F$$$J$$B&$N%T%s$r>o$K(B HIGH(5V) $B$K8GDj!#(B
    // PWM$B$OIiO@M}(B(Active-Low: 0$B$N;~$K(BON)$B$G6nF0$7$^$9!#(B
    #define MOTOR_FIXED_LVL 1
    #define PWM_MODE_VAL    0x0F 
#else
    #define MOTOR_FIXED_LVL 0
    #define PWM_MODE_VAL    0x0C 
#endif

// =========================================================
// 2. $B%3%s%U%#%0%l!<%7%g%s@_Dj(B ($B%^%$%3%s$N4pK\%O!<%I%&%'%"@_Dj(B)
// =========================================================
// CONFIG1
#pragma config FOSC = INTOSC    // $BFbItH/?6%/%m%C%/$r;HMQ(B ($B30It?e>=$r;H$o$:%T%s$r@aLs(B)
#pragma config WDTE = OFF       // $B%&%)%C%A%I%C%0%?%$%^!<L58z(B (DCC$B?.9fBT$A$G%j%;%C%H$5$l$k$N$rKI$0(B)
#pragma config PWRTE = ON       // $B%Q%o!<%"%C%W%?%$%^!<M-8z(B ($BEE8;EjF~D>8e$NEE050BDj$rBT$D(B)
#pragma config MCLRE = OFF      // MCLR$B%T%sL58z(B (RA3$B%T%s$r%j%;%C%HF~NO$G$O$J$/HFMQF~NO$K$9$k(B)
#pragma config CP = OFF         // $B%W%m%0%i%`%a%b%j$NFI$_=P$7J]8nL58z(B
#pragma config CPD = OFF        // $B%G!<%?(BEEPROM$B$NFI$_=P$7J]8nL58z(B
#pragma config BOREN = ON       // $B%V%i%&%s%"%&%H%j%;%C%HM-8z(B ($BEE05Dc2<;~$K0BA4$KDd;_$7K=Av$rKI$0(B)
#pragma config CLKOUTEN = OFF   // $B%/%m%C%/=PNOL58z(B
#pragma config IESO = OFF       // $BFbIt(B/$B30It%/%m%C%/$N@Z$jBX$(5/F0L58z(B
#pragma config FCMEN = OFF      // $B%U%'%$%k%;!<%U%/%m%C%/%b%K%?!<L58z(B

// CONFIG2
#pragma config WRT = OFF        // $B%U%i%C%7%e%a%b%j$N<+8J=q$-9~$_J]8nL58z(B
#pragma config PLLEN = OFF      // 4$BG\(BPLL$BL58z(B (OSCCON$B%l%8%9%?$GD>@\(B16MHz$B$r@_Dj$9$k$?$a(B)
#pragma config STVREN = ON      // $B%9%?%C%/%*!<%P!<%U%m!<%j%;%C%HM-8z(B
#pragma config BORV = LO        // $B%V%i%&%s%"%&%H%j%;%C%H$NEE05ogCM$rDc$a(B($BLs(B1.9V)$B$K@_Dj(B
#pragma config LVP = OFF        // $BDcEE05%W%m%0%i%_%s%0L58z(B (ON$B$@$H(BRA3$B%T%s$,;H$($J$/$J$k$?$aI,?\(B)

// =========================================================
// 3. EEPROM CV$B=i4|CM@_Dj(B ($B9)>l=P2Y;~$N@_DjCM(B)
// =========================================================
// $BG[Ns$N%$%s%G%C%/%9$,(B (CV$BHV9f(B - 1) $B$KBP1~$7$^$9!#(B

// CV1$B!A(B8: $B4pK\@_Dj(B ($B%"%I%l%9!"B.EY%+!<%V!"%P!<%8%g%sEy(B)
// [0]CV1((I1D^Z=(B)=3, [1]CV2($B;OF0(B)=0, [2]CV3($B2CB.(B)=0, [3]CV4($B8:B.(B)=0
// [4]CV5($B:GBg(B)=255, [5]CV6($BCf4V(B)=0, [6]CV7(Ver)=20, [7]CV8(ID/(IX>/D(B)=156
__EEPROM_DATA(0x03, 0x00, 0x00, 0x00, 0xFF, 0x00,   20, 0x9C); 

// CV9$B!A(B16: $B%b!<%?!<(BPWM$B@_Dj(B
// [8]CV9($B<~GH?t(B)=0 ($B=i4|CM$OD69b<~GH!&L52;@_Dj(B)
__EEPROM_DATA(0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 

// CV17$B!A(B24: $B%m%s%0%"%I%l%9@_Dj(B
// [16]CV17($B>e0L(B)=192, [17]CV18($B2<0L(B)=128 -> $B%G%U%)%k%H(B0000$BAjEv(B
__EEPROM_DATA(0xC0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 

// CV25$B!A(B32: $B%G%3!<%@4pK\@_Dj(B
// [28]CV29($BF0:n(B)=2(28Step$BM-8z(B), [29]CV30($BN>6K@-(BFx)=0($BDL>o%b!<%?!<%b!<%I(B)
__EEPROM_DATA(0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00); 

// CV33$B!A(B40: $B%U%!%s%/%7%g%s%^%C%T%s%0(B ($B=PNO%T%s$KBP$9$k%\%?%s3d$jEv$F(B)
// [32]CV33(RA1$B3dEv(B)=16(F0$BA0?J(B), [34]CV35(RA0$B3dEv(B)=32(F0$B8e?J(B)
__EEPROM_DATA(0x10, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00); 

// CV41$B!A(B48: $B=VCGBP:v%a%b%j(B1 ($B<+F0$GFI$_=q$-$5$l$k$?$a=i4|CM(B0)
// [47]CV48 = $B?J9TJ}8~$H(BF0$B!A(BF4$B$N%l%8%e!<%`MQ%a%b%j(B
__EEPROM_DATA(0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 

// CV49$B!A(B56: $B=VCGBP:v%a%b%j(B2 ($B<+F0$GFI$_=q$-$5$l$k$?$a=i4|CM(B0)
// [48]CV49 = F5$B!A(BF12$B$N%l%8%e!<%`MQ%a%b%j(B
__EEPROM_DATA(0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 

// CV57$B!A(B64: $B%-%C%/%9%?!<%H@_Dj(B ($BH/?J;~$NK`;$BGGK(B)
// [63]CV64($B;~4V(B)=0 (1$B$K$D$-Ls(B2ms), [64]CV65($B6/EY(B)=0 ($B>e>h$;(BPWM$BCM(B)
__EEPROM_DATA(0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00); 

// C$B%W%m%0%i%`$+$i(BEEPROM$B$NHVCO$K%"%/%;%9$9$k$?$a$NDj?tDj5A(B
#define EEPROM_CV1   0x00   
#define EEPROM_CV2   0x01   
#define EEPROM_CV3   0x02   
#define EEPROM_CV4   0x03   
#define EEPROM_CV5   0x04   
#define EEPROM_CV6   0x05   
#define EEPROM_CV8   0x07   
#define EEPROM_CV9   0x08   
#define EEPROM_CV17  0x10   
#define EEPROM_CV18  0x11   
#define EEPROM_CV29  0x1C   
#define EEPROM_CV30  0x1D   
#define EEPROM_CV33  0x20   // RA1$B$X$N(BF0$B!A(BF4$B3dEv(B
#define EEPROM_CV34  0x21   // RA1$B$X$N(BF5$B!A(BF12$B3dEv(B
#define EEPROM_CV35  0x22   // RA0$B$X$N(BF0$B!A(BF4$B3dEv(B
#define EEPROM_CV36  0x23   // RA0$B$X$N(BF5$B!A(BF12$B3dEv(B
#define EEPROM_CV48  0x2F   // $B%l%8%e!<%`5-21(B1
#define EEPROM_CV49  0x30   // $B%l%8%e!<%`5-21(B2
#define EEPROM_CV64  0x3F   // $B%-%C%/;~4V(B
#define EEPROM_CV65  0x40   // $B%-%C%/6/EY(B

// =========================================================
// 4. $B>uBV4IM}JQ?t(B ($B%G%3!<%@FbIt$NF0:n>uBV$rJ];}(B)
// =========================================================

// --- $B%\%?%s!&J}8~>uBV(B ($B%a%b%j@aLs$N$?$a$N%S%C%H%Q%C%-%s%0(B) ---
// f0_4_dir: [$B6u(B] [$B6u(B] [F4] [F3] [F2] [F1] [F0] [$B?J9TJ}8~(B(0=$BA0(B,1=$B8e(B)]
unsigned char f0_4_dir = 0; 
// f5_12:    [F12] [F11] [F10] [F9] [F8] [F7] [F6] [F5]
unsigned char f5_12 = 0;    

// --- $BB.EY!&2C8:B.(B($B%b%a%s%?%`(B)$B!&%-%C%/%9%?!<%H4IM}(B ---
unsigned char target_speed = 0;      // $B%3%^%s%I%9%F!<%7%g%s$+$i;X<($5$l$?L\I8B.EY(B (0$B!A(B255)
unsigned char current_speed = 0;     // $B%b%a%s%?%`Ey$K$h$j=y!9$KJQ2=$7$F$$$k8=:_$NFbItB.EY(B (0$B!A(B255)
unsigned char current_motor_dir = 0; // $B<B:]$N%b!<%?!<$N?J9TJ}8~(B (CV29$B$NH?E>@_Dj$r2CL#$7$?CM(B)
unsigned char speed_tick = 0;        // $B%b%a%s%?%`MQ!#FbItB.EY$r(B1$BCJJQ$($k$^$G$N;D$jBT5!;~4V(B
unsigned char motor_out_update = 0;  // 1$B$K$9$k$H!"%a%$%s%k!<%WFb$G%b!<%?!<$N%O!<%I%&%'%"=PNO$,99?7$5$l$k%U%i%0(B
unsigned int  kick_timer = 0;        // $B%-%C%/%9%?!<%H$N;D$j;~4V$r7W$k%+%&%s%?(B (1$B$K$D$-Ls(B0.5ms)

// --- $B%?%$%^!<Jd@5MQJQ?t(B ($BHs>o$K=EMW(B) ---
// DCC$B%Q%1%C%H(B($B?t%_%jIC(B)$B$N<u?.BT$A$G%V%m%C%/$5$l$F$$$k4V$b!"N"$G(BTimer0$B$,9o$s$@;~4V(B(0.5ms$B<~4|(B)
// $B$r<h$j$3$\$5$J$$$h$&%9%H%C%/$7$F$*$/JQ?t!#$3$l$K$h$j%b%a%s%?%`$d%-%C%/$N;~4V$,@53N$K$J$k!#(B
unsigned char tmr0_ticks = 0;

// --- $B%W%m%0%i%_%s%0!&=VCGBP:vMQ(B ---
unsigned char page_register = 0;     // PAGE$B%b!<%I%W%m%0%i%_%s%0$K$*$1$k!"8=:_$N;XDj%Z!<%8(B
unsigned char last_saved_cv48 = 0;   // EEPROM$B=q$-49$(<wL?J]8n$N$?$a!"A02sJ]B8$7$?(Bf0_4_dir$B>uBV$rJ];}(B
unsigned char last_saved_cv49 = 0;   // $BA02sJ]B8$7$?(Bf5_12$B>uBV$rJ];}(B

// --- CV$B%-%c%C%7%eJQ?t(B ---
// EEPROM$B$NFI$_=P$7$OCY$$$?$a!"5/F0;~$d(BCV$B=q$-9~$_;~$K0lEY$@$1(BRAM$BJQ?t$K%3%T!<$7!"(B
// $BAv9TCf$N%a%$%s%k!<%W$+$i$O$3$N%-%c%C%7%e$r9bB.$K;2>H$9$k!#(B
unsigned char cv1_c, cv2_c, cv3_c, cv4_c, cv5_c, cv6_c, cv9_c;
unsigned char cv17_c, cv18_c, cv29_c, cv30_c;
unsigned char cv33_c, cv34_c, cv35_c, cv36_c;
unsigned char cv64_c, cv65_c; 

// =========================================================
// 5. DCC$B<u?.%G!<%?JQ?t(B ($B%Q%1%C%H%P%C%U%!(B)
// =========================================================
// $B<u?.$7$?@8$N(BDCC$B%Q%1%C%H(B($B:GBg(B6$B%P%$%H(B)$B$rJ];}$7$^$9!#(B
unsigned char data1 = 0; // $BBh(B1$B%P%$%H(B ($B%7%g!<%H%"%I%l%9(B $B$^$?$O(B $B%m%s%0%"%I%l%9>e0L(B)
unsigned char data2 = 0; // $BBh(B2$B%P%$%H(B ($BL?Na(B $B$^$?$O(B $B%m%s%0%"%I%l%92<0L(B)
unsigned char data3 = 0; // $BBh(B3$B%P%$%H(B
unsigned char data4 = 0; // $BBh(B4$B%P%$%H(B
unsigned char data5 = 0; // $BBh(B5$B%P%$%H(B
unsigned char data6 = 0; // $BBh(B6$B%P%$%H(B ($B%(%i!<8!=P(B/XOR$B%A%'%C%/%5%`MQ(B)
unsigned char packet_ready = 0;  // 1=$B%Q%1%C%H<u?.40N;!#%a%$%s%k!<%W$G%G!<%?=hM}$r3+;O$9$k9g?^(B
unsigned char packet_length = 0; // $B<u?.$7$?%Q%1%C%H$NM-8z$J%P%$%H?t(B(3$B!A(B6)

// =========================================================
// $B4X?t%W%m%H%?%$%W@k8@(B
// =========================================================
void init_hardware(void);
void update_pwm_config(void);
void load_cv_cache(void);
void write_cv(unsigned char cv_addr, unsigned char cv_data);
void load_saved_state(void);
void save_state_if_changed(void);
void func_out(void);
unsigned char calculate_speed_curve(unsigned char speed);
void motor_out(void); 
void send_ack(void);
void process_service_mode(void);
void process_operation_mode(void);
void process_momentum(void);

// =========================================================
// $B!Z4X?t![(B DCC$B%Q%k%9I}7WB,(B ($B%]!<%j%s%0!\(BTimer1$BJ}<0(B)
// $B%l!<%k$+$iF~NO$5$l$k(BRA2$B%T%s$N(BHigh/Low$B$r4F;k$7!"(B1$B$D$NGH7A$ND9$5$rB,$j$^$9!#(B
// $BLa$jCM(B: 1($BC;$$(B=DCC$B$N(B'1'), 0($BD9$$(B=DCC$B$N(B'0'), 2($B%?%$%`%"%&%H(B=$B%N%$%:(B/$BL5?.9f(B)
// =========================================================
unsigned char measure_pulse(void) {
    // $B!Z=EMW![(B $B%T%s$N4F;k%k!<%W$KBZ:_$7$F$$$k4V$b!"(BTimer0$B$N%*!<%P!<%U%m!<%U%i%0(B(0.5ms$B<~4|(B)
    // $B$,N)$C$F$$$J$$$+3NG'$7!"N)$C$F$$$l$P%9%H%C%/JQ?t(B(tmr0_ticks)$B$r2C;;$9$k!#(B
    if (INTCONbits.TMR0IF) {
        INTCONbits.TMR0IF = 0;
        tmr0_ticks++;
    }

    // 1. RA2$B$,(BLow$B$K$J$k$N$rBT$D(B (DCC$B%Q%k%9$NN)$A>e$,$j%(%C%8$r@53N$KB*$($k$?$a(B)
    TMR1H = 0; TMR1L = 0;
    while(PORTAbits.RA2 == 1) { 
        // 16MHz$B@_Dj;~!"(BTimer1($B%W%j%9%1!<%i(B1:4)$B$O(B 1$B%+%&%s%H(B=1us$B!#(B
        // TMR1H$B$,(B1$B$K$J$k(B(256us$B7P2a$9$k(B)$B$^$G(BHigh$B$N$^$^$J$i%?%$%`%"%&%H(B($B%(%i!<(B2)$B$GH4$1$k!#(B
        if (TMR1H > 0) return 2; 
    }
    
    // 2. RA2$B$,(BHigh$B$K$J$k(B($B%Q%k%9$N3+;O(B)$B$N$rBT$D(B
    TMR1H = 0; TMR1L = 0;
    while(PORTAbits.RA2 == 0) { 
        if (TMR1H > 0) return 2; 
    }
    
    // 3. $B%Q%k%9$ND9$5(B(High$B$N4|4V(B)$B$r<BB,$9$k(B
    TMR1H = 0; TMR1L = 0;
    while(PORTAbits.RA2 == 1) { 
        if (TMR1H > 0) return 2; 
    }
    
    // 4. $B%Q%k%9I}$NH=Dj(B (75us$B$rogCM$H$9$k(B)
    // '1'$B$OJRGHLs(B58us$B!"(B'0'$B$OLs(B100us$B!#(B75us$B$h$jC;$1$l$P(B'1'$B!"D9$1$l$P(B'0'$B$HH=Dj$9$k!#(B
    unsigned char pulse = TMR1L;
    if (pulse < 75) return 1;
    return 0;
}

// =========================================================
// $B!Z4X?t![(B DCC$B%Q%1%C%H<u?.=hM}(B ($B>uBVA+0\%k!<%W(B)
// $B%W%j%"%s%V%k(B($B3+;O9g?^(B)$B$rC5$7!":GBg(B6$B%P%$%H$N%G!<%?$r%S%C%HC10L$GAH$_N)$F$^$9!#(B
// =========================================================
void receive_dcc(void) {
    packet_ready = 0;
    unsigned char bit_val;
    unsigned char buf[6] = {0}; // $B<u?.Cf$N%G!<%?$r0l;~J]B8$9$k%P%C%U%!(B
    unsigned char p_count = 0;
    
WAIT_PREAMBLE:
    p_count = 0;
    
PREAMBLE_LOOP:
    // $B%W%j%"%s%V%k(B('1'$B$NO"B3(B)$B$rC5$9(B
    bit_val = measure_pulse();
    if (bit_val == 2) return; // $B%?%$%`%"%&%H;~$O%Q%1%C%H<u?.$rD|$a!"%a%$%s%k!<%W$X5"$k(B
    
    if (bit_val == 1) {
        p_count++; // '1'$B$J$i%+%&%s%H%"%C%W(B
        goto PREAMBLE_LOOP;
    } else {
        // '0'$B$,Mh$?;~!"D>A0$K(B'1'$B$,(B10$B2s0J>eB3$$$F$$$J$1$l$P%N%$%:$HH=CG$7C5$7D>$9(B
        if (p_count < 10) goto WAIT_PREAMBLE; 
    }
    
    // --- $B%G!<%?%P%$%H$N<u?.(B ($B:GBg(B6$B%P%$%H(B) ---
    for (packet_length = 0; packet_length < 6; packet_length++) {
        
        // 8$B%S%C%HJ,$N%G!<%?$rFI$_9~$_!":8%7%U%H$7$F(B1$B%P%$%H$KAH$_N)$F$k(B
        for (unsigned char i = 0; i < 8; i++) {
            bit_val = measure_pulse();
            if (bit_val == 2) return;
            buf[packet_length] = (buf[packet_length] << 1) | bit_val;
        }
        
        // 8$B%S%C%HFI$s$@D>8e$KMh$k!V%;%Q%l!<%?%S%C%H!W$r3NG'(B
        bit_val = measure_pulse();
        if (bit_val == 2) return;
        
        if (bit_val == 1) {
            // $B%;%Q%l!<%?$,(B '1' ($B%(%s%I%S%C%H(B) $B$J$i!"$3$N%Q%1%C%H$O$3$3$G=*N;(B
            packet_length++; 
            if (packet_length >= 3) {
                // $B:GDc(B3$B%P%$%HB7$C$F$$$l$PM-8z!#%P%C%U%!$+$i0l3gE>Aw$9$k!#(B
                // ($B<u?.$7$J$+$C$?8eH>%P%$%H$K$O(Bbuf$B=i4|2=;~$N(B0$B$,F~$j!"%4%_%G!<%?$,>C5n$5$l$k(B)
                data1 = buf[0]; data2 = buf[1]; data3 = buf[2];
                data4 = buf[3]; data5 = buf[4]; data6 = buf[5];
                packet_ready = 1; 
            }
            return;
        }
        // $B%;%Q%l!<%?$,(B '0' $B$J$i!"$^$@<!$N%P%$%H$,B3$/$N$G%k!<%W$r7QB3$9$k(B
    }
}

// =========================================================
// $B!Z4X?t![(B $B%O!<%I%&%'%"=i4|2=(B
// $B%^%$%3%s5/F0;~$K(B1$BEY$@$18F$P$l!"%T%s$NLr3d$d%?%$%^!<$r@_Dj$7$^$9!#(B
// =========================================================
void init_hardware(void) {
    OSCCON = 0x78;      // $BFbIt%/%m%C%/$r(B16MHz$B$K@_Dj(B
    ANSELA = 0x00;      // $BA4$F$N%T%s$r%G%8%?%k(BI/O$B$K@Z$jBX$((B
    TRISA  = 0x0C;      // RA2(DCC$BF~NO(B), RA3(MCLR)$B$rF~NO%T%s$K!"B>$r=PNO%T%s$K@_Dj(B
    LATA   = 0x00;      // $B=PNO%T%s$N=i4|>uBV$r$9$Y$F(BLow(0V)$B$K$9$k(B

    APFCON = 0x03;      // PWM$B=PNO%T%s$N3d$jEv$F$rJQ99(B (P1A$B$r(BRA5$B$X!"(BP1B$B$r(BRA4$B$X(B)

    // ECCP1 ($B%O!<%I%&%'%"(BPWM$B%b%8%e!<%k(B) $B$N@_Dj(B
    CCP1CON = PWM_MODE_VAL; // $BKAF,$GDj5A$7$?%I%i%$%PMQO@M}(B($B@5(B/$BIiO@M}(B)$B$r%;%C%H(B
    CCPR1L = 0;             // $B=i4|(BDuty($BB.EY(B)$B$O(B0%
    PSTR1CON = 0x00;        // $B%9%F%"%j%s%0(B(PWM$B$N=PNO@h(B)$B$r0lC6A4$FL58z2=$70BA4$r3NJ](B

    // Timer0 $B$N@_Dj(B ($B%b%a%s%?%`$d%-%C%/%9%?!<%H$N;~4V$r%+%&%s%H$9$k$?$a(B)
    OPTION_REG = 0x82;      // $B%W%j%9%1!<%i(B1:8$B!#(B16MHz$B6nF0;~!"Ls(B0.512ms$B<~4|$G%U%i%0$,N)$D(B
    INTCONbits.TMR0IF = 0;  // $B3d$j9~$_%U%i%0$r%/%j%"(B

    // Timer1 $B$N@_Dj(B (DCC$B%Q%k%9I}$N7WB,MQ(B)
    T1CONbits.TMR1CS = 0;   // $B%/%m%C%/8;$rFbIt%/%m%C%/(B(Fosc/4)$B$K@_Dj(B
    T1CONbits.T1CKPS = 2;   // $B%W%j%9%1!<%i(B1:4$B!#$3$l$K$h$j!V(B1$B%+%&%s%H!a(B1us$B!W$K$J$k(B
    T1CONbits.TMR1ON = 1;   // Timer1$B$r5/F0(B

    // $B3d$j9~$_$O%N%$%:$K$h$kK=Av$rKI$0$?$a0l@Z;HMQ$7$J$$(B($B%]!<%j%s%0J}<0(B)
    INTCONbits.PEIE = 0;  
    INTCONbits.GIE  = 0;  
}

// =========================================================
// $B!Z4X?t![(B PWM$B<~GH?t$N99?7(B
// CV9$B$N@_DjCM$K1~$8$F%b!<%?!<$+$iLD$kH/?62;(B($B<~GH?t(B)$B$rD4@0$7$^$9!#(B
// =========================================================
void update_pwm_config(void) {
    unsigned char cv9_val = cv9_c & 0x0F; 
    
    // PR2$B$O(BPWM$B$N(B1$B<~4|$ND9$5$r7h$a$k%l%8%9%?(B
    // bit3$B$,(B0$B$J$i9b<~GH4s$j(B(126$B%9%F%C%W$G(B100%)$B!"(B1$B$J$iDc<~GH4s$j(B(254$B%9%F%C%W$G(B100%)
    if ((cv9_val & 0x08) == 0) PR2 = 126;
    else                       PR2 = 254;
    
    // Timer2$B$N%W%j%9%1!<%i(B($BJ,<~Hf(B)$B$rJQ99$7!":G=*E*$J(BPWM$B<~GH?t$r7hDj(B
    unsigned char ps = cv9_val & 0x03; 
    if (ps == 0)      T2CON = 0x04; // 1:1  ($BL52;0h(B: $BLs(B31.4kHz)
    else if (ps == 1) T2CON = 0x05; // 1:4  ($B9b2;0h(B: $BLs(B7.8kHz)
    else if (ps == 2) T2CON = 0x06; // 1:16 ($BCf2;0h(B: $BLs(B2.0kHz)
    else              T2CON = 0x07; // 1:64 ($BDc2;0h(B: $BLs(B490Hz) $B6KDcB.=E;k(B
}

// =========================================================
// $B!Z4X?t![(B EEPROM$B%-%c%C%7%e$NFI$_9~$_(B
// $B5/F0;~$d(BCV$B=q$-9~$_8e$K8F$P$l!"(BEEPROM$B$NCM$r(BRAM$B$KE83+$7$^$9!#(B
// =========================================================
void load_cv_cache(void) {
    cv1_c  = eeprom_read(EEPROM_CV1);  cv2_c  = eeprom_read(EEPROM_CV2); 
    cv3_c  = eeprom_read(EEPROM_CV3);  cv4_c  = eeprom_read(EEPROM_CV4); 
    cv5_c  = eeprom_read(EEPROM_CV5);  cv6_c  = eeprom_read(EEPROM_CV6); 
    cv9_c  = eeprom_read(EEPROM_CV9);  cv17_c = eeprom_read(EEPROM_CV17);
    cv18_c = eeprom_read(EEPROM_CV18); cv29_c = eeprom_read(EEPROM_CV29);
    cv30_c = eeprom_read(EEPROM_CV30); cv33_c = eeprom_read(EEPROM_CV33);
    cv34_c = eeprom_read(EEPROM_CV34); cv35_c = eeprom_read(EEPROM_CV35);
    cv36_c = eeprom_read(EEPROM_CV36); cv64_c = eeprom_read(EEPROM_CV64); 
    cv65_c = eeprom_read(EEPROM_CV65); 
    
    // $B?7IJ$N%^%$%3%s$J$I$GL$@_Dj(B(0xFF)$B$N>l9g$O0BA4$N$?$a(B0($BL58z(B)$B$H$9$k(B
    if (cv64_c == 0xFF) cv64_c = 0;
    if (cv65_c == 0xFF) cv65_c = 0;
    
    update_pwm_config(); // $BD>$A$K%O!<%I%&%'%"$XH?1G(B
}

// =========================================================
// $B!Z4X?t![(B CV$B$N=q$-9~$_=hM}(B
// =========================================================
void write_cv(unsigned char cv_addr, unsigned char cv_data) {
    if (cv_addr == EEPROM_CV8) {
        // NMRA$B5,3J(B: $B%a!<%+!<(BID(CV8)$B$K(B 8 $B$r=q$-9~$^$l$?$i!V%U%!%/%H%j!<%j%;%C%H!W(B
        if (cv_data == 8) {
            eeprom_write(EEPROM_CV1,  3);     
            eeprom_write(EEPROM_CV17, 0xC0);  
            eeprom_write(EEPROM_CV18, 0x80);  
            eeprom_write(EEPROM_CV29, 2);     
        }
    } else {
        eeprom_write(cv_addr, cv_data);
    }
    load_cv_cache(); // $B=q$-49$(8e!"$9$0$K%-%c%C%7%e$XH?1G$5$;$k(B
}

// =========================================================
// $B!Z4X?t![(B $B%l%8%e!<%`5!G=(B: $B>uBV$NFI$_9~$_(B
// $BEE8;=VCG$+$iI|5"$7$?:]!"D>A0$N%i%$%H>uBV$d?J9TJ}8~$rI|85$7$^$9!#(B
// =========================================================
void load_saved_state(void) {
    f0_4_dir = eeprom_read(EEPROM_CV48);
    f5_12    = eeprom_read(EEPROM_CV49);
    
    if (f0_4_dir == 0xFF) { f0_4_dir = 0; eeprom_write(EEPROM_CV48, 0); }
    if (f5_12 == 0xFF)    { f5_12 = 0;    eeprom_write(EEPROM_CV49, 0); }
    
    last_saved_cv48 = f0_4_dir;
    last_saved_cv49 = f5_12;
    
    // $BI|85$7$??J9TJ}8~$H!"(BCV29$B$N!VH?E>%U%i%0!W$r(BXOR$B$7$F%b!<%?!<$N6K@-$r7hDj(B
    current_motor_dir = (f0_4_dir & 0x01) ^ (cv29_c & 0x01);
}

// =========================================================
// $B!Z4X?t![(B $B%l%8%e!<%`5!G=(B: $B>uBV$NJ]B8(B
// $B>uBV$,JQ2=$7$?;~!V$@$1!W(BEEPROM$B$K=q$-9~$_$^$9!#(B(EEPROM$B=q$-49$(<wL?J]8n(B)
// =========================================================
void save_state_if_changed(void) {
    if (f0_4_dir != last_saved_cv48) { eeprom_write(EEPROM_CV48, f0_4_dir); last_saved_cv48 = f0_4_dir; }
    if (f5_12 != last_saved_cv49)    { eeprom_write(EEPROM_CV49, f5_12);    last_saved_cv49 = f5_12; }
}

// =========================================================
// $B!Z4X?t![(B ACK$B%Q%k%9Aw?.(B ($B%W%m%0%i%_%s%0%H%i%C%/$G$N1~Ez(B)
// $B%3%^%s%I%9%F!<%7%g%s$K!VFI=q@.8y!W$rCN$i$;$k$?$a!"%b!<%?!<$H%i%$%H$r(B
// $B=V4VE*$K(BON$B$K$7$F!"(BNMRA$B5,3J$GDj$a$i$l$?EEN.(B(60mA$B0J>e$r(B6ms$B4V(B)$B$r>CHq$7$^$9!#(B
// =========================================================
void send_ack(void) {
    unsigned char backup_lata = LATA; 
    unsigned char backup_duty = CCPR1L;
    unsigned char backup_pstr = PSTR1CON;
    
    LATA4 = MOTOR_FIXED_LVL; 
    LATA |= 0x03;            // $B%i%$%H=PNO(B(RA0,RA1)$B$r6/@)(BON
    PSTR1CON = 0x01;         // $B%b!<%?!<=PNO$rM-8z2=(B
    CCPR1L = PR2;            // PWM 100% ($B%U%k%9%m%C%H%k(B)
    
    __delay_ms(6);           // 6$B%_%jIC4VEEN.$rN.$7B3$1$k(B
    
    CCPR1L = backup_duty;
    PSTR1CON = backup_pstr;
    LATA = backup_lata;
}

// =========================================================
// $B!Z4X?t![(B $B%U%!%s%/%7%g%s(B($B%i%$%H(B)$B=PNO=hM}(B
// $B%f%K%P!<%5%k%^%C%T%s%0!#(BCV33$B!A(B36$B$N@_Dj$K4p$E$$$F!"2!$5$l$?%\%?%s$r(B
// $BG$0U$N%T%s(B(RA0/RA1)$B$X=PNO$7$^$9!#(B
// =========================================================
void func_out(void){
    // F1$B!A(BF4$B$N>uBV$rCj=P$7!"Hf3S$7$d$9$$$h$&%S%C%H0LCV$rD4@0(B
    unsigned char state_A = (f0_4_dir >> 2) & 0x0F; 
    unsigned char actual_dir = (f0_4_dir & 0x01) ^ (cv29_c & 0x01); 
    
    // F0($B%X%C%I(B/$B%F!<%k(B)$B%\%?%s$,(BON$B$N;~!"?J9TJ}8~$K1~$8$F%S%C%H$r9g@.(B
    if (f0_4_dir & 0x02) {
        if (actual_dir == 0) state_A |= 0x10; // $BA0?J$J$i%S%C%H(B4 ($BCM(B:16)
        else                 state_A |= 0x20; // $B8e?J$J$i%S%C%H(B5 ($BCM(B:32)
    }
    
    // CV33$BEy$N@_DjCM$H8=:_$N%\%?%s>uBV(B(state_A)$B$r%S%C%HO@M}@Q(B(&)$B$GHf3S$7!"(B
    // $B=E$J$k%S%C%H$,(B1$B$D$G$b$"$l$P!"%T%s$r(BHIGH(1)$B$K$7$FE@Et$5$;$^$9!#(B
    LATA1 = ((cv33_c & state_A) || (cv34_c & f5_12)) ? 1 : 0;
    LATA0 = ((cv35_c & state_A) || (cv36_c & f5_12)) ? 1 : 0;
}

// =========================================================
// $B!Z4X?t![(B 3$BE@@^$l@~B.EY%+!<%V7W;;(B
// $B%3%s%H%m!<%i$N%9%m%C%H%kCM(B(0$B!A(B255)$B$r!"(BCV2($B;OF0(B), CV5($B:GBg(B), CV6($BCf4V(B)
// $B$K4p$E$$$F<B:]$N%b!<%?!<=PNOEE05CM$KJQ49$7$^$9!#(B
// =========================================================
unsigned char calculate_speed_curve(unsigned char speed) {
    if (speed == 0) return 0;
    unsigned int v_start = cv2_c;
    unsigned int v_high  = cv5_c;
    unsigned int v_mid   = cv6_c;
    
    // CV6($BCf4V(B)$B$,L$@_Dj(B(0)$B$N>l9g$O!";OE@$H=*E@$N??$sCf$r<+F07W;;(B
    if (v_mid == 0) v_mid = (v_start + v_high) >> 1;
    
    unsigned int result = 0;
    
    if (speed <= 128) {
        // $BA0H>(B: $B;OF0EE05!ACf4VEE05(B $B$^$G$rD>@~Jd4V(B
        if (v_mid >= v_start) result = v_start + (((unsigned int)speed * (v_mid - v_start)) >> 7);
        else result = v_start; 
    } else {
        // $B8eH>(B: $BCf4VEE05!A:GBgEE05(B $B$^$G$rD>@~Jd4V(B
        if (v_high >= v_mid) result = v_mid + (((unsigned int)(speed - 128) * (v_high - v_mid)) >> 7);
        else result = v_mid; 
    }
    
    if (result > 255) result = 255;
    return (unsigned char)result;
}

// =========================================================
// $B!Z4X?t![(B $B%b!<%?!<(B PWM$B=PNO(B / $BN>6K@-(BFx $B=PNO(B
// $B7W;;$5$l$?B.EY$H?J9TJ}8~$K4p$E$$$F!"%O!<%I%&%'%"(BPWM$B$rA`:n$7$^$9!#(B
// =========================================================
void motor_out(void) {
    if (cv30_c == 2) {
        // --- $BN>6K@-(BFx$B%b!<%I(B ---
        // $B%b!<%?!<C<;R$r%i%$%H=PNO$H$7$F;H$&FC<l%b!<%I(B($B;THN$NL52~B$%i%$%H4pHDMQ(B)$B!#(B
        // $BB.EY$K4X78$J$/!"(BF0$B%\%?%s$KO"F0$7$F(BPWM100%$B=PNO$r9T$$$^$9!#(B
        unsigned char actual_dir = (f0_4_dir & 0x01) ^ (cv29_c & 0x01); 
        unsigned char f0_on = (f0_4_dir & 0x02); 
        if (f0_on) {
            CCPR1L = PR2; 
            if (actual_dir == 0) { LATA4 = MOTOR_FIXED_LVL; PSTR1CON = 0x01; } 
            else                 { LATA5 = MOTOR_FIXED_LVL; PSTR1CON = 0x02; } 
        } else {
            PSTR1CON = 0x00; LATA &= ~(0x30); CCPR1L = 0; // $B>CEt(B
        }
    } 
    else {
        // --- $BDL>o$N%b!<%?!<@)8f%b!<%I(B ---
        if (current_speed == 0 && kick_timer == 0) {
            PSTR1CON = 0x00; LATA &= ~(0x30); CCPR1L = 0; // $B40A4Dd;_(B
        } else {
            unsigned int calc_spd = calculate_speed_curve(current_speed);
            
            // $BH/?J;~$N$_!"%-%C%/%9%?!<%HEE05(B(CV65)$B$r>e>h$;$7$FK`;$$rBG$AGK$k(B
            if (kick_timer > 0) calc_spd += cv65_c; 
            
            // PWM$B<~GH?t$K9g$o$;$F(BDuty$BHf$N:GBgCM$r%9%1!<%j%s%0(B
            unsigned char duty;
            if ((cv9_c & 0x08) == 0) {
                duty = calc_spd >> 1; 
                if (duty > 126) duty = 126; 
            } else {
                if (calc_spd > 254) duty = 254; 
                else duty = (unsigned char)calc_spd;
            }
            
            CCPR1L = duty; // PWM$B%l%8%9%?$X=q$-9~$_(B

            // PSTR1CON($B%9%F%"%j%s%0%l%8%9%?(B)$B$G(BH$B%V%j%C%8$N6K@-$r@Z$jBX$($k(B
            if (current_motor_dir == 0) { LATA4 = MOTOR_FIXED_LVL; PSTR1CON = 0x01; } 
            else                        { LATA5 = MOTOR_FIXED_LVL; PSTR1CON = 0x02; } 
        }
    }
}

// =========================================================
// $B!Z4X?t![(B $B%W%m%0%i%_%s%0%b!<%I=hM}(B (Service Mode)
// PAGE$B%b!<%I!"(BDIRECT$B%b!<%I(B(Byte / Bit)$B$NN>J}$KBP1~$7$F$$$^$9!#(B
// =========================================================
void process_service_mode(void) {
    if ((data1 & 0xF0) != 0x70) return; 
    
    if (packet_length == 3) {
        // --- PAGE$B%b!<%I(B (3$B%P%$%H9=@.(B) ---
        unsigned char is_write = (data1 & 0x08) ? 1 : 0; 
        unsigned char reg_num = data1 & 0x07; 
        unsigned char cv_addr = 0;
        unsigned char target_data = data2;
        
        if (reg_num <= 3) {         
            if (page_register == 0) { packet_ready = 0; return; }
            cv_addr = (page_register - 1) * 4 + reg_num;
        } 
        else if (reg_num == 4) cv_addr = EEPROM_CV29;
        else if (reg_num == 5) {  
            if (is_write) page_register = data2;
            if (!is_write && page_register != data2) { packet_ready = 0; return; }
            send_ack(); 
            packet_ready = 0; return;     
        } 
        else cv_addr = reg_num; 
        
        if (is_write) {
            send_ack(); // $B%?%$%`%"%&%H$rKI$0$?$a!"(BEEPROM$B$X=q$-9~$`A0$K$9$0(BACK$B$rJV$9(B
            write_cv(cv_addr, target_data); 
        } else {
            // $BFI$_=P$7(B(Verify)$B;~$O!";}$C$F$$$kCM$H0lCW$7$?;~$N$_(BACK$B$rJV$9(B
            if (eeprom_read(cv_addr) == target_data) send_ack(); 
        }
    } 
    else if (packet_length == 4 && (data1 & 0xF4) == 0x74) {
        // --- DIRECT$B%b!<%I(B Byte (4$B%P%$%H9=@.(B) ---
        unsigned char is_write = (data1 & 0x08) ? 1 : 0; 
        unsigned char cv_addr = data2; 
        unsigned char target_data = data3;
        if (is_write) {
            send_ack();      
            write_cv(cv_addr, target_data); 
        } else {
            if (eeprom_read(cv_addr) == target_data) send_ack(); 
        }
    }
    else if (packet_length == 4 && (data1 & 0xFC) == 0x78) {
        // --- DIRECT$B%b!<%I(B Bit ($B%9%^%[7O%3%s%H%m!<%iEy$GI,?\(B) ---
        unsigned char cv_addr = data2; 
        unsigned char k_bit = (data3 & 0x10) ? 1 : 0; // 1=$B=q$-9~$_(B, 0=$BFI$_=P$7(B
        unsigned char d_bit = (data3 & 0x08) ? 1 : 0; // $BBP>]%S%C%HCM(B
        unsigned char bit_pos = data3 & 0x07;         // $B%S%C%H0LCV(B(0$B!A(B7)
        
        unsigned char current_cv_val = eeprom_read(cv_addr);
        unsigned char bit_mask = 1 << bit_pos;
        unsigned char actual_bit = (current_cv_val & bit_mask) ? 1 : 0;
        
        if (k_bit == 0) {
            if (actual_bit == d_bit) send_ack();
        } else {
            send_ack(); 
            if (d_bit) current_cv_val |= bit_mask;  
            else       current_cv_val &= ~bit_mask; 
            write_cv(cv_addr, current_cv_val);
        }
    }
}

// =========================================================
// $B!Z4X?t![(B $BDL>o1?E>%Q%1%C%H=hM}(B (Operation Mode)
// =========================================================
void process_operation_mode(void) {
    unsigned char inst = 0, next_d = 0, cv_addr = 0, cv_data = 0;
    
    // $B%"%I%l%9$N0lCW3NG'(B (CV29$B$G%7%g!<%H(B/$B%m%s%0$rH=CG(B)
    if ((cv29_c & 0x20) == 0) { 
        if (data1 != cv1_c) return; 
        inst = data2; next_d = data3; cv_addr = data3; cv_data = data4;
    } else {
        if (data1 != cv17_c || data2 != cv18_c) return;
        inst = data3; next_d = data4; cv_addr = data4; cv_data = data5;
    }
    
    if ((inst & 0xFC) == 0xEC) {
        // OPS$B%b!<%I(B(POM: $BAv9TCf%W%m%0%i%_%s%0(B)$B!#%5%$%l%s%H$K=q$-9~$`(B
        write_cv(cv_addr, cv_data); 
    } 
    else if ((inst & 0xC0) == 0x40 || inst == 0x3F) {
        // --- $BB.EY$H?J9TJ}8~$NL?Na(B ---
        unsigned char old_target = target_speed;
        
        if (inst == 0x3F) {
            // 128$B%9%F%C%W%b!<%I(B
            if(next_d & 0x80) f0_4_dir &= ~0x01; else f0_4_dir |= 0x01; 
            unsigned char s = next_d & 0x7F;
            if (s == 1) { target_speed = 0; current_speed = 0; motor_out_update = 1; } // E-Stop
            else if (s == 0) target_speed = 0; 
            else target_speed = (s - 1) * 2;   // 1$B!A(B126$B$r(B 0$B!A(B255$B$NFbItB.EY$K3HD%(B
        } else {
            // 14/28$B%9%F%C%W%b!<%I(B
            if(inst & 0x20) f0_4_dir &= ~0x01; else f0_4_dir |= 0x01; 
            unsigned char s = inst & 0x0F;
            unsigned char c = (inst & 0x10) >> 4;
            unsigned char step28 = (s << 1) | c;
            if (step28 == 1) { target_speed = 0; current_speed = 0; motor_out_update = 1; } 
            else if (step28 == 0) target_speed = 0; 
            else target_speed = (step28 - 1) * 9; 
        }
        
        // $BDd;_>uBV$+$iH/?J$7$?=V4V$r8!CN$7!"%-%C%/%9%?!<%HMQ%?%$%^!<(B(CV64)$B$r%;%C%H(B
        if (old_target == 0 && target_speed > 0) {
            kick_timer = (unsigned int)cv64_c << 2; // $BLs(B2ms$BC10L$N%+%&%s%HCM$KJQ49(B
            if (kick_timer > 0) motor_out_update = 1;
        }
    } 
    else if ((inst & 0xE0) == 0x80) {
        // Group1 $B%U%!%s%/%7%g%sL?Na(B (F0$B!A(BF4)
        unsigned char tmp = (inst & 0x0F) << 2;
        if(inst & 0x10) tmp |= 0x02; 
        f0_4_dir = (f0_4_dir & 0x01) | tmp; 
    } 
    else if ((inst & 0xF0) == 0xB0) {
        // Group2 $B%U%!%s%/%7%g%sL?Na(B (F5$B!A(BF8)
        f5_12 = (f5_12 & 0xF0) | (inst & 0x0F); 
    } 
    else if ((inst & 0xF0) == 0xA0) {
        // Group2 $B%U%!%s%/%7%g%sL?Na(B (F9$B!A(BF12)
        f5_12 = (f5_12 & 0x0F) | ((inst & 0x0F) << 4); 
    }
}

// =========================================================
// $B!Z4X?t![(B $B2C8:B.(B($B%b%a%s%?%`(B) $B$H(B $B%-%C%/%9%?!<%H$NN"2s$7=hM}(B
// =========================================================
void process_momentum(void) {
    // $B%a%$%s%k!<%WBZ:_Cf$KH/@8$7$?%?%$%^!<%U%i%0$bO3$i$5$:2s<}$9$k(B
    if (INTCONbits.TMR0IF) {
        INTCONbits.TMR0IF = 0;
        tmr0_ticks++;
    }
    
    // $B%9%H%C%/$5$l$?%?%$%^!<%F%#%C%/(B(0.5ms$BC10L(B)$B$rA4$F>C2=$7!"@5$7$/;~4V$r?J$a$k(B
    while (tmr0_ticks > 0) {
        tmr0_ticks--;
        
        if (kick_timer > 0) {
            kick_timer--;
            // $B%-%C%/=*N;;~$KDL>oEE05$X2<$2$k$?$a=PNO$r99?7(B
            if (kick_timer == 0) motor_out_update = 1; 
        }
        
        if (speed_tick > 0) speed_tick--; 
        
        // $B%+%&%s%H$,(B0$B$K$J$C$?$i!"FbItB.EY$r(B1$BCJJQ$($k(B (CV$BCM(B1=$BLs(B0.13$BIC%9%1!<%k(B)
        if (speed_tick == 0) {
            unsigned char actual_target_dir = (f0_4_dir & 0x01) ^ (cv29_c & 0x01);
            
            if (current_motor_dir != actual_target_dir) {
                // $B0BA4J]8n(B: $BAv9TCf$K5UE>L?Na$,Mh$?>l9g$O!"Dd;_$9$k$^$G(BCV4$B%l!<%H$G6/@)8:B.(B
                kick_timer = 0; 
                if (current_speed > 0) {
                    if (cv4_c == 0) current_speed = 0; 
                    else { current_speed--; speed_tick = cv4_c; } 
                } else {
                    current_motor_dir = actual_target_dir; 
                }
                motor_out_update = 1;
            } 
            else if (current_speed < target_speed) {
                // $B2CB.(B: $BL\I8B.EY$^$G(BCV3$B%l!<%H$G=y!9$K>e$2$k(B
                if (cv3_c == 0) current_speed = target_speed;
                else { current_speed++; speed_tick = cv3_c; } 
                motor_out_update = 1;
            } 
            else if (current_speed > target_speed) {
                // $B8:B.(B: $BL\I8B.EY$^$G(BCV4$B%l!<%H$G=y!9$K2<$2$k(B
                if (cv4_c == 0) current_speed = target_speed;
                else { current_speed--; speed_tick = cv4_c; } 
                motor_out_update = 1;
            }
        }
    }
}

// =========================================================
// $B!Z%a%$%s4X?t![(B $B%W%m%0%i%`$N?4B!It(B
// =========================================================
void main(void) {
    init_hardware();    
    load_cv_cache();    
    load_saved_state(); 
    
    // $B5/F0D>8e$N=PNOH?1G(B ($B=VCG$+$i$N%l%8%e!<%`(B)
    func_out();       
    motor_out();        

    while(1) {
        // DCC$B%Q%1%C%H$NBT5!$H<u?.(B ($B?.9f$,ES@d$($?$i$9$0$KLa$C$F$/$k(B)
        receive_dcc(); 
        
        if (packet_ready) {
            packet_ready = 0; 
            
            // XOR$B%A%'%C%/%5%`$N3NG'(B ($B%G!<%?2=$18!>Z(B)
            if ((data1 ^ data2 ^ data3 ^ data4 ^ data5 ^ data6) == 0) {
                
                // $B%j%;%C%H%Q%1%C%H(B (00 00 00) $B$N=hM}(B
                if (data1 == 0x00 && data2 == 0x00) {
                    target_speed = 0;
                    current_speed = 0;
                    motor_out_update = 1;
                }
                else if ((data1 & 0xF0) == 0x70) {
                    process_service_mode();   
                }
                else {
                    process_operation_mode(); 
                }
            }
            
            func_out(); 
            save_state_if_changed(); 
            
            // $BN>6K@-(BFx$B%b!<%I;~$OB(;~H?1G(B
            if (cv30_c == 2) motor_out_update = 1;
        }

        // $B2C8:B.$H%-%C%/%9%?!<%H$N%P%C%/%0%i%&%s%I7W;;(B
        process_momentum();

        // $BL\I8B.EY$d%b%a%s%?%`$K$h$C$F=PNO$9$Y$-CM$KJQ99$,$"$C$?>l9g$N$_(BPWM$B$r99?7(B
        if (motor_out_update) {
            motor_out();
            motor_out_update = 0;
        }
    }
}