/*   KumaLab Robot Library test program (WDT)   */
#include "3048f.h"

/* ========================= */
/* 補助関数群　              */
/* ========================= */

/*#define MOTORTEST 1*/

int SinTable[2048]={   /* sin(0-90deg):1024, atan(0-1):1024*/
#include "sintable.h"
};

/* input: 0(0PI)-65535(2PI-delta)
   output: -32767(-1) - 32767(1) */
int Sine(unsigned int th)
{
  switch(th>>14)
    {
    case 0: return  SinTable[     ((th&0x3fff)>>4)];
    case 1: return  SinTable[1023-((th&0x3fff)>>4)];
    case 2: return -SinTable[     ((th&0x3fff)>>4)];
    case 3: return -SinTable[1023-((th&0x3fff)>>4)];
    }
  return 0;
}

int Cosine(unsigned int th)
{
  switch(th>>14)
    {
    case 3: return  SinTable[     ((th&0x3fff)>>4)];
    case 0: return  SinTable[1023-((th&0x3fff)>>4)];
    case 1: return -SinTable[     ((th&0x3fff)>>4)];
    case 2: return -SinTable[1023-((th&0x3fff)>>4)];
    }
  return 0;
}
#define ATOF 1024
int __atan1(int y,int x)
{
  int a;
  if(y==x) return 8192; /* 45deg */
  if(y<x)
    {
      a=((long)y*1024)/x; /* y/x * 1024 */
      return SinTable[ATOF+a];
    }
  else
    {
      a=((long)x*1024)/y; /* y/x * 1024 */
      return 16384-SinTable[ATOF+a];
    }
}

int atan2(int y,int x)
{
  if(x==0)
    {
      if(y==0) return 0; /* unknown */
      if(y>0) return 16384;  /* 90deg*/
      else return -16384; /* -90deg*/
    }
  if(x>0)
    {
      if(y==0) return 0; /* 0deg*/
      if(y>0)  return  __atan1( y,x);
      else     return -__atan1(-y,x);
    }
  else
    {
      if(y==0) return -32768; /* -180deg*/
      /*if(y>0)  return 32768-__atan1( y,-x);*/
      /*else     return __atan1(-y,-x)-32768;*/
      if(y>0)  return (-__atan1( y,-x))^0x8000;
      else     return __atan1(-y,-x)^0x8000;
    }
}

/* ========================= */
/* ＤＤＳおよび積和          */
/* ========================= */


int DDSPhase;
int DDSCount;
int DDSFreq;
#define DDSCycle=32768
long FRSc;
long FRSs;
int FRs,FRc;
int DDSls,DDSlc;

void DDS(void)
{
  int a,s,c,i;
  DDSPhase=(DDSPhase+DDSFreq)&0x7fff;
  s=Sine(DDSPhase<<2);
  c=Cosine(DDSPhase<<2);
  /* DDS */
  DA.DR0=(unsigned int)(c>>9)+128;
  DA.DR1=(unsigned int)(s>>9)+128;
  DDSCount++; DDSCount=DDSCount&0x3fff;

  /* characteristic */
  /* order */
  /* (AD:9bit sin:15bit :24bit)>18bit count:14bit*/ 
  if(DDSCount==0)
    {
      FRs=(int)(FRSs>>16);
      FRc=(int)(FRSc>>16);
      FRSc=0; FRSs=0;
    }
  else
    {
      i=(int)(FRSs>>16)+(int)(FRSc>>16); /* dummy */
    }
  for(i=0;i<25;i++)
    ;
  a=(AD.DRA>>6)-512;
  FRSs+=((((long)DDSls)*a+0x20)>>6);
  FRSc+=((((long)DDSlc)*a+0x20)>>6);
  DDSls=s; DDSlc=c;
  AD.CSR.BYTE=0x20;
}

