/* ================================================================ *
 * Kumagai Labo. Robot Control Library  for H8/3052                 *
 *                                        by KUMAGAI Masaaki        *
 *     Ver 1.1 / 060501
 *
 * ================================================================ */


/* Dummys */
int __dummyc=0;
char *__dummys="d";

/* dependency */
#ifdef USE_KLR_DEAD_RECKONING
#define USE_KLR_STEP_MOTOR
void KLRDR_DeadReckoning(int r,int l);
#endif

#ifdef USE_KLR_STEP_MOTOR
#define USE_KLR_WDT_CLOCK
void KLRM_StepOut(void);
#endif



/* ========================= */
/* 内部クロック              */
/* ========================= */
#pragma interrupt(WDTInterrupt)

#ifdef USE_KLR_WDT_CLOCK

volatile long SysTime;

/* Default setting for about 8192 Hz */
#ifndef WDTCYCLE
/* 0: 48.8k  1: 3k  2:1.5k  3:763Hz  4:381Hz  5:191Hz  6:47.7Hz  7: 23.8Hz */
#define WDTCYCLE 1
#endif
#ifndef WDTSHORTCUT
#define WDTSHORTCUT (255-95)  /* 255-95 */
#endif

void WDTInterrupt(void)
{
  *((unsigned int *)0xffffa8)=0x5a00 | WDTSHORTCUT;  /* WDT TCNT */
  if(*((unsigned char *)0xffffa8)&0x80)
    {
      *((unsigned int *)0xffffa8)=0xa538 | WDTCYCLE;    /* WDT Clear 0011 1111 */
      SysTime++;
#ifdef USE_KLR_STEP_MOTOR
      KLRM_StepOut();
#endif
    }
}

void InitializeWDTTimer(void)
{
  /* Timer WDT */
  *((unsigned int *)0xffffa8)=0xa538 | WDTCYCLE;    /* WDT Clear 0011 1111 */
  SysTime=0;
}


#else  /* USE_KLR_WDT_CLOCK */
void WDTInterrupt(void)  /* dummy */
{
}
#endif /* USE_KLR_WDT_CLOCK */

/* ========================= */
/* ラジコンサーボ　　　　　　*/
/* ========================= */

#ifdef USE_KLR_RCSERVO
volatile unsigned short __KLRS_Command[16];
volatile unsigned short *KLRS_Command;
volatile unsigned char __KLRS_Address[8];

void SetupKLRServo(void);

void InitializeKLRServo(void)
{
  int i;
  KLRS_Command=__KLRS_Command;
  for(i=0;i<16;i++)
    KLRS_Command[i]=10000;
  for(i=0;i<8;i++)
    __KLRS_Address[i]=i<<4;
  SetupKLRServo();
}

void SetKLRServoBase(unsigned short *d)
{
  KLRS_Command=d;
  SetupKLRServo();
}
  

