#include <ch.h>
#include <hal.h>
#include "dccput.h"

/*
This code is an example of using the openocd debug message system.

Before the message output is seen in the debug window, the functionality
will need enabling:

** GDB **
From the gdb prompt: monitor target_request debugmsgs enable

** Telnet **
From the Telnet prompt: target_request debugmsgs enable

Spen
spen@spen-soft.co.uk
*/

//Sine table for the TMC236 with mixed decay on falling slope. 
//Left turn table and right turn table different in order to keep phase direction bit 
// stable at 0 current

unsigned char sinus_tab_l[64]=
{
0x20, 0x22, 0x26, 0x28, 0x2c, 0x2e, 0x30, 0x34, 0x36, 0x38, 0x38, 0x3a, 0x3c, 0x3c, 0x1e, 0x1e,
0x1e, 0x1e, 0x1e, 0x1c, 0x1c, 0x1a, 0x18, 0x18, 0x16, 0x14, 0x10, 0x0e, 0x0c, 0x08, 0x06, 0x02,
0x21, 0x23, 0x27, 0x29, 0x2d, 0x2f, 0x31, 0x35, 0x37, 0x39, 0x39, 0x3b, 0x3d, 0x3d, 0x1f, 0x1f,
0x1f, 0x1f, 0x1f, 0x1d, 0x1d, 0x1b, 0x19, 0x19, 0x17, 0x15, 0x11, 0x0f, 0x0d, 0x09, 0x07, 0x03
};
unsigned char sinus_tab_r[64]=
{
0x21, 0x02, 0x06, 0x08, 0x0c, 0x0e, 0x10, 0x14, 0x16, 0x18, 0x18, 0x1a, 0x1c, 0x1c, 0x1e, 0x1e,
0x1e, 0x1e, 0x1e, 0x3c, 0x3c, 0x3a, 0x38, 0x38, 0x36, 0x34, 0x30, 0x2e, 0x2c, 0x28, 0x26, 0x22,
0x20, 0x03, 0x07, 0x09, 0x0d, 0x0f, 0x11, 0x15, 0x17, 0x19, 0x19, 0x1b, 0x1d, 0x1d, 0x1f, 0x1f,
0x1f, 0x1f, 0x1f, 0x3d, 0x3d, 0x3b, 0x39, 0x39, 0x37, 0x35, 0x31, 0x2f, 0x2d, 0x29, 0x27, 0x23
};

unsigned char PhaseCount=0;
char dir_store=0;


/**********************************************************
  This function generates the
  microsteps. The values are read from the
  table and output to the TMC236 or TMC239
  via the SPI interface.
  Call this function with the "dir" parameter set to:
  dir=-1: step in negative direction
  dir=+1: step in positive direction.
  dir= 0: update the driver, e.g. after a power down or for 
          a change of the mixed decay flag
  
  Call this function for example in a timer interrupt.
  
  md controls the mixed decay bit in the driver:
  md=0: Slow decay only
  md=1: Mixed decay only on falling slope
  md=2: Mixed decay always

	The SPI output of the data has to be adapted here, as it
	is controller specific.
	Here, an AVR processor is assumed where SPI output can be
	done just by setting the SPDR register.
***********************************************************/
unsigned int step(char dir, unsigned char md)
{
  unsigned int io;

  PhaseCount+=dir;
  if ((int8_t)dir > 0) dir = 1;
  else if ((int8_t)dir < 0) dir = -1;

  if (dir != 0) dir_store = dir;
  if(dir_store==1) 
    io=(sinus_tab_r[PhaseCount & 63]<<6) | sinus_tab_r[(PhaseCount+16) & 63];
  else 
    io=(sinus_tab_l[PhaseCount & 63]<<6) | sinus_tab_l[(PhaseCount+16) & 63];
  
  if(md==2) io |= 0x820;      // Set mixed decay bits to 1, if desired
  else if (md==0) io &= 0x7DF;  // Clear mixed decay bits if mixed decay always off.

return io;
}

static void spicb(SPIDriver *spip);

static SPIConfig spicfgcambridge = {
 spicb,
 GPIOB,
 12,
 (SPI_CR1_DFF  |SPI_CR1_BR_2|SPI_CR1_BR_1)
};

