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

#define VERSION	1.77

#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 LIFT	1500 	// lift of paws
#define DELT	700	// single paw


FILE					*file;
enum movements 	{forward,backward,rightturn,leftturn,backrightturn,
						 backleftturn,rightaxis,leftaxis};
int  					mem_fd;
void 					*gpio_map;
volatile unsigned *gpio;
int					LUT[]={3,4,11,1,7,9,0,2,10,5,6,8,0};
int 					SUT[]={3,4,5,0,1,2,6,7,8,9,10,11,0};
int					magcalib[6][3]={
						{290,0,-388,},
                  {288,5,-373,},
                  {280,0,-361,},
                  {291,0,-334,},
                  {260,0,-366,},
                  {304,0,-340,}};

char 					text[512];
bool 					write_sensors=false;
int 					states[12],position[12];
unsigned int		sensorstates=0;
// position is an estimate of the actual motor position (in motor steps)
// state gives the internal stepper motor state
// cycle gives at what point in the movement cycle we are and keeps parameters
// the cycle consists of 300 single steps with paws on the ground and
// 60 delta-5 steps with lifted paws. Hence cycle is from 0 through 359

struct movementcycle
{  enum movements	mymode;
	int				currentcycle;		// value between 0 and 750
	int				bias[6];          // phases in the cycle
   int				hallstates[6];		// hall states of first 6 motors
   bool				isvalid,shouldbedown;
   bool				pawdown[6];			// paw up or down
   bool				synced[6];			// if not synced, there is a problem
} mcycle;

void WriteError(int error) 	// error messages
{  int len;
	FILE*	file;
	file=fopen("/var/www/p2/lasterror.txt","wt");
	if (file!=NULL)
   {  if (error)
   	{	strcpy(text,"error: ");
        	sprintf(&text[strlen(text)],"%d",error);
         len=strlen(text);
         text[len++]=' ';
         text[len++]=' ';
         if (error==1) strcpy(&text[len],"proplem with a position[] entry");
         else if (error==2)
         	strcpy(&text[len],"error in routine CalcCyclePosition()");
      }
      else strcpy(text,"No error.");
      fwrite(text,strlen(text),1,file);
      fclose(file);}
}

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

bool GetHallState(int sensor)
{  unsigned int state,addr;
	bool s;
  	addr=SUT[sensor];   		// correct inverted wiring
	int wr=addr>5 ? 18 : 17;
	if (wr==18) addr-=6;
	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;
   INP_GPIO(24);			 // set data pin 24 for read
   GPIO_CLR=1<<wr;
   usleep(1000);			// wait
   state=GPIO_LRD;
   GPIO_SET=1<<wr;
   s=((state&(1<<24))!=0);
   if (s) state=1;
   else state=0;
   sensorstates&=(~(1<<sensor)); 	// clear
   sensorstates|=(state<<sensor);  // set
   return s;}

void InitSteppers()			// initialize stepper states to 00
{  int i;
   OUT_GPIO(7);      // write 1
   OUT_GPIO(8);      // write 2
   OUT_GPIO(9);      // Address A0
   OUT_GPIO(10);     // Address A1
   OUT_GPIO(11);     // Address A2
   OUT_GPIO(17);
   OUT_GPIO(18);
   OUT_GPIO(24);     // data
   OUT_GPIO(25);     // write 3
	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);
   write_sensors=true;}

// 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
   GPIO_CLR=(addr^7)<<9;
   GPIO_SET=addr<<9;
   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(100);                        // 100 µs is enough
   GPIO_SET=1<<wline;               	// write disable high (CD4099 chip)
   usleep(dt);}                        // before changing anything, wait for the motor to step

int MotorOn(int motor)
{	int state=states[motor];
   if (state&1) SetMotorBit(motor,1,true,DELT);
   if (state&2) SetMotorBit(motor,2,true,DELT);
   SetMotorBit(motor,3,true,DELT);
	return 0;}

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