void InitializeDDS(void)
{
  DDSPhase=0;  DDSCount=0;
  DDSFreq=1;
  /* continue AN0-AN0 */
  AD.CSR.BYTE=0x00; /* 0xb9;   /* 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;
}

/* ========================= */
/* モータ駆動 　             */
/* ========================= */
long MTPhase;
int MTFreq;
int MTMode;
unsigned char MTPattern[3][8]=
{
  {0x1,0x2,0x4,0x8,0x1,0x2,0x4,0x8},  /* 1 phase */
  {0x3,0x6,0xc,0x9,0x3,0x6,0xc,0x9},  /* 1-2 phase */
  {0x1,0x3,0x2,0x6,0x4,0xc,0x8,0x9},
};

void InitializeMotor(void)
{
  MTFreq=0; MTPhase=0; MTMode=0;
  P1.DDR=0x0f;
}

void Motor(void)
{
  int st;
  MTPhase+=MTFreq;
  st=(MTPhase>>15)&7;  /* 32768=2^15Hz */
  P1.DR.BYTE=MTPattern[MTMode][st];
}

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

volatile long SysTime;

/* about 16384 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-21)  /* 255-95 */
#endif

#ifdef MOTORTEST
int WDTSHORTCUTS[8]={234,234,235,234,234,235,234,234};
#else
int WDTSHORTCUTS[8]={210,211,211,210,211,211,210,211};
#endif
int WDTSCc;

void WDTInterrupt(void)
{
  WDTSCc=(WDTSCc+1)&0x7;
  /**((unsigned int *)0xffffa8)=0x5a00 | WDTSHORTCUT;  /* WDT TCNT */
  *((unsigned int *)0xffffa8)=0x5a00 | WDTSHORTCUTS[WDTSCc];  /* WDT TCNT */
  if(*((unsigned char *)0xffffa8)&0x80)
    {
      *((unsigned int *)0xffffa8)=0xa538 | WDTCYCLE;    /* WDT Clear 0011 1111 */
      SysTime++;
#ifdef MOTORTEST
      Motor();
#else
      DDS();
#endif
    }
}

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


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

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

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

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

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

unsigned char KLR_LEDpat[16]={
#if 0
  /* 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,
#else
  /* 0:10111101 */ 0xbd,
  /* 1:00010100 */ 0x14,
  /* 2:00111011 */ 0x3b,
  /* 3:00011111 */ 0x1f,
  /* 4:10010110 */ 0x96,
  /* 5:10001111 */ 0x8f,
  /* 6:10101111 */ 0xaf,
  /* 7:10010101 */ 0x95,
  /* 8:10111111 */ 0xbf,
  /* 9:10011111 */ 0x9f,
  /* A:10110111 */ 0xb7,
  /* b:10101110 */ 0xae,
  /* c:00101010 */ 0x2a,
  /* d:00111110 */ 0x3e,
  /* E:10101011 */ 0xab,
  /* F:10100011 */ 0xa3,
#endif
};

int __KLR_LEDHKC;
unsigned char LEDPattern[4];
void KLRLEDKeyCycle(void)
{
  unsigned long sw;
  __KLR_LEDHKC=(__KLR_LEDHKC+1)&0x3;
  /*KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                 (((KLR_LEDpat[(KLRLK_LEDValue>>(__KLR_LEDHKC*4))&0xf])|
                   (((KLRLK_DPMask>>__KLR_LEDHKC)&1)?0x10:0))<<8)),&sw);*/
  KLRLEDKeyRaw(~((0x8>>__KLR_LEDHKC)|
                 (LEDPattern[__KLR_LEDHKC]<<8)),&sw);
  KLRLK_SWValue=sw&0xffff;
  KLRLK_Push=(sw&0x10000)?1:0;
}

void InitializeKLRLEDKey(void)
{
  KLR_LEDKEYP.DDR=0x0e;
  KLRLK_LEDValue=0;
  KLRLEDKeyCycle();
}

/* 数値をＢＣＤ表記に変換する */
/* 例：１２３→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;
}

/* 平方根 */
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;
}