void SetupKLRServo(void)
{
  /* Disabled */
  ITU.TSTR.BYTE&=(~0x18); /* ITU3,4 stop */
  /* DMAstop */
  DMAC0A.DTCR.BYTE&=0x7f;  /* disable */
  DMAC1A.DTCR.BYTE&=0x7f;  /* disable */
  DMAC1B.DTCR.BYTE&=0x7f;  /* disable */

  /* ITU3 */
  ITU3.TCR.BYTE=0x20;
  ITU3.GRA=0xf424;    /* 25,000,000 / 0xf424(62500) = 400Hz = 2.5ms = 20ms / 8 */
  ITU3.TCNT=0;
  /* ITU4 */
  ITU4.TCR.BYTE=0x20;
  ITU4.GRA=0xf424;    /* 25,000,000 / 0xf424(62500) = 400Hz = 2.5ms = 20ms / 8 */
  ITU4.TCNT=0;
  
  PB.DDR=0xf0;

  /* DMA 0A */
  DMAC0A.MAR=__KLRS_Address;
  DMAC0A.IOAR=((int)&(PB.DR.BYTE))&0xff; 
  DMAC0A.ETCR=0x0808; /* loop, 8unit */
  DMAC0A.DTCR.BYTE=DMAC0A.DTCR.BYTE|0x93;  /* byte, source=ITU3 Cmatch */
  /*               ~~~~~~~~~~~~~~~~~  force reading ZERO of DTE        */
  
  /* DMA 1A */
  DMAC1A.MAR=KLRS_Command;
  DMAC1A.IOAR=((unsigned int)&(ITU3.GRB))&0xff;
  DMAC1A.ETCR=0x0808; /* loop, 8unit */
  DMAC1A.DTCR.BYTE=DMAC1A.DTCR.BYTE|0xd3;  /* word, source=ITU3 Cmatch */
  /* DMA 1B */
  DMAC1B.MAR=KLRS_Command+8;
  DMAC1B.IOAR=((unsigned int)&(ITU4.GRB))&0xff;
  DMAC1B.ETCR=0x0808; /* loop, 8unit */
  DMAC1B.DTCR.BYTE=DMAC1B.DTCR.BYTE|0xd3;  /* word, source=ITU3 Cmatch */
  
  /* ITU3 INT enable */
  ITU3.TSR.BYTE=0;
  ITU3.TIER.BIT.IMIEA=1;
  /* ITU Start */
  ITU.TMDR.BYTE |= 0x18; /* PWM */
  ITU.TSTR.BYTE |= 0x18; /* Count start */
}

#endif /* USE_KLR_RCSERVO */



/* ========================= */
/* ステッピングモータ駆動　　*/
/* ========================= */
#ifdef USE_KLR_STEP_MOTOR

#ifndef KLRM_PORT
#define KLRM_PORT P1
#endif

unsigned char KLRM_PatternL[8],KLRM_PatternR[8];
unsigned char __KLRM_Pattern[8]={0x1,0x3,0x2,0x6,0x4,0xc,0x8,0x9}; /* 1-2相 */
volatile unsigned int KLRM_SPCR,KLRM_SPCL;     /* 位相進行カウンタ */
volatile unsigned int KLRM_SpeedR,KLRM_SpeedL; /* モータスピード */
        /* unsigned でいい？ */
volatile unsigned int KLRM_SLIndexR,KLRM_SLIndexL; /* 前回の出力パターン */

void KLRM_StepOut(void)
{
  unsigned char pl,pr;
  unsigned int sl,sr;
  KLRM_SPCR+=KLRM_SpeedR;
  KLRM_SPCL+=KLRM_SpeedL;
  sr=(KLRM_SPCR>>13)&0x7;
  sl=(KLRM_SPCL>>13)&0x7;

  pr=KLRM_PatternR[sr];
  pl=KLRM_PatternL[sl];

  KLRM_PORT.DR.BYTE= pr | pl;

  /* 自己位置推定 */
  pr=(sr-KLRM_SLIndexR)&7;  /* 進むと1、戻ると7、維持で0*/
  pl=(sl-KLRM_SLIndexL)&7;
  
#ifdef USE_KLR_DEAD_RECKONING
  if(pr==1) KLRDR_DeadReckoning( 1, 0);
  if(pr==7) KLRDR_DeadReckoning(-1, 0);
  if(pl==1) KLRDR_DeadReckoning( 0, 1);
  if(pl==7) KLRDR_DeadReckoning( 0,-1);
#endif
  
  KLRM_SLIndexR=sr;  KLRM_SLIndexL=sl;
}

void InitializeKLRSteppingMotor(void)
{
  int i;
  for(i=0;i<8;i++)
    {
#ifdef KLRSM_REVR
      KLRM_PatternR[i]=(__KLRM_Pattern[i]);
#else
      KLRM_PatternR[i]=(__KLRM_Pattern[7-i]);
#endif
#ifdef KLRSM_REVL
      KLRM_PatternL[i]=(__KLRM_Pattern[7-i]<<4);
#else
      KLRM_PatternL[i]=(__KLRM_Pattern[i]<<4);
#endif
    }
  KLRM_SPCR=KLRM_SPCL=0;
  KLRM_SpeedR=KLRM_SpeedL=0;
  KLRM_SLIndexR=KLRM_SLIndexL=0;

  KLRM_PORT.DR.BYTE=0;
  KLRM_PORT.DDR=0xff;
}
#endif /* USE_KLR_STEP_MOTOR */



