#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
*/


typedef union union32and16
{
uint32_t u32bit;
int32_t s32bit;
uint16_t u16bit[2];
int16_t s16bit[2];
}UNION32AND16;

//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;



static void spicb(SPIDriver *spip);

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

static SPIConfig spicfgTMC429 = {
 spicb,
 GPIOA,
 15,
 (SPI_CR1_DFF|SPI_CR1_BR_2| 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;

  UNION32AND16 dataout,datain;
  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, &spicfgTMC429); 

  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;

#define REGISTER 0
#define COMMONREG (3 << 29)
#define TYPEREG (9 << 25)
#define READONLY (1 << 24)

while (1)
{

dataout.u32bit = (REGISTER|COMMONREG|READONLY|TYPEREG);
{
uint16_t temporary;
temporary = dataout.u16bit[1];
dataout.u16bit[1] = dataout.u16bit[0];
dataout.u16bit[0] = temporary;
}
chSysLockFromIsr();
spiSelectI(&SPID1);
//spiExchange(&SPID1, 1, &dataout.u16bit[1], &datain.u16bit[1]); 
spiExchange(&SPID1, 2, &dataout.u16bit[0], &datain.u16bit[0]); 
//spiUnselectI(&SPID1);
chSysUnlockFromIsr();

hex2ascii(datain.u16bit[0], (unsigned char *)valStr);
hex2ascii(datain.u16bit[1], (unsigned char *)(&valStr[4]));
DCCputs(valStr);

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

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