void SetLEDValue(int val,int zerosup,int dpmask)
{
  int i,sf;
  sf=0;
  if(val<0) { sf=1; val=-val; }
  val=HexToInt(val);
  for(i=0;i<4;i++)
    LEDPattern[i]=((dpmask>>i)&1)?0x40:0;
  for(i=0;i<4;i++)
    LEDPattern[i]|=KLR_LEDpat[(val>>(i*4))&0xf];
  if(zerosup)
    {
      if(sf) LEDPattern[3]|=0x02;
      return ;
    }
  for(i=3;i>=1;i--)
    {
      if(LEDPattern[i]!=KLR_LEDpat[0]) break;
      LEDPattern[i]=0x0;
    }
  if(sf)
    {
      if(i==3) LEDPattern[3]|=0x02;
      else LEDPattern[i+1]=0x02;
    }
}

void SetLEDHex(unsigned int val,int dpmask)
{
  int i;
  for(i=0;i<4;i++)
    LEDPattern[i]=((dpmask>>i)&1)?0x40:0;
  for(i=0;i<4;i++)
    LEDPattern[i]|=KLR_LEDpat[(val>>(i*4))&0xf];
}

int __dummyi=1;

#ifdef MOTORTEST
/* Main for Motor */
void main(void)
{
  long nexttime;
  unsigned long sec=0;
  unsigned long sw;
  int cmdspeed,slowup;
  char *__dummys="ccc";
  long sspeed;
  
  InitializeMotor();
  InitializeWDTTimer();
  InitializeKLRLEDKey();

  /* poweron setup */
  sspeed=0;
  nexttime=SysTime;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+33;  /* 32768Hz/1000Hz */
      sec++; /* sec は 0.001s ごとにカウント */

      KLRLEDKeyCycle();
      cmdspeed=BCDToInt(KLRLK_SWValue&0xfff);
      MTMode=(KLRLK_SWValue>>12)&0x3;
      slowup=(KLRLK_SWValue>>12)&0x4;
      if(!KLRLK_Push)
        cmdspeed=0;

      if(!slowup)
        {
          MTFreq=cmdspeed;
        }
      else  /* slowupmode */
        {
          if(!KLRLK_Push)
            cmdspeed=0;
          if((sspeed>>2)<cmdspeed)
            sspeed++;
          if((sspeed>>2)>cmdspeed)
            sspeed--;
          MTFreq=sspeed>>2;
        }
      
      SetLEDValue(MTFreq,1,0);
    }
}

#else
/* Main for Analog */
void main(void)
{
  long nexttime;
  unsigned long sec;
  unsigned long sw;
  int disptimer,lddsf;
  char *__dummys="ccc";
  
  InitializeDDS();
  InitializeWDTTimer();
  InitializeKLRLEDKey();

  DDSFreq=100;
  nexttime=SysTime;
  sec=0;
  disptimer=0;
  while(1)
    {
      while(SysTime<nexttime)
        ;
      nexttime=SysTime+16;  /* 16384Hz/1000Hz */
      sec++; /* sec は 0.001s ごとにカウント */

      KLRLEDKeyCycle();
      /*if(KLRLK_Push)
        KLRLK_LEDValue=HexToInt(
          BCDToInt(KLRLK_SWValue&0xff)+
          BCDToInt(KLRLK_SWValue>>8)
          );
      else
         KLRLK_LEDValue=KLRLK_SWValue;*/
      lddsf=DDSFreq;
      DDSFreq=BCDToInt(KLRLK_SWValue&0xfff);
      if(DDSFreq==0) DDSFreq=1000;
      
      if(DDSFreq!=lddsf)
        disptimer=1000;
      /*SetLEDHex(0xdef0,0);
      continue;*/
      
      if(KLRLK_Push)
        {
          SetLEDHex(((sec>>11)&1)?FRc:FRs,0);
        }
      else
        {
          if(disptimer>0)
            {
              disptimer--;
              SetLEDValue(DDSFreq,0,0x1);
            }
          else if((sec>>11)&1)
            {
              SetLEDValue(SqrtI((long)FRc*FRc+(long)FRs*FRs)*1000/0x1fB0,1,0x8);
            }
          else
            {
/*#define INV*/
#ifdef INV
              int deg=((long)atan2(FRs,-FRc)*1800)/32768l;
#else
              int deg=((long)atan2(-FRs,FRc)*1800)/32768l;
#endif
              SetLEDValue(deg,0,0x2);
            }
        }
    }
}
#endif