/****************************************************************************/
/* Copyright 2005 MBARI.                                                    */
/* Monterey Bay Aquarium Research Institute Proprietary Information.        */
/* All rights reserved.                                                     */
/****************************************************************************/
#include <16f876A.h>

#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#use delay(clock=4000000, restart_wdt)
#include "Gulper_Main.h"
#include "RS485.c"
#include "Command_Handler.c"


#if DEBUG
#device ICD=TRUE
#fuses XT,WDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP
#else
#fuses XT,WDT,NOPROTECT,PUT,NOLVP
#endif

//#define  RS485_RX_BUFFER_SIZE 64
#define  RS485_USE_EXT_INT    FALSE


int8 in_char = 0;
int8 next_in = 0;
int8 next_out = 0;
int8 *command;
int8 data[1];
Boolean uart_TX = false;       // Used for BIT uart checking 

#define INTS_PER_SECOND 15     // (20000000/(4*256*256))
#define T2_INTS_PER_SECOND 98  // 1/( (1/4000000) * 4 * 4 * 255 * 10   )


#INT_TIMER1                        // This function is called every time
void clock_isr() {                 // timer 1 overflows (65535->0), which is
                                   // approximately 15 times per second for
    if(--int_count==0) {           // this program.
      ++seconds;
      int_count = INTS_PER_SECOND;
    }
}

#INT_TIMER2                        // This function is called every time
void timer_2_isr() {               // timer 2 overflows, which is
                                   // approximately 98 times per second
    if(--T2_int_count==0) {           
      ++T2_seconds;
      T2_int_count = T2_INTS_PER_SECOND;
    }
}

// Purpose: Clears elapsed time to "start" the clock
void startTimer(void)
{
  disable_interrupts(INT_TIMER1);
 	seconds=0;
 	int_count=INTS_PER_SECOND;
 	enable_interrupts(INT_TIMER1);				
}

// Purpose: Stores elapsed time
void getElapsedTime(void)
{
	disable_interrupts(INT_TIMER1);
 	ElapsedSeconds=seconds;
 	Elapsed_int_count=INTS_PER_SECOND-int_count; // Number of interrupts since the last second, there are 15 per sec
 	enable_interrupts(INT_TIMER1);
}

// Purpose: Clears elapsed time to "start" the clock for Timer 2
void T2_startTimer(void)
{
  disable_interrupts(INT_TIMER2);
 	T2_seconds=0;
 	T2_int_count=T2_INTS_PER_SECOND;
 	enable_interrupts(INT_TIMER2);				
}

// Purpose: Stores elapsed time from Timer 2
void T2_getElapsedTime(void)
{
	disable_interrupts(INT_TIMER2);
 	T2_ElapsedSeconds=T2_seconds;
 	T2_Elapsed_int_count=T2_INTS_PER_SECOND-T2_int_count; // Number of interrupts since the last second
 	enable_interrupts(INT_TIMER2);
}


// Purpose: Interrupt service routine for handling incoming proximity sensor. 
#INT_EXT
void proximity_isr() {
  //Only service proximity sensor after firing has been commanded
  //and only if proximity has not expired
  if((Fired) && (!proximity) && (!proximity_expired)) {
    getElapsedTime();
    output_bit(PIN_B4, HI); // Turn on proximity LED
    proximity = true;
  }
  //Otherwise, check for a misfire assuming that prox timeout has not expired
  else if ( (!Fired) && (!prox_error) && (!proximity_expired) ) {
  	prox_error = true;
  }
  	
}

// Purpose: Holds software in infinite loop to allow watchdog to reset
void software_reset() {
  for(;;) {
    null;
  }
}

// Purpose: The built in test executive procedure
void BIT_Exec() {
	// Check for a UART held in TX mode for more than 2 sec
  if( (input(PIN_B5)) && (!uart_TX) ) {
    T2_startTimer();
    uart_TX = true;
  }
  else if( (input(PIN_B5)) && (uart_TX) ) {
  	T2_getElapsedTime();
  	if(T2_ElapsedSeconds >= 2) {
  	  software_reset(); // Call reset and wait for watchdog timer to expire	
  	} 	
  }
  else {
    uart_TX	= false;
  }  	  
}