/* ========================= */
/* デッドレコニング　　　　　*/
/* ========================= */
#ifdef USE_KLR_DEAD_RECKONING
volatile long KLRDR_PosX,KLRDR_PosY;   /* 座標：ステップ単位 */
volatile int  KLRDR_Yaw;           /* 角度：内部単位 0:0度 YAWSTEP で360度 */
volatile long KLRDR_RelYaw;        /* 角度：相対回転角度 */
volatile long KLRDR_AbsStep,KLRDR_RelStep;
  /* 進行ステップ：RelStep 前進＋後進−カウント  AbsStep 前進後進とも＋ */
  /*       前方直進時：ともに移動ステップ＊２ 、おおざっぱに移動距離＊２*/

void KLRDR_DeadReckoning(int r,int l)
{
  if(r>0)   /* 右、前 */
    {
      KLRDR_PosX+=XDeltaTable[KLRDR_Yaw];
      KLRDR_PosY+=YDeltaTable[KLRDR_Yaw];
      KLRDR_Yaw=(KLRDR_Yaw+1)%YAWSTEP;     KLRDR_RelYaw++;
      KLRDR_RelStep++; KLRDR_AbsStep++;
    } else if(l>0) {   /* 左、前 */
      KLRDR_Yaw=(KLRDR_Yaw+YAWSTEP-1)%YAWSTEP;  KLRDR_RelYaw--;
      KLRDR_PosX+=XDeltaTable[KLRDR_Yaw];
      KLRDR_PosY+=YDeltaTable[KLRDR_Yaw];
      KLRDR_RelStep++; KLRDR_AbsStep++;
    } else if(r<0) {   /* 右、後 */
      KLRDR_Yaw=(KLRDR_Yaw+YAWSTEP-1)%YAWSTEP;  KLRDR_RelYaw--;
      KLRDR_PosX-=XDeltaTable[KLRDR_Yaw];
      KLRDR_PosY-=YDeltaTable[KLRDR_Yaw];
      KLRDR_RelStep--; KLRDR_AbsStep++;
    } else if(l<0) {   /* 左、後 */
      KLRDR_PosX-=XDeltaTable[KLRDR_Yaw];       
      KLRDR_PosY-=YDeltaTable[KLRDR_Yaw];
      KLRDR_Yaw=(KLRDR_Yaw+1)%YAWSTEP;          KLRDR_RelYaw++;
      KLRDR_RelStep--; KLRDR_AbsStep++;
    }
}

void InitializeDeadReckoning(void)
{
  KLRDR_PosX=KLRDR_PosY=0;
  KLRDR_Yaw=0;
  KLRDR_RelStep=KLRDR_AbsStep=0;
  KLRDR_RelYaw=0;
}


#endif /* USE_KLR_DEAD_RECKONING */


/* ========================= */
/* カラーセンサ  　　　　　　*/
/* ========================= */
#ifdef USE_KLR_COLOR_SENSOR

#ifndef KLRCS_NUMSENSOR    /* カラーセンサ数 1-4 */
#define KLRCS_NUMSENSOR 4
#endif

#ifndef KLRCS_SPERIOD      /* 処理周期 50ms程度以上 */
#define KLRCS_SPERIOD 8
#endif

#ifndef KLRCS_REDPOWER     /* 赤RED 出力レベル */
#define KLRCS_REDPOWER 0x50
#endif
#ifndef KLRCS_GREENPOWER   /* 緑LED 出力レベル */
#define KLRCS_GREENPOWER 0x10
#endif
#ifndef KLRCS_OFFSET       /* オフセットレベル */
#define KLRCS_OFFSET 0x0
#endif
#ifndef KLRCS_THRESHOLD
#define KLRCS_THRESHOLD 0x20 /* 閾値 */
#endif

#define KLRCS_UNKNOWN 0
#define KLRCS_RED 1
#define KLRCS_GREEN 2
#define KLRCS_YELLOW 3

#define KLRCS_WAIT 0
#define KLRCS_STATE 1
#define KLRCS_UPDATED 2

