#include "parser.h"
#include "debugport.h"
#include "dwarfcore.h"
#include "trapservo.h"
#include "heater.h"
#include "rvalve.h"
#include "cpld.h"
#include "espdwarf.h"

static char endCmd=0;
extern parserInstance debugPort;
//convert output byte to ascii
void convwritebyte(unsigned char tosend)
{
unsigned char temp;
temp = tosend >> 4;
tosend &= 0x0f;

	if (temp >9 ) temp += ('A'-10);
	else temp += '0';
	parserPutByte(&debugPort,temp);
	
	
	if (tosend >9 ) tosend += ('A'-10);
	else tosend += '0';
	parserPutByte(&debugPort,tosend);

}
//convert ascii hex to dec
unsigned char convreadbyte(unsigned char temp,char endCmd)
{
	if(temp==27){endCmd=1; return temp;}	 
	if(temp < 64)temp-= 48;
  	else if((temp>64)&(temp<=96))temp-=55;
  	else temp-=87;
                       
    return temp;

}
void sendSerialData(int32 variable)
{
unsigned char *pos_bytes;
	
	pos_bytes = (unsigned char *)&variable;
	convwritebyte(pos_bytes[3]);
 	convwritebyte(pos_bytes[2]);
 	convwritebyte(pos_bytes[1]);
 	convwritebyte(pos_bytes[0]);

}
void sendSerialByte(byte tosend)
{
	parserPutByte(&debugPort,tosend);
}