//
// Main execution loop
//
main()
{
  
  // Change edge definition for prox sensor interrupt
  EXT_INT_EDGE(H_TO_L);  
  
  // Set up the system clocks
  int_count=INTS_PER_SECOND;
  T2_int_count=T2_INTS_PER_SECOND;
  setup_wdt(WDT_288MS); // Initialize the watchdog. See fuses WDT also
  
  //Using timer 1 for proximity timing so as not to interfere with WDT/RTC/Timer0
  setup_timer_1(T1_INTERNAL | T1_DIV_BY_1); 

  //Using timer 2 for BIT and actuation voltage timing
  setup_timer_2(T2_DIV_BY_4, 0xFF, 10);   

  // Initialize Serial and enable interrupts
  rs485_init();
  enable_interrupts(INT_EXT);
  enable_interrupts(INT_TIMER1);
  enable_interrupts(INT_TIMER2);

  // Initialize Outputs
  output_bit(PIN_C4, HI); 
	output_bit(PIN_C5, LO);
  output_bit(PIN_B1, LO);
  output_bit(PIN_B2, LO);
  output_bit(PIN_B4, LO);
  
  // Set up analog input for AN0
  setup_adc(ADC_CLOCK_INTERNAL);
  setup_adc_ports(AN0);
  set_adc_channel(0);

  // Check for WDT reset and output error if necessary
  switch ( restart_cause() )
  {
    case WDT_TIMEOUT:
    {
      sprintf(response_message, "WDT\r\n");
      send_error_response(response_message);
      break;
    }
    case NORMAL_POWER_UP:
    {
      break;
    }
  }

 	// Send ID info via "blank" message
 	// send_response fills in the address
 	// For DEBUG only
 	//
  //## sprintf(response_message, "\r\n");  
  //## send_response(response_message);
          

/* Begin Loop */        
    for(;;)
		{ 
		  // strobe the dog
		  restart_wdt(); 
		
 	  	if(rs485_get_new_message(RS485_ID, FALSE)) // Check for new command
		 	{
		 		// Check to see which command was received...
        if(!strcmp(cmd_InBuffer,fire)) {
          handle_fire_command();
        }
        else if(!strcmp(cmd_InBuffer,query))  {
          handle_query_command();
        }
        else if(!strcmp(cmd_InBuffer,status))  {
          handle_status_command();
        }
        else if(!strcmp(cmd_InBuffer,version))  {
          handle_version_command();
        }
        else if(!strcmp(cmd_InBuffer,actuation))  {
          handle_actuation_voltage_command();
        }
        else if(!strcmp(cmd_InBuffer,reset))  {
          software_reset();
        }
        
        //...Or if the command is invalid
        else { 
          sprintf(response_message, "COMMAND ERROR\r\n");
          send_error_response(response_message);
        }
			}	// Check for new command

    
    // If > 30 secs have elapsed and proximity ISR has not been called,
    // report 0 for time elapsed
    if( (Fired) && (!proximity) && (!proximity_expired) ) {
      getElapsedTime();
      if(ElapsedSeconds >= 30) {
        proximity_expired = true;
      }  
    }

    // Check actuation voltage for >= 2 volts.
    if( (Fired) && (!actuation_bool) && (!proximity_expired) ) {
      float adc_value_loc;
      adc_value_loc = Read_ADC();
    	adc_value_loc = adc_value_loc / 256 * 5.00; // scale for 0-5 Volts

    	if (adc_value_loc >= 2)
       {
        T2_getElapsedTime();
        Final_Actuation_Milliseconds = ( (T2_ElapsedSeconds * 1000) + (T2_Elapsed_int_count * 10.2040) );
        actuation_bool = true;
        pin_puller_retracted = true;
       }
    }


    // Finally, call BIT to do safety checks if we're not in the firing sequence
    if ( (!fired) || (proximity || proximity_expired) )
     {
     // BIT_Exec();
     }

  } // Main control loop
        
   

}