int KLRCS_Sense[KLRCS_NUMSENSOR];
int KLRCS_RedAD[KLRCS_NUMSENSOR];
int KLRCS_GreenAD[KLRCS_NUMSENSOR];
int KLRCS_state;
int KLRCS_stateint;

int KLRColorSense(void)
{
  int i,c;
  KLRCS_stateint=(KLRCS_stateint+1)%KLRCS_SPERIOD;
  if(KLRCS_stateint) return KLRCS_WAIT; /* 待ち */
  KLRCS_state=(KLRCS_state+1)%2;
  switch(KLRCS_state)
    {
    case 0:
      for(i=0;i<KLRCS_NUMSENSOR;i++)
        KLRCS_RedAD[i]=((*((&AD.DRA)+i))>>6);
      DA.DR0=0xff;  DA.DR1=KLRCS_GREENPOWER;   /* red:off, green:on */
      return KLRCS_STATE;

    case 1:
      for(i=0;i<KLRCS_NUMSENSOR;i++)
        KLRCS_GreenAD[i]=((*((&AD.DRA)+i))>>6);
      DA.DR0=KLRCS_REDPOWER;  DA.DR1=0xff;   /* red:on, green:on */

      for(i=0;i<KLRCS_NUMSENSOR;i++)
        {
          c=KLRCS_RedAD[i]-KLRCS_GreenAD[i]+KLRCS_OFFSET;
          KLRCS_Sense[i]=KLRCS_YELLOW;
          if(c> KLRCS_THRESHOLD) KLRCS_Sense[i]=KLRCS_RED;
          if(c<-KLRCS_THRESHOLD) KLRCS_Sense[i]=KLRCS_GREEN;
        }
      return KLRCS_UPDATED;
    }
  return KLRCS_WAIT;
}

void InitializeKLRColorSense(void)
{
  /* continue AN0-AN3 */
  AD.CSR.BYTE=0xbb;   /* 1011 1011 */

  /* DA enable */
  DA.DR0=0xff;
  DA.DR1=0xff;
  DA.CR.BIT.DAOE0=1;
  DA.CR.BIT.DAOE1=1;
  DA.CR.BIT.DAE=1;
  KLRCS_stateint=KLRCS_state=0;
}

  
#endif /*USE_KLR_COLOR_SENSOR*/ 



/* ========================= */
/* ＬＥＤキーボード  　　　　*/
/* ========================= */

#ifdef USE_KLR_LED_SWITCH

#ifndef KLR_LEDKEYP
#define KLR_LEDKEYP P8  /* 使用ポート */
#endif
#define KLR_LEDKEYLB 16 /* Bit数 */
#define KLR_LEDKEYSB 24 /* Bit数 */

#define KLR_LKCLK B0
#define KLR_LKRCK B1
#define KLR_LKLSD B2
#define KLR_LKSSD B3

unsigned char  KLRLK_Push;     /* プッシュスイッチ状態 */
unsigned short KLRLK_SWValue;  /* ４桁スイッチ状態 BCD*/
unsigned short KLRLK_LEDValue; /* LEDに設定する数値 HEX*/

void KLRLEDKeyRaw(unsigned long led,unsigned long *sw)
{
  int i;
  unsigned long s=0;
  KLR_LEDKEYP.DR.BIT.KLR_LKCLK=0;
  KLR_LEDKEYP.DR.BIT.KLR_LKRCK=0;
  for(i=KLR_LEDKEYLB-1;i>=0;i--)
    {
      KLR_LEDKEYP.DR.BIT.KLR_LKLSD=(led&(1<<i))?1:0;
      KLR_LEDKEYP.DR.BIT.KLR_LKCLK=1;
      KLR_LEDKEYP.DR.BIT.KLR_LKCLK=0;
    }
  KLR_LEDKEYP.DR.BIT.KLR_LKRCK=1;
  KLR_LEDKEYP.DR.BIT.KLR_LKRCK=0;
  KLR_LEDKEYP.DR.BIT.KLR_LKRCK=1;
  KLR_LEDKEYP.DR.BIT.KLR_LKLSD=0;
  for(i=0;i<KLR_LEDKEYSB;i++)
    {
      s<<=1;
      s|=(KLR_LEDKEYP.DR.BIT.KLR_LKSSD)?1:0;
      KLR_LEDKEYP.DR.BIT.KLR_LKCLK=1;
      KLR_LEDKEYP.DR.BIT.KLR_LKCLK=0;
    }
  /* bit swap */
  *sw=((s>>12)&0x0f0f)|((s>>4)&0xf0f0)|((s&0xff)<<16);
}

