#include <16F676.h>
#case
/*----------------------------------------------------------------------------*/

#use delay( clock = 4000000 )
#fuses NOWDT, PROTECT, INTRC_IO, NOMCLR, NOBROWNOUT, CPD

typedef unsigned int uint8;
typedef short bool;
typedef unsigned long uint16;
#define false 0
#define true  1

#define EYE             PIN_A0  // A/D input
#define CURRENT_SENSE   PIN_A1  // A/D input
#define IN1             PIN_A2  // output
// A3
#define L2              PIN_A4  // output
#define L3              PIN_A5  // output
#define SWITCH          PIN_A5  // output
#use fast_io( A )

#define H_DISABLE       PIN_C0  // output
#define BATT_LEVEL      PIN_C1  // input
#define H_SLEEP         PIN_C2  // output
#define EYE_ENABLE      PIN_C3  // output
#define L1			    PIN_C4  // output
#define IN2             PIN_C5  // output
#use fast_io( C )

uint16 gear;
uint16 currentLevel;
bool currentSet;
bool motorOn;
int sleepCounter;
bool motorForward;

bool eyeTriggered;

bool motionSeen;
bool tubeEmpty;
int eyeLevel;
int oldEyeLevel;

#define MOTOR_START_HOLDOFF 3000
uint16 motorStartHoldoff;

int avgCurrent;
int current1;
int current2;
int current3;
int current4;
int current_count;

bool curLow;
uint16 curHigh;
uint16 curMedium;
uint16 countdown;

// .0385v per tick
// 180 = 7v
// 206 = 8v
// 231 = 9v
int battery_level;

//------------------------------------------------------------------------------
int state;
enum
{
	ceStateIdle,    // balls loaded, waiting for movement
	ceStateHunting, // no eye, low torque, waiting to be loaded
	ceStateWinding, // waiting for spring to wind and motion to stop
	ceStateUnJam1,  // unjam stage 1 (reverse full speed)
	ceStateUnJam2,  // unjam stage 2 (reverse in low gear)

	ceLastState
};


//------------------------------------------------------------------------------
#pragma SEPARATE
void downshift()
{
	// 175 = 7v
	// 187 = 7.5v
	// 200 = 8v
	// 212 = 8.5v
	// 226 = 9v
	// .0196/v
//	output_low( L1 );
//	output_low( L2 );
//	output_low( L3 );
			
	set_adc_channel( 5 );

	if ( battery_level < 175 ) // 7v
	{
		gear = 900; // LOW BATT!
	}
	else if ( battery_level < 181 ) // 7.25
	{
		gear = 1000;
	}
	else if ( battery_level < 194 ) // 7.75
	{
		gear = 1100;
	}
	else if ( battery_level < 206 ) // 8.25
	{
		gear = 1200;
	}
	else if ( battery_level < 218 ) // 8.75
	{
		gear = 1300;
	}
	else // 9.25
	{
		gear = 1400;
	}
	
	battery_level = read_adc();
}