void RelayOn()
{	OUT_GPIO(24);								// set data line to output
   GPIO_CLR=1<<9;
   GPIO_CLR=1<<10;
   GPIO_SET=1<<11;
   GPIO_SET=1<<24;
   usleep(200);                       	// wait for address and data to settle
	GPIO_CLR=1<<25;							// write data (enable low CD4099)
   usleep(700);                      	// 700 ms
   GPIO_SET=1<<25;               		// write disable high (CD4099 chip)
   usleep(100);}

void RelayOff()
{  int i;
	OUT_GPIO(24);	// set data line to output
   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(700);                        // 700 ms
   GPIO_SET=1<<25;  		             	// write disable high (CD4099 chip)
   usleep(100);
   for (i=0;i<12;i++) MotorOff(i);}

void VStep(int motor,bool advance)  // return new state
{  MotorOn(motor);
	if (advance)
   {	SetMotorBit(motor,2,true,DELT);			// state 2
      usleep(DELT);
   	SetMotorBit(motor,1,true,DELT);   		// state=3
   	usleep(DELT);
   	SetMotorBit(motor,2,false,DELT);		// state=1
      usleep(DELT);}
   else
   { 	SetMotorBit(motor,1,true,DELT);			// state=1
   	usleep(DELT);
   	SetMotorBit(motor,2,true,DELT);			// state=3
      usleep(DELT);
   	SetMotorBit(motor,1,false,DELT);		// state=2
     	usleep(DELT);}
   MotorOff(motor);
   states[motor]=0;}

void HStep(int motor,bool advance)  // return new state
{  int state=states[motor]&0x3;
	if (advance)
   {	if (state==0) state=2;
     	else if (state==2) state=3;
      else if (state==3) state=1;
      else state=0;}
   else
   {	if (state==0) state=1;
     	else if (state==1) state=3;
      else if (state==3) state=2;
      else state=0;}
   SetMotorBit(motor,1,state&1,DELT);
   SetMotorBit(motor,2,state&2,DELT);
   states[motor]=state;}

bool ReadStatesAndCycle()
{  FILE *file;
	int i;
	file=fopen("/var/www/p2/states.p2","rb");
   if (file!=NULL)
   {	for (i=0;i<12;i++)
      {	fread(&states[i],sizeof(int),1,file);
        	fread(&position[i],sizeof(int),1,file);}
      fread(&mcycle,sizeof(struct movementcycle),1,file);
      fclose(file);
      return true;}
   return false;}

bool WriteStatesAndCycle()
{  FILE *file;
	int i;
	file=fopen("/var/www/p2/states.p2","wb");
   if (file!=NULL)
   {	for (i=0;i<12;i++)
   	{	fwrite(&states[i],sizeof(int),1,file);
        	fwrite(&position[i],sizeof(int),1,file);}
      fwrite(&mcycle,sizeof(struct movementcycle),1,file);
      fclose(file);
      return true;}
   return false;}

int CalcCyclePosition(int n)
{	int p;
	p=mcycle.bias[n];
   p+=mcycle.currentcycle;
   if (p>749) p=-400;
   if (p<-400) p=-399;
   return p;}

