#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <stdbool.h>
#include <string.h>

#define PAGE_SIZE (4*1024)
#define BLOCK_SIZE (4*1024)
#define BCM2708_PERI_BASE        0x20000000
#define GPIO_BASE                (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
#define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
#define OUT_GPIO(g) *(gpio+((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))

#define GPIO_SET *(gpio+7)  // sets   bits which are 1 ignores bits which are 0
#define GPIO_CLR *(gpio+10) // clears bits which are 1 ignores bits which are 0
#define GPIO_LRD *(gpio+13) // pin level read

#define DELT 400

int  					mem_fd;
void 					*gpio_map;
volatile unsigned *gpio;

void setup_io()
{	if ((mem_fd=open("/dev/mem", O_RDWR|O_SYNC))<0)
   {	printf("can't open /dev/mem \n");
      exit(-1);}
   gpio_map=mmap(NULL,BLOCK_SIZE,PROT_READ|PROT_WRITE,MAP_SHARED,
   	mem_fd,GPIO_BASE);
   close(mem_fd);
   if (gpio_map==MAP_FAILED)
   {	printf("mmap error %d\n",(int)gpio_map);
      exit(-1);}
   gpio=(volatile unsigned *)gpio_map;}

// motors are numbered 0 through 11

// convert motor number to 'coil' address: 1,2 or 3 for enable
// it returns 8 bit, wline will indicate write 1-5
int MotorAddress(int motor,int coil,int *wline)
{  int m=motor;
	if (m>5) m-=6;
	int addr=3*m+coil-1;
   if (motor<6)
	{	*wline=7;
   	if (addr>7)
   	{	*wline=8;
   		addr-=8;
      	if (addr>7)
      	{	*wline=25;
      		addr-=8;}
   	}
   }
   else
	{	*wline=22;
   	if (addr>7)
   	{	*wline=23;
   		addr-=8;
      	if (addr>7)
      	{	*wline=25;
      		addr-=6;}
   	}
   }
	return addr;}

void SetMotorBit(int motor,int coil,bool dat,int dt)
{	int wline,addr=MotorAddress(motor,coil,&wline);
   GPIO_CLR=1<<24;							// clear data line
   if (addr&1) GPIO_SET=1<<9;
   else GPIO_CLR=1<<9;
	if (addr&2) GPIO_SET=1<<10;
   else GPIO_CLR=1<<10;
	if (addr&4) GPIO_SET=1<<11;
   else GPIO_CLR=1<<11;
   OUT_GPIO(24);								// set data line to output
   if (dat) GPIO_SET=1<<24;
   usleep(200);                       	// wait for address and data to settle
	GPIO_CLR=1<<wline;						// write data (enable low CD4099)
   usleep(400);                        	// 700 for single motor 300 for two
   GPIO_SET=1<<wline;               	// write disable high (CD4099 chip)
   usleep(100);}

void InitSteppers()			// initialize stepper states to 00
{  int i;
   OUT_GPIO(7);
   OUT_GPIO(8);
   OUT_GPIO(9);
   OUT_GPIO(10);
   OUT_GPIO(11);
   OUT_GPIO(17);
   OUT_GPIO(18);
   INP_GPIO(24);
   OUT_GPIO(24);
   OUT_GPIO(25);
	GPIO_SET=1<<7;		// write 1 disable
	GPIO_SET=1<<8;		// write 2 disable
   GPIO_SET=1<<25;	// write 3 disable
   GPIO_SET=1<<17;	// read 1 disable
   GPIO_SET=1<<18;	// read 2 disable
   for (i=0;i<12;i++) MotorOff(i);}

int MotorOff(int motor)
{	SetMotorBit(motor,3,false,DELT);
   SetMotorBit(motor,1,false,DELT);
   SetMotorBit(motor,2,false,DELT);
   return 0;}

void RelayOff()
{	GPIO_CLR=1<<9;
   GPIO_CLR=1<<10;
   GPIO_SET=1<<11;
   GPIO_CLR=1<<24;
   usleep(200);                       	// wait for address and data to settle
	GPIO_CLR=1<<25;						// write data (enable low CD4099)
   usleep(400);                        	// 700 for single motor 300 for two
   GPIO_SET=1<<25;               	// write disable high (CD4099 chip)
   usleep(100);}

int main(int argc, char* argv [])
{  setup_io();
	InitSteppers();				// initialize motors
   RelayOff();
	return 0;}