// Servo test command parser
void parseTestCmd(MotorChannel* mc,RotaryValve* rv,Heater* h,char* debugFlag)
{
#define putString(s)  parserPutString(&debugPort,(s))
#define getByte()     parserGetByte(&debugPort)
#define putByte(byte) parserPutByte(&debugPort,(byte))

 char temp;
 unsigned char comm_bytes[4]={0,0,0,0};
 unsigned long int pos_holder;
 int i;
 unsigned char intreg;
 char valveDir=0;
 int16 newSetPoint=0;
 char tempdebugFlag;
 tempdebugFlag=*debugFlag;

  temp=getByte();putByte(temp);
	  switch (temp)
	  {
	case 'H'://turn on power to home sensor
		set_cpld_bit(DCORENCPWR,DCOR0ENC5);
 		writeDWRF(DWRFIFG,0x00);
		break;
	case 'h'://turn off power to home sensor
		clear_cpld_bit(DCORENCPWR,DCOR0HOME5);
		break;
	case 'r'://read
	_DINT();_NOP();
		intreg=readDWRF(DWRFIFG);
		convwritebyte(intreg);
		_EINT();break;
	case 'l':
		_DINT();_NOP();
		intreg=readDWRF(DCORLIMITS);
		convwritebyte(intreg);_EINT();break;
	case 'Z'://enable homing
		set_cpld_bit(DCORENCCTL,DCOR0HMZERO);
		break;
	case 'z'://disable homing
		clear_cpld_bit(DCORENCCTL,DCOR0HMZERO);
		break;
	case 'b':
		putByte(0x0a);
		putByte(0x0d);
		mc->relative=0x00010000;
		reportErr("Servo goalPosition at %08lx", mc->goalPosition, 0, 0);
		reportErr("Servo lastPosition at %08lx", mc->lastIntermediate, 0, 0);
		reportErr("Servo intermediate at %08lx", mc->intermediate, 0, 0);
		reportErr("Servo relative at %08lx", mc->relative, 0, 0);
 		//startmove(mc->relative,mc);
		reportErr("Servo goalPosition at %08lx", mc[0].goalPosition, 0, 0);
		reportErr("Servo lastPosition at %08lx", mc->lastIntermediate, 0, 0);
		reportErr("Servo intermediate at %08lx", mc->intermediate, 0, 0);
		reportErr("Servo relative at %08lx", mc->relative, 0, 0);
		mc->tag = 0x02;
		mc->source = 0x10;
		break;
	case 'B':
		mc->relative=-0x00010000;
		//putByte(0x0a);
		//putByte(0x0d);
		break;
	 case 'j':	//JOG +dir
		for(i=0;i<4;i++)
		{
		temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp);
	   	}
	  if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	   	pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		
		mc->relative=pos_holder;
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'J':	//JOG -dir
		for(i=0;i<4;i++)
		{
		temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp);
	   	}
	if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	   pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		mc->relative=0;
		mc->relative=pos_holder;
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'g':	//Go to absolute pos
		for(i=0;i<4;i++)
		{
	    temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp); 
	   	}
		if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	  	pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		mc->relative=0;
		mc->relative=pos_holder-mc->position;
		     }
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'M'://Turn motor on
	    set_cpld_bit(DCOROUT,DCORMOTORPWR);
		_NOP();
		break;
	case 'm'://Turn motor off
		clear_cpld_bit(DCOROUT,DCORMOTORPWR);
		mc->relative=0;
	_NOP();
		break;	
	case 'P'://Spit out one position record
		putByte(0x0a);
		putByte(0x0d);
		sendSerialData((int32)mc->position);
		sendSerialData((int32)mc->latchPosition);
		 reportErr ("pos %x\n", mc->position);
		 reportErr("latch %x\n",mc->latchPosition);
		break;
    case 'D'://stream out position records
    	set8(tempdebugFlag,debugEnc);
		break;
    case 'd'://stop streaming position records
    	clear8(tempdebugFlag,debugEnc);
		break;
	case 'i'://spit out current to motor 0
	/*	convwritebyte((unsigned char)((mc[1].motorCurrent>>8)&0x000000ff));
 		convwritebyte((unsigned char)(mc[1].motorCurrent&0x000000ff));
 		putByte(0x0a);
		putByte(0x0d);*/
		reportErr("Servo intermediate at %08lx", mc[1].motorCurrent, 0, 0);
		break;
	case 'I'://spit out temp sensor measurement
		convwritebyte((unsigned char)((h->currentTemperature>>8)&0x000000ff));
 		convwritebyte((unsigned char)(h->currentTemperature&0x000000ff));
 		putByte(0x0a);
		putByte(0x0d);
		//sendMotorPos(&mc1,6);
		break;
	case 'f':
		sendSerialData(h->sum);
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 't'://command a set heater PWM
		for(i=0;i<2;i++)
		{
	    temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp); 
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {
	  	
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'T'://command a set temperature for closed loop heater control
		for(i=0;i<3;i++)
		{
	    temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp); 
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {
	  	newSetPoint=0;
		newSetPoint+=comm_bytes[2];
		newSetPoint|=(comm_bytes[1]<<4);
		newSetPoint|=(comm_bytes[0]<<8);
		if(newSetPoint>4095)newSetPoint=4095;
		h->tProgram.targetT=newSetPoint;
		calculateSlope(h);
		h->sum=0;
		h->state=heaterOn;
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'S':
		for(i=0;i<1;i++)
		{
	    temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp); 
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {	
		}
		putByte(0x0a);
		putByte(0x0d);
		//set_cpld_bit(DCORSENSOR,DCORTEMPEN);
		break;
	case 's':
	//	clear_cpld_bit(DCORSENSOR,DCORTEMPEN);
		break;	
	case 'w'://scroll out heater PWM values- toggles on and off
		if(tempdebugFlag&debugHeatPWM)
		{
		clear8(tempdebugFlag,debugHeatPWM);
		}
		else
		{set8(tempdebugFlag,debugHeatPWM);}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'p':
	    if(tempdebugFlag&debugPressure)
		{
		clear8(tempdebugFlag,debugPressure);
		}
		else
		{set8(tempdebugFlag,debugPressure);}
		break;
	case 'W'://scroll out heater PWM values- toggles on and off
		if(tempdebugFlag&debugTemp)
		{
		clear8(tempdebugFlag,debugTemp);
		}
		else
		{set8(tempdebugFlag,debugTemp);}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'v':
		for(i=0;i<4;i++)
		{
	    temp=getByte();
	   	comm_bytes[i]=convreadbyte(temp,endCmd);
	   	putByte(temp); 
	   	}
		if(endCmd==1){endCmd=0;break;}
	    else{
	  	pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		valveDir=comm_bytes[0];
		if(valveDir>1)valveDir=1;
		if(valveDir<0)valveDir=0;
		if(pos_holder>512)pos_holder=512;
		//set8(tempdebugFlag,DBUGVALVEFLAG);
	   	set_cpld_io(DRVCTL,get_cpld_io(DRVCTL)|(MTRPWR+ENCPWR0));
 //       startShaftMove(pos_holder,valveDir,rv);
        }
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'V':
		if(tempdebugFlag&debugValve)
		{
		clear8(tempdebugFlag,debugValve);
    	}
		else
		{
		set8(tempdebugFlag,debugValve);
	    }
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'e':
        //clear_cpld_bit(DRVMTRCTL,(M01+M00));
        break;
		
	case '?':
		break;
	default: 
		putByte(temp);
		putString("invalid input\n");
		break;
	  }
	  *debugFlag=tempdebugFlag;
}