static SPIConfig spicfgtrinamic = {
 spicb,
 GPIOA,
 15,
 (SPI_CR1_DFF  |SPI_CR1_BR_1| SPI_CR1_CPOL | SPI_CR1_CPHA)
};

static void spicb(SPIDriver *spip)
{
 chSysLockFromIsr();
 spiUnselectI(spip);
 chSysUnlockFromIsr();
}

void hex2ascii (uint16_t value, unsigned char *string)
{
int index;
string[4] = 0;

for (index=3;index>=0;index--)
 {
   string[index] = '0' + (value & 0xf);
   if (string[index] > ('0' + 9)) string[index] += 7;
   value = value >> 4;
 }
}

#define SYSIOADDR 0x2
#define CHANNEL 2
#define SICADDR 0x12
#define SCWADDR 0x0
#define WRITEFLAG 0xF000
#define NEWFLAG 0x20
#define VALIDFLAG 0x10
#define GOBIT 0x1
#define SIEBIT 0x80
#define SIFBIT 0x40
#define SMAPIO1 0x1
#define SCTRLNEW 0x10
#define SAUTOCLRNO 0x0
#define SAUTOCLRBEFORE 0x40
#define IO1AH 0x2
#define IO1DO 0x1

int readResult(unsigned int channel, int16_t *position)
{
uint16_t addrw, addrr;
uint16_t scww, scwr;
uint16_t ptefw,ptefr;
uint16_t resaw,resar;

addrw = (channel&0x3)<<8;
scww = 0;
ptefw = 0;
resaw = 0;

while (!palReadPad(GPIOB,11));
//do
{
  chSysLockFromIsr();
  spiSelectI(&SPID2);
  palClearPad(GPIOB, 10);
  spiExchange(&SPID2, 1, &addrw, &addrr); 
  spiExchange(&SPID2, 1, &scww, &scwr); 
  spiExchange(&SPID2, 1, &ptefw, &ptefr); 
  spiExchange(&SPID2, 1, &resaw, &resar); 
  palSetPad(GPIOB, 10);
  spiUnselectI(&SPID2);
  chSysUnlockFromIsr();
}
//while ((!(scwr&NEWFLAG)));
if (!(scwr&VALIDFLAG)) return -1;
*position = resar;
return 0; 
}


int startRead (unsigned int channel)
{
uint16_t addrw, addrr;
uint16_t scww, scwr;
uint32_t portdata;

//char valStr[255];

addrw = ((channel&0x3)<<8)|WRITEFLAG;
scww = GOBIT|SIEBIT;

  chSysLockFromIsr();
  spiSelectI(&SPID2);
  palClearPad(GPIOB, 10);
  spiExchange(&SPID2, 1, &addrw, &addrr); 
  spiExchange(&SPID2, 1, &scww, &scwr); 
  palSetPad(GPIOB, 10);
  spiUnselectI(&SPID2);
  chSysUnlockFromIsr();

while (palReadPad(GPIOB,11));
//hex2ascii(addrr, (unsigned char *)valStr);
//valStr[4] = ' ';
//hex2ascii(scwr, (unsigned char *)(&valStr[5]));
//DCCputs(valStr);
return 0; 
}