void MoveMotorToAbsPosition(int n,int pos)
{	int i,k,m=LUT[n],loops,dir;
	bool hallstate,newstate;
	if (pos<-255) pos=-255;
   else if (pos>255) pos=255;
   loops=pos-position[n];
   if (loops==0) return;
   if (loops>0) dir=1;
   else
   {	loops=abs(loops);
   	dir=-1;}
   if (((n>2)&&(n<6))||(n>8)) dir=-dir;
   // now make sure that motor state=0 upon exit
   i=states[m];
   if (i!=0)
   {	if (dir>0)
   	{	if (i==1) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   	else
   	{	if (i==2) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   }	// not 0
   k=loops/4;
   k+=k;
   k+=k;
   loops=k+i;
   pos=position[n];
	MotorOn(m);
   hallstate=GetHallState(n);
   for (i=0;i<loops;i++)
   {  if (((n>2)&&(n<6))||(n>8)) pos-=dir;
   	else pos+=dir;
   	HStep(m,(dir>0));
      newstate=GetHallState(n);
      if (newstate!=hallstate)	// update position
      {  if (((n>2)&&(n<6))||(n>8)) k=-dir;
         else k=dir;
      	if (k>0)
        	{	if (!newstate) pos=magcalib[n][0];}
         else
         {	if (newstate) pos=magcalib[n][1];
            else pos=magcalib[n][2];}
         hallstate=newstate;}
   }
   mcycle.hallstates[n]=newstate;
   MotorOff(m);
   position[n]=pos;}

void Move2MotorsToAbsPositions(int n1,int pos1,int n2,int pos2)
{	int i,k,m1=LUT[n1],loops1,dir1,m2=LUT[n2],loops2,dir2;
	bool hallstate1,newstate1,hallstate2,newstate2;
	if (pos1<-255) pos1=-255;
   else if (pos1>255) pos1=255;
   loops1=pos1-position[n1];
   if (loops1>0) dir1=1;
   else
   {	loops1=abs(loops1);
   	dir1=-1;}
   if (((n1>2)&&(n1<6))||(n1>8)) dir1=-dir1;
   i=states[m1];
   if (i!=0)
   {	if (dir1>0)
   	{	if (i==1) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   	else
   	{	if (i==2) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   }	// not 0
   k=loops1/4;
   k+=k;
   k+=k;
   loops1=k+i;
   pos1=position[n1];
   if (pos2<-255) pos2=-255;
   else if (pos2>255) pos2=255;
   loops2=pos2-position[n2];
   if (loops2>0) dir2=1;
   else
   {	loops2=abs(loops2);
   	dir2=-1;}
   if (((n2>2)&&(n2<6))||(n2>8)) dir2=-dir2;
   if ((loops1==0)&&(loops2==0)) return;
   i=states[m2];
   if (i!=0)
   {	if (dir2>0)
   	{	if (i==1) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   	else
   	{	if (i==2) i=3;
   		else if (i==3) i=2;
      	else i=1;}
   }	// not 0
   k=loops2/4;
   k+=k;
   k+=k;
   loops2=k+i;
   pos2=position[n2];
	MotorOn(m1);
   MotorOn(m2);
   hallstate1=GetHallState(n1);
   hallstate2=GetHallState(n2);
   while ((loops1>0)||(loops2>0))
   {	// usleep(500);
      if (loops1>0)
   	{  if (((n1>2)&&(n1<6))||(n1>8)) pos1-=dir1;
      	else pos1+=dir1;
      	HStep(m1,(dir1>0));
      	newstate1=GetHallState(n1);
      	if (newstate1!=hallstate1)	// update position
      	{  if (((n1>2)&&(n1<6))||(n1>8)) k=-dir1;
         	else k=dir1;
      		if (k>0)
        		{	if (!newstate1) pos1=magcalib[n1][0];}
         	else
         	{	if (newstate1) pos1=magcalib[n1][1];
            	else pos1=magcalib[n1][2];}
         		hallstate1=newstate1;}
      	loops1--;}
      if (loops2>0)
   	{  if (((n2>2)&&(n2<6))||(n2>8)) pos2-=dir2;
      	else pos2+=dir2;
      	HStep(m2,(dir2>0));
      	newstate2=GetHallState(n2);
      	if (newstate2!=hallstate2)	// update position
      	{  if (((n2>2)&&(n2<6))||(n2>8)) k=-dir2;
         	else k=dir2;
      		if (k>0)
        		{	if (!newstate2) pos2=magcalib[n2][0];}
         	else
         	{	if (newstate2) pos2=magcalib[n2][1];
            	else pos2=magcalib[n2][2];}
         		hallstate2=newstate2;}
      	loops2--;}
   }
   MotorOff(m1);
   MotorOff(m2);
   mcycle.hallstates[n1]=newstate1;
   mcycle.hallstates[n2]=newstate2;
   position[n1]=pos1;
   position[n2]=pos2;}

void LiftPaw(int n,int d,bool update)
{	int m,dir,j;
	m=LUT[n+6];
   MotorOn(m);
   if (mcycle.shouldbedown) dir=1;
   else dir=-1;
   if (n>2) dir=-dir;
   for (j=0;j<d;j++) VStep(m,dir>0);
   MotorOff(m);
   if (update) mcycle.pawdown[n]=mcycle.shouldbedown;}

void Lift2Paws(int n,bool update)
{  int m1,m2,j;
	bool down=mcycle.shouldbedown;
   if (n>5) n-=6;
   if (n>3) n-=3;
	m1=LUT[n+6];
   m2=LUT[n+9];
   if (!down)
   {	j=m1;
   	m1=m2;
      m2=j;}
   MotorOn(m1);
   MotorOn(m2);
	SetMotorBit(m1,2,true,DELT);				// state m1=2
   SetMotorBit(m2,1,true,DELT);				// state m2=1
   SetMotorBit(m1,1,true,DELT);   			// state m1=3
  	SetMotorBit(m2,2,true,DELT);				// state m2=3
  	SetMotorBit(m1,2,false,DELT); 			// state m1=1
   SetMotorBit(m2,1,false,DELT);				// state m2=2
   MotorOff(m1);
   MotorOff(m2);
   states[m1]=states[m2]=0;
   if (update) mcycle.pawdown[n]=mcycle.pawdown[n+3]=down;}

int DoCycle()
{  int i,j,n,p1,p2,cs=mcycle.currentcycle;
	if (cs>1) return 664;
	for (i=0;i<6;i++)
   {	if (!mcycle.pawdown[i]) return 665;}
   if (mcycle.mymode==forward)
   {	for (cs=0;cs<750;cs+=10)	
		{  p1=mcycle.bias[0]+cs;
         if (p1>749)	// lift and reposition
         {	p1=-400;
         	for (i=0;i<3;i++)
         	{	mcycle.shouldbedown=false;
            	LiftPaw(i,LIFT/3,false);
               MoveMotorToAbsPosition(i,p1);
         		mcycle.shouldbedown=true;
               LiftPaw(i,LIFT/3,false);}	// next i;
         }
         p2=mcycle.bias[3]+cs;
         if (p2>749) // lift and reposition
         {	p2=-400;
         	for (i=3;i<6;i++)
         	{	mcycle.shouldbedown=false;
            	LiftPaw(i,LIFT/3,false);
               MoveMotorToAbsPosition(i,p2);
         		mcycle.shouldbedown=true;
               LiftPaw(i,LIFT/3,false);}	// next i;
         }
      	for (i=0,n=3;i<3;i++,n++) Move2MotorsToAbsPositions(n,p1,i,p2);} // next cs
   }	// if forward
	return 0;}

int main(int argc, char* argv [])
{  int i,j,n=0,m,d,loops=0,dir=1,pfunc=0,pos,error=0;
	bool newstate,hallstate,found;
   if (argc>1)
   {	pfunc=atoi(argv[1]);  // on or off
   	if (argc>2)
   	{	n=atoi(argv[2]);
   		if (n>11) n=11;
      	if (argc>3)
   		{	loops=atoi(argv[3]);
         	if (argc>4)
            {	if (atoi(argv[4])>0) write_sensors=true;
            }		// a4
         }     //a3
      }    //a2
	}  // a1
 	setup_io();
   if 	  (pfunc==-1) RelayOff();
   else if (pfunc==1)  RelayOn();
   else if (pfunc==0)						// step up down
   {	ReadStatesAndCycle();
      if (loops<0)
   	{	dir=-1;
      	loops=abs(loops);}
      if (((n>2)&&(n<6))||(n>8)) dir=-dir;
      m=LUT[n];
   	states[m]=0;
   	MotorOn(m);
   	for (i=0;i<loops;i++) VStep(m,dir>0);
   	MotorOff(m);
      mcycle.isvalid=false;
      WriteStatesAndCycle();}
   else if (pfunc==2)
   {  RelayOff();
   	m=system("sudo reboot &"); }
   else if (pfunc==3)	// initialise
   {	RelayOff();
   	setup_io();
		InitSteppers();	// initialize motors
      RelayOn();
      mcycle.isvalid=false;
      WriteStatesAndCycle();}
   else if (pfunc==4)	// initialise, find and move to 0 degrees
   {	RelayOff();
      setup_io();
		InitSteppers();	// initialize motors
      RelayOn();
      mcycle.mymode=forward;
      mcycle.currentcycle=0;
      mcycle.bias[0]=-399;
      mcycle.bias[1]=-399;
      mcycle.bias[2]=-399;
      mcycle.bias[3]=-25;
      mcycle.bias[4]=-25;
      mcycle.bias[5]=-25;
      usleep(100000);			// 0.1s pause
      for (n=5;n>=0;n--)
      {	m=LUT[n];
   		states[m]=0;
      	MotorOn(m);
      	found=false;
         if (n>2) dir=-1;
         else dir=1;
         loops=100;
         while ((!found)&&(loops<1000))
         {	newstate=hallstate=GetHallState(n);
            for (i=0;(i<loops)&&(hallstate==newstate);i++)
      		{	HStep(m,dir>0);
         		newstate=GetHallState(n);}
            if (newstate!=hallstate)
            {  d=dir;
            	if (newstate) // pos B of nbg
               {	if (n>2) d=-d;
               	if (d<0)
                  {	pos=magcalib[n][1];
                  	if (pos!=0)
                     {	if (pos>0) d=-1;
                     	else d=1;
                  		if (n>2) d=-d;
                  		pos=abs(pos);
                        for (i=0;i<pos;i++) HStep(m,d>0);}	// move to 0
                  	position[n]=0;
                     found=true;}
                  else
                  {	dir=-dir;
                  	loops>>=1;}
               }
               if (!newstate)	// pos A of pos C
               {	if (n>2) d=-d;
               	if (d>0) pos=magcalib[n][0];
                 	else pos=magcalib[n][2];
                  position[n]=pos;
                  mcycle.hallstates[n]=GetHallState(n);
         			mcycle.synced[n]=true;
         			pos=CalcCyclePosition(n);
         			MoveMotorToAbsPosition(n,pos);
                  found=true;}
            }
         	dir=-dir;
            loops+=loops;}
      	MotorOff(m);
         if (!found)
         {	error|=(1<<n);
         	position[n]=666;}
         mcycle.currentcycle=0;
      	MoveMotorToAbsPosition(n,0);} // next n
      for (i=0;i<3;i++)
      {	// Move2MotorsToAbsPositions(i,mcycle.bias[i],i+3,mcycle.bias[i+3]);
      	mcycle.pawdown[i]=false;}
      mcycle.isvalid=false;
      WriteStatesAndCycle();}
   else if (pfunc==5)	// move to abs position
   {  ReadStatesAndCycle();
      pos=loops*204.8/36.0;	// from degrees to steps
      MoveMotorToAbsPosition(n,pos);
      mcycle.isvalid=mcycle.synced[n]=false;
      WriteStatesAndCycle();}
   else if (pfunc==6)			// do a cycle
   {
       // under construction
   }    
   else if (pfunc==7)	// goto rest
   {	ReadStatesAndCycle();
   	for (i=0;i<LIFT;i++)
      {	for (n=0;n<3;n++)								
         {	mcycle.shouldbedown=false;
            Lift2Paws(n,i>LIFT-2);}	// next n
      }	// next i;
   	WriteStatesAndCycle();}
   else if (pfunc==8)	// lift poepie
   {	ReadStatesAndCycle();
   	for (i=0;i<LIFT;i++)
      {	for (n=0;n<3;n++)
         {	mcycle.shouldbedown=true;
            Lift2Paws(n,i>LIFT-2);}	// next n
      }	// next i;
   	WriteStatesAndCycle();}
   else if (pfunc==9)
   {  m=system("sudo amixer cset numid=1 -- 100% &");
   	m=system("sudo mpg123 /var/www/p2/miauw.mp3 &");}
   else if (pfunc==10)
   {	ReadStatesAndCycle();
      MotorOn(LUT[n]);}
   else if (pfunc==11)
   {	MotorOff(LUT[n]);}
   if (write_sensors)
   {	for (i=0;i<6;i++) GetHallState(i);
   	file=fopen("/var/www/p2/hall.txt","wt");
   	if (file!=NULL)
   	{  strcpy(text,"[10 20 30 40 50 60] ");
         for (i=0,n=2;i<6;i++,n+=3)
         {	if (sensorstates&1) text[n]='1';
         	sensorstates>>=1;}
   		fwrite(text,strlen(text),1,file);
   		fclose(file);}
   }
	return 0;}