//------------------------------------------------------------------------------
#pragma SEPARATE
void stateMachine()
{
	// figure this is entered every 60us (16,666Hz)

	// set channel to eye-sense before doing stuff, to ensure ADC setup
	// time
	set_adc_channel( 0 );

	// ----- Handle motor current samples
	if ( motorStartHoldoff )
	{
		// holdoff and wait for motor to start, it consumes a bunch of
		// power until then
		motorStartHoldoff--;
		current1 = 0;
		current2 = 0;
		current3 = 0;
		current4 = 0;
		avgCurrent = 0;
		curHigh = 0;
		curMedium = 0;
	}
	else if ( currentSet )
	{
		if ( !current_count )
		{
			current_count++;
			current1 = currentLevel;
		}
		else if ( current_count == 1 )
		{
			current_count++;
			current2 = currentLevel;
		}
		else if ( current_count == 2 )
		{
			current_count++;
			current3 = currentLevel;
		}
		else
		{
			current4 = currentLevel;
			current_count = 0;
		}

		avgCurrent = (int)(((uint16)current1 + (uint16)current2 + (uint16)current3 + (uint16)current4) / (uint16)4);
		currentSet = false;
	}

	// ----- process Eye logic
/*
	empty - .140

	DARK-
	peek  1.14
	trough  .147

	NORMAL-
	peek  3.11
	trough  1.58

	.0196

*/

	eyeLevel = read_adc();
	if ( eyeLevel > oldEyeLevel + 1
		 || eyeLevel < oldEyeLevel - 1 )
	{
		motionSeen = true;
		set_timer0(0);
		eyeTriggered = false; // ISR sets this true at a 13.3bps rate
	}
	else if ( eyeTriggered )
	{
		motionSeen = false;
	}

	if ( !motionSeen && eyeLevel < 20 )
	{
		tubeEmpty = true;
	}
	else
	{
		tubeEmpty = false;
	}

	// ------ turn the crank on state machine
	
	switch ( state )
	{
		case ceStateIdle:    // balls loaded: waiting for movement
			break;
			
		case ceStateHunting: // no eye: low torque: waiting to be loaded
			downshift();
			if ( tubeEmpty && curHigh ) // process unjam
			{
				state = ceStateUnJam1;
				motorForward = false;
				gear = 0;
				countdown = 10000;
			}
			else if ( motionSeen )
			{
				countdown = 5000;
				state = ceStateWinding;
			}
			break;

		case ceStateWinding:
			if ( countdown )
			{
				countdown--;
			}
			else if ( !motionSeen )
			{
				if ( tubeEmpty )
				{
					state = ceStateHunting;
				}
				else
				{
					state = ceStateIdle;
					motorOn = false;
				}
			}
			else if ( motionSeen ) // guess we're feeding!
			{
				if ( !curMedium )
				{
					gear -= 200; // faster!
				}
				
				countdown = 4000;
			}
			
			break;
			
		case ceStateUnJam1:
			if ( !--countdown )
			{
				downshift();
				state = ceStateUnJam2;
				motorStartHoldoff = MOTOR_START_HOLDOFF;
				countdown = 10000;
			}
			break;

		case ceStateUnJam2:
			if ( curHigh && !--countdown )
			{
				motorForward = true;
				motorStartHoldoff = MOTOR_START_HOLDOFF;
				state = ceStateHunting;
			}
			break;
		

		default:
			state = ceStateHunting;
			break;
	}

	if ( avgCurrent < 73 ) // free-wheeling (upshift)
	{
		curLow = true;

		output_high( L1 );
		output_low( L2 );
		output_low( L3 );
	}
	else if ( avgCurrent < 82 ) // normal torque (right gear)
	{
		curMedium = 16000;

		output_low( L1 );
		output_high( L2 );
		output_low( L3 );
	}
	else // stalled, shift to gear 1
	{
		curHigh = 16000;

		output_low( L1 );
		output_low( L2 );
		output_high( L3 );
	}

	if ( curHigh )
	{
		output_high( L3 );
		curHigh--;
	}
	else
	{
		output_low( L3 );
	}

	if ( curMedium )
	{
		output_high( L2 );
		curMedium--;
	}
	else
	{
		output_low( L2 );
	}
}

//------------------------------------------------------------------------------
void main()
{
	output_low( EYE_ENABLE ); // eye off
	output_high( H_DISABLE ); // sleep the H-bridge
	output_low( H_SLEEP );
	output_high( IN1 );
	output_high( IN2 );

	set_tris_c( 0x02 );     // OO|OOIO

powerCycle:

	set_tris_a( 0x03 );     // IO|OOII

	output_low( EYE_ENABLE ); // eye off
	output_high( H_DISABLE ); // sleep the H-bridge
	output_low( H_SLEEP );
	output_high( IN1 );
	output_high( IN2 );
	output_low( L1 );
	output_low( L2 );
	output_low( L3 );

/*
	enable_interrupts( INT_RA );
	ext_int_edge( H_TO_L );
	enable_interrupts( GLOBAL );

	do
	{
		sleep();

		delay_ms( 50 );

		for( sleepCounter = 0;
			 sleepCounter < 200 && !input(SWITCH);
			 sleepCounter++ )
		{
			output_high ( L1 );
			delay_ms(2);
		}

		output_low ( L1 );

	} while ( input(SWITCH) );
	
	disable_interrupts( GLOBAL );
*/

	set_tris_a( 0x03 );     // OO|OOII
	port_a_pullups( 0x00 );

	setup_adc( ADC_CLOCK_DIV_8 ); // 2uS conversion time
	setup_port_a( sAN0 | sAN1 | sAN5 | VSS_VDD );

	setup_timer_1( T1_INTERNAL );

	setup_counters( RTCC_INTERNAL, RTCC_DIV_256 );
	enable_interrupts( INT_RTCC );
	enable_interrupts( GLOBAL );

	output_high( EYE_ENABLE );
	output_high( IN1 );
	output_low( IN2 );
	output_high( H_SLEEP );
			   
	currentSet = false;
	motorOn = true;
	motorForward = true;
	state = ceStateHunting;
	
	motorStartHoldoff = MOTOR_START_HOLDOFF;

	for(;;)
	{
		if ( motorOn )
		{
			if ( motorForward )
			{
				output_high( IN1 );
				output_low( IN2 );
			}
			else
			{
				output_low( IN1 );
				output_high( IN2 );
			}

			set_timer1(0);
			output_low( H_DISABLE );
			while( get_timer1() < 400 ) // "on" pulse constant
			{
				stateMachine();
			}

			set_adc_channel( 1 );
			currentSet = true;
			delay_us( 2 ); // time for the capacitor to charge (probobly unnecessary)
			
			currentLevel = read_adc();
		}

		output_high( H_DISABLE );
				
		set_timer1(0);
		while( get_timer1() < gear )
		{
			stateMachine();
		}
	}
}

//------------------------------------------------------------------------------
#int_rtcc
// timer0 is .29412 milliseconds per tick
void rtcc()
{
	eyeTriggered = true;
	return;
}