char KLR_LEDpat[16]={
  /* 0:11001111 */ 0xcf,
  /* 1:00001001 */ 0x09,
  /* 2:01100111 */ 0x67,
  /* 3:01101011 */ 0x6b,
  /* 4:10101001 */ 0xa9,
  /* 5:11101010 */ 0xea,
  /* 6:11101110 */ 0xee,
  /* 7:11001001 */ 0xc9,
  /* 8:11101111 */ 0xef,
  /* 9:11101011 */ 0xeb,
  /* A:11101101 */ 0xed,
  /* b:10101110 */ 0xae,
  /* c:00100110 */ 0x26,
  /* d:00101111 */ 0x2f,
  /* E:11100110 */ 0xe6,
  /* F:11100100 */ 0xe4,
};

int __KLR_LEDHKC;
void KLRLEDKeyCycle(void)
{
  unsigned long sw;
  __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
  KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                 ((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])<<8)),&sw);
  KLRLK_SWValue=sw&0xffff;
  KLRLK_Push=(sw&0x10000)?1:0;
}
  
void InitializeKLRLEDKey(void)
{
  KLR_LEDKEYP.DDR=0x07;
  KLRLK_LEDValue=0;
  KLRLEDKeyCycle();
}

#endif /* USE_KLR_LED_SWITCH */




/* ========================= */
/* カラーセンサTAOS230       */
/* ========================= */
#ifdef USE_KLR_COLOR_SENSOR_V2

/* 設定 */
#define KLRCS_USE_ITU0
#define KLRCS_USE_ITU1
#define KLRCS_USE_ITU2

#ifndef KLRCS_SCALING  /* 出力スケーリング */
#define KLRCS_SCALING 3
#endif

/* センサ側 S2 S3 端子ビット */
#define KLRCS_SRED   0
#define KLRCS_SCLEAR 1
#define KLRCS_SBLUE  2
#define KLRCS_SGREEN 3
/* ユーザ側 */
#define KLRCS_CLEAR 0
#define KLRCS_RED   1
#define KLRCS_GREEN 2
#define KLRCS_BLUE  3
/* マッピング */
unsigned char KLRCS_Map[4]={
  KLRCS_SCLEAR,KLRCS_SRED,KLRCS_SGREEN,KLRCS_SBLUE
  };

unsigned int KLRCS_Sense[3][4];
/* [SENSE]: 0-2 */
/* [COLOR]: CLEAR/RED/GREEN/BLUE */

static int KLRCS_Seq;

void KLRCS_ColorSense(void)
{
  int cs;
  cs=0;
#ifdef KLRCS_USE_ITU0
  KLRCS_Sense[cs][KLRCS_Seq]=ITU0.TCNT;  cs++;
  ITU0.TCNT=0;
#endif
#ifdef KLRCS_USE_ITU1
  KLRCS_Sense[cs][KLRCS_Seq]=ITU1.TCNT;  cs++;
  ITU1.TCNT=0;
#endif
#ifdef KLRCS_USE_ITU2
  KLRCS_Sense[cs][KLRCS_Seq]=ITU2.TCNT;  cs++;
  ITU2.TCNT=0;
#endif
  KLRCS_Seq=(KLRCS_Seq+1)&3;
  PA.DR.BYTE=(KLRCS_SCALING<<4)|(KLRCS_Map[KLRCS_Seq]<<6);
}