int main(void) {

  int position;
  int16_t positionarray[64];
  int index=0,sumindex;
  uint16_t stepdata,drvstatus;
  char valStr[255];
  halInit();
  chSysInit();
   
  DCCputs("ChiDemo Blinky v1.0 -- 1/8/13 brent@mbari.org");
  
  palSetPadMode(GPIOB, 11, PAL_MODE_INPUT);
  palSetPadMode(GPIOB, 10, PAL_MODE_OUTPUT_PUSHPULL);
  palSetPadMode(GPIOB, 7, PAL_MODE_OUTPUT_PUSHPULL);
  
  spiStart(&SPID2, &spicfgcambridge); 
  spiStart(&SPID1, &spicfgtrinamic); 

  palSetPad(GPIOB, 12);
  palSetPad(GPIOA, 15);
  palSetPadMode(GPIOB, 12, PAL_MODE_OUTPUT_PUSHPULL |
                           PAL_STM32_OSPEED_HIGHEST);           /* NSS.     */
  palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(5) |
                           PAL_STM32_OSPEED_HIGHEST);           /* SCK.     */
  palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(5));              /* MISO.    */
  palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(5) |
                           PAL_STM32_OSPEED_HIGHEST);           /* MOSI.    */

  palSetPadMode(GPIOA, 15, PAL_MODE_OUTPUT_PUSHPULL |
                           PAL_STM32_OSPEED_HIGHEST);           /* NSS.     */
  palSetPadMode(GPIOB, 3, PAL_MODE_ALTERNATE(5) |
                           PAL_STM32_OSPEED_HIGHEST);           /* SCK.     */
  palSetPadMode(GPIOB, 4, PAL_MODE_ALTERNATE(5));              /* MISO.    */
  palSetPadMode(GPIOB, 5, PAL_MODE_ALTERNATE(5) |
                           PAL_STM32_OSPEED_HIGHEST);           /* MOSI.    */
{
  uint16_t addrw, addrr;
  uint16_t dataw, datar;

  addrw = WRITEFLAG|SYSIOADDR;
  dataw = (IO1AH|IO1DO);

  chSysLockFromIsr();
  spiSelectI(&SPID2);
  palClearPad(GPIOB, 10);
  spiExchange(&SPID2, 1, &addrw, &addrr); 
  spiExchange(&SPID2, 1, &dataw, &datar); 
  palSetPad(GPIOB, 10);
  spiUnselectI(&SPID2);
  chSysUnlockFromIsr();
chThdSleepMilliseconds(1);

  addrw = ((CHANNEL&0x3)<<8)|WRITEFLAG|SICADDR;
  dataw = (SMAPIO1|SCTRLNEW|SAUTOCLRNO);

  chSysLockFromIsr();
  spiSelectI(&SPID2);
  palClearPad(GPIOB, 10);
  spiExchange(&SPID2, 1, &addrw, &addrr); 
  spiExchange(&SPID2, 1, &dataw, &datar); 
  palSetPad(GPIOB, 10);
  spiUnselectI(&SPID2);
  chSysUnlockFromIsr();
chThdSleepMilliseconds(1);

  addrw = ((CHANNEL&0x3)<<8)|WRITEFLAG;
  dataw = SIEBIT;

  chSysLockFromIsr();
  spiSelectI(&SPID2);
  palClearPad(GPIOB, 10);
  spiExchange(&SPID2, 1, &addrw, &addrr); 
  spiExchange(&SPID2, 1, &dataw, &datar); 
  palSetPad(GPIOB, 10);
  spiUnselectI(&SPID2);
  chSysUnlockFromIsr();
chThdSleepMilliseconds(1);

}

for (sumindex=0;sumindex<64;sumindex++) positionarray[sumindex]=0;
index=0;
position = 0;

while (1)
{
if (position > 3) stepdata = step(-8,0);
else if (position < -3) stepdata = step(8,0);
else if (position < 0) stepdata = step(1,0);
else if (position>0) stepdata = step(-1,0);

chSysLockFromIsr();
spiSelectI(&SPID1);
    palClearPad(GPIOB, 11);
spiExchange(&SPID1, 1, &stepdata, &drvstatus); 
    palSetPad(GPIOB, 11);
spiUnselectI(&SPID1);
chSysUnlockFromIsr();

for (index=0,position=0;index<1;index++)
{
//chThdSleepMilliseconds(4);
startRead(CHANNEL);
//chThdSleepMilliseconds(4);

if (readResult(CHANNEL,&positionarray[index])>=0)
 {
     position += positionarray[index];
//   index = (index+1)&0x7;
//   for (sumindex=0,position=0;sumindex<8;sumindex++) position += positionarray[sumindex];
//   position = position/512;
//   hex2ascii(position, (unsigned char *)valStr);
//   DCCputs(valStr);
  }
else
  {
   DCCputs("BAD REPORT");
   index=-1;
   position = 0;     
  }

}//End of for loop
position = position/64;
//hex2ascii(position, (unsigned char *)valStr);
//DCCputs(valStr);

}//End of while loop
}//End of main