/*
 * ping.h
 *
 * Created: 8/4/2012 2:15:38 PM
 *  Author: Peter Cole
 */ 

#include <pololu/orangutan>
#include <pololu/OrangutanDigital/OrangutanDigital.h>
#include <pololu/OrangutanMotors/OrangutanMotors.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#if Serial==1
#include <pololu/OrangutanSerial/OrangutanSerial.h>
#endif
#include <stdlib.h>
#include <limits.h>


#include <pololu/OrangutanTime/OrangutanTime.h>

#ifndef PING_H_
#define PING_H_


/******option*******/
#define Serial     0
#define PingMode1  1
#define pingMode2  1

/***********************/

/****constants*********/
const int bufferSize=32;
/*********************/
	


//#define Debug2    1
	
	
	unsigned long PulseHigh;
	

	//int returnpulse;
	/*// alternatively, keeping unsigned long cm and inches:
> // the multiplier method gives you 2 virtual decimal points of accuracy;
> // e.g. cm == 250 means 2.50 centimeters
> // Remember this whenever you interpret the number!
> cm = 100*static_cast<unsigned long>(pulse_width/(29./2.));*/
	
	//unsigned char numChannel;
	//unsigned char pingsensor;
	
	
class Ping{
	protected:
	long inches;
	long cm;
	char SendBuffer[bufferSize];
	char ReceiveBufffer[bufferSize];
	unsigned char receivePos;
	
	
	
	public:
	
	 Ping(){ // !!!!
		 //inches=cm=0;
		 
		 // !!!!
		 // Start scanning from object creation
		 //OrangutanPulseIn::start((unsigned char[]) {idx},1);
	 }
	
	
	~Ping(){
		
			// !!!!
			// STOP scanning for pulse interrupts
			// (necessary to free memory and resources)
			// (This complements OrangutanPulseIn::start)
			//OrangutanPulseIn::stop();	
			
				
		
		}
		
		unsigned long pulse_width;
		unsigned char state;
		
	//	unsigned char PingPulse(unsigned char,unsigned char );
		
//};
//type pin 

		void PingPulseOut(unsigned char pin){
		 
			//OrangutanPulseIn::start((unsigned char[]){},);
				 
	
		 
	
			//OrangutanDigital::setOutput(IO_B1,HIGH);
			//delayMicroseconds(5);
			OrangutanDigital::setOutput(pin,LOW);
			
			//delay_us(100); // !!!!
			OrangutanTime::delayMicroseconds(2);//2 Microseconds 
			OrangutanDigital::setOutput(pin,HIGH);
			//delay_us(2000); // !!!!
			OrangutanTime::delayMicroseconds(5);//5Microseconds 
			OrangutanDigital::setOutput(pin,LOW);
		
			//delayMicroseconds(2);//2 microseconds
			delay_ms(150);//100ms
			//returnpulse=1;
			//return returnpulse=1;
			
		}		
		
						
	