void InitializeKLRColorSense(void)
{
  int i,j;
  PA.DDR=0xf0;
  KLRCS_Seq=0;
  for(i=0;i<3;i++)
    for(j=0;j<4;j++)
      KLRCS_Sense[i][j]=0;
  i=0;
#ifdef KLRCS_USE_ITU0
  ITU0.TCR.BYTE=0x04+i;  /* 0 00 00 1?? */
  ITU.TSTR.BIT.STR0=1; i++;
#endif
#ifdef KLRCS_USE_ITU1
  ITU1.TCR.BYTE=0x04+i;  /* 0 00 00 1?? */
  ITU.TSTR.BIT.STR1=1; i++;
#endif
#ifdef KLRCS_USE_ITU2
  ITU2.TCR.BYTE=0x04+i;  /* 0 00 00 1?? */
  ITU.TSTR.BIT.STR2=1; i++;
#endif
}



#endif /* USE_KLR_COLOR_SENSOR_V2 */



/* ========================= */
/* 初期化関数　              */
/* ========================= */
void InitializeKLRlib(void)
{
#ifdef USE_KLR_WDT_CLOCK
  InitializeWDTTimer();
#endif

#ifdef USE_KLR_RCSERVO
  InitializeKLRServo();
#endif

#ifdef USE_KLR_STEP_MOTOR 
  InitializeKLRSteppingMotor();
#endif

#ifdef USE_KLR_DEAD_RECKONING
  InitializeDeadReckoning();
#endif

#ifdef USE_KLR_COLOR_SENSOR
  InitializeKLRColorSense();
#endif
#ifdef USE_KLR_LED_SWITCH
  InitializeKLRLEDKey();
#endif

#ifdef USE_KLR_COLOR_SENSOR_V2
  InitializeKLRColorSense();
#endif
}

/* ========================= */
/* 便利？関数　              */
/* ========================= */

/* 数値をＢＣＤ表記に変換する */
/* 例：１２３→0x１２３       */
unsigned long HexToInt(unsigned long h)
{
  int i;
  unsigned long d=0;
  for(i=0;i<8;i++)
    {
      d=d|((h%10)<<(i*4));
      h=h/10;
    }
  return d;
}

/* 数値をＢＣＤ表記から数値に変換 */
/* 例：１２３→0x１２３       */
unsigned long BCDToInt(unsigned long b)
{
  int i;
  unsigned long d=0;
  unsigned long k=1;
  for(i=0;i<8;i++)
    {
      d+=k*(b&0xf);
      b=b>>4; k=k*10;
    }
  return d;
}

/* 固定小数(16.16形式)数値をＢＣＤ表記に変換する */
/* 例：１２．５→0x００１２３０００       */
unsigned long HexToIntFP16(unsigned long h)
{
  int i;
  unsigned long d=0;
  unsigned long l=h&0xffff;
  h=(h>>16)&0xffff;
  for(i=0;i<4;i++)
    {
      d=d|((h%10)<<(i*4));
      h=h/10;
    }
  for(i=0;i<4;i++)
    {
      l=l*10;
      d=(d<<4)|(l>>16);
      l=l&0xffff;
    }
  return d;
}

/* 固定小数点 16.16x16.16 -> 16.16 */
long Mul_FP16_FP16(long a,long b)
{
  int f=1;
  unsigned long a0,a1,b0,b1;
  if(a<0) { a*=-1; f*=-1; }
  if(b<0) { b*=-1; f*=-1; }
  a1=(a>>16)&0xffff;
  a0= a     &0xffff;
  b1=(b>>16)&0xffff;
  b0= b     &0xffff;
  return (((a1*b1)<<16)+(a1*b0)+(a0*b1)+((a0*b0)>>16)) *f;
}

/* v を min以上max未満に制限する */
#define Limit(v,min,max) ( ((v)<(min))?(min): (((v)>(max))?(max):(v)) )

/* v が +-rの範囲にあればゼロにする */
#define ZeroMask(v,r) (  ((v)>(r))?(v):  (((v)<(-r))?(v):0))


/* 平方根 */
unsigned long SqrtI(long l)
{
  int i;
  unsigned long s,b;
  s=0; b=0x8000;
  if(l<0) l=-l;
  for(i=15;i>=0;i--,b>>=1)
    {
      if((s|b)*(s|b)<=l) s|=b;
    }
  return s;
}