		#if Serial==1
		void serialInfo(){
			OrangutanSerial::setBaudRate(9600);
			OrangutanSerial::receiveRing(ReceiveBufffer,sizeof(ReceiveBufffer));
				
		}
		void wait_to_sending_to_finish(){
			OrangutanSerial::sendBufferEmpty();
				
		}
		#endif
		/******************************************************************/
		void PingPulseIn1(unsigned char pin2,unsigned char channel){
		
		struct PulseInputStruct pulseInfo;
	
	
			//unsigned long LastHighPulseMicroseconds=0;
			
						
			//bool pulseInStart=false;
			
			
		
			OrangutanDigital::setInput(pin2,HIGH_IMPEDANCE);
			#if Serial==1
			OrangutanSerial::receiveRing(ReceiveBufffer,sizeof(ReceiveBufffer));
			//OrangutanPulseIn::stop();
			//delay(10);
			memcpy_P(SendBuffer,"inches",sizeof(SendBuffer));
			OrangutanSerial::sendBlocking(SendBuffer,strlen("inches"));
			
			#endif
			 
			//OrangutanDigital::setInput(pin2,HIGH_IMPEDANCE);
						
			 		
			#if pingMode2==1
			//pulse_width=get_ticks()-pulseInfo.lastPCTime;
			pulse_width=get_ticks();
			state=pulseInfo.inputState;
			
				
			OrangutanPulseIn::start((unsigned char[]){pin2},1);
			
			//pulse_width= OrangutanPulseIn::getLastHighPulse(channel); // !!!!
			//unsigned long lastLowPulseMicroseconds=OrangutanPulseIn::toMicroseconds(OrangutanPulseIn::getLastLowPulse(0));
			unsigned long LastHighPulseMicroseconds=OrangutanPulseIn::toMicroseconds(pulse_width);
			#if PingMode1==1
			#if Serial==1
			if (LastHighPulseMicroseconds==0)
			 {
				 memcpy_P(SendBuffer,"inches=0",12);
			 }
			#endif
			OrangutanPulseIn::getCurrentState(3,&pulse_width,&state);
		//unsigned long elapsed=-lastLowPulseMicroseconds+LastHighPulseMicroseconds;
				
			
						#endif			
	
			if (LastHighPulseMicroseconds>= 115) // !!!!
			
			{
				if(LastHighPulseMicroseconds > 10000){ // !!!!
				//OrangutanDigital::setOutput(IO_D1,LOW);
				//delay_ms(200);
				OrangutanDigital::setOutput(IO_D1,HIGH);
				
				//OrangutanDigital::setOutput(IO_D1,LOW);
				}
				// if more than 100 ms have passed since last pulse ended
			
			
				// and the pulse input channel is currently high
				///http://pomprocker.blogspot.com/2009/01/adding-parallax-ping-to-avr-using-c.html
				//this where i found the information to calculate the distance.
				//cm=pulse_width/(14.5);//previous code
			
		    	cm=LastHighPulseMicroseconds/(14.5);//previous code
				// cm=(elapsed*16154.473755779)+1.14223552;//if this calculate right.
				//it should only be -or+ .00000001 off.
				inches=cm/2.54;
				if (cm>300)
				{
					OrangutanDigital::setOutput(IO_D1,LOW);
					delay(5);
				}
				// inches = 100*static_cast<unsigned long>(pulse_width/37.);
				//delay_ms(5000);
				
			
				//delay_us(100);
				//returnpulse=0;
						
						
			
				//pulseInStart=false;
				//OrangutanDigital::setOutput(IO_D1,LOW);
				//delay(1000);
				//return returnpulse=0;
				#endif
				
			}				
					
	
				//return 1;
		}	
		void PingPulseIn2(unsigned char pingPin,unsigned char channelB){
			bool pulseInStart=false;
		
			if (pulseInStart==false)
			{
				delayMicroseconds(100);
				OrangutanDigital::setOutput(IO_D1,HIGH);
				pulseInStart=true;
			}
			if (pulseInStart==true)
			{
				delay(1000);
				OrangutanDigital::setOutput(IO_D1,LOW);
				OrangutanPulseIn::start((unsigned char[]){pingPin},1);
				while(!OrangutanPulseIn::newHighPulse(channelB));
				PulseHigh=OrangutanPulseIn::getLastHighPulse(channelB);
				delay(300);
				pulseInStart=false;
			
				
			}
			if (pulseInStart==false)
			{
			
				OrangutanPulseIn::stop();
			
			
			}
		}			
				
	
	};
	
	class Followerbot: protected Ping{
		
		
		
		public:
		
		Followerbot(){}
		~Followerbot(){}
			int motorspeed;
			
		//num1 must be greater than num2	
		void Movement (   int motorspeed,int num1,int num2){
			#if Serial==1
			if (inches==0)
			{
				memcpy_P(SendBuffer,"Error",sizeof(SendBuffer));
			}
			#endif
			if (inches<=num1&&inches>=num2)
			{
				OrangutanMotors::setSpeeds(motorspeed,motorspeed);
			}
			if (motorspeed>255)
			{
				motorspeed=255;
				
			}
						
		}
		//num1 must be greater than num2
		void slowdown(int slow,int num1,int num2){
			if (inches<=num1&&inches>=num2)
			{
				OrangutanMotors::setSpeeds(slow,slow);
			}
		}
		//num1 must be greater than num2
	void stopBOT(int num1,int num2){
			if (inches<=num1&&inches>=num2)
			{
				OrangutanMotors::setSpeeds(0,0);
				delay_ms(100);
				OrangutanMotors::setSpeeds(-50,50);
				delay(50);
				if (!inches<num2)
				{
					OrangutanMotors::setSpeeds(motorspeed,motorspeed);
				}
				
				
			}
			
			
		}
		void scanSurrounding(){
			int Position =0;
			for (int i=0;i<100;i++)
			{
				double Check1Inches=inches;
				delay_ms(19);
				
				double Check2Inches=inches;
				if (Check1Inches==Check2Inches&&Position==0)
				{
					OrangutanMotors::setSpeeds(50,-50);
					delay_ms(1000);		//this delay may need adjustments
					OrangutanMotors::setSpeeds(0,0);
					Position=1;
					delay_ms(3000);
				}
				if (Check1Inches==Check2Inches&&Position==1)
				{
					OrangutanMotors::setSpeeds(-50,50);
					delay_ms(1000);
					OrangutanMotors::setSpeeds(0,0);
					Position=2;
					delay_ms(3000);
				}
				if (Check1Inches==Check2Inches&&Position==2)
				{
					OrangutanMotors::setSpeeds(-50,50);
					delay_ms(1000);
					OrangutanMotors::setSpeeds(0,0);
					Position=0;
					delay_ms(3000);
				}
				
			}
			
			

		}
					
			
			void checkMotors(){
				OrangutanMotors::setSpeeds(255,255);
			}
		
		
		
	};


#endif /* PING_H_ */