#include "xc.h"
#include "config.h"
#include "extern.h"
#include "../CommonCode/types.h"
#include "libq.h"

int BlinkToggle = 0;
    
int OnOffBlink(int x, int WarningThreshold, int AlarmThreshold)
{   
    if((x >=  WarningThreshold) && (x <  AlarmThreshold))  //Blink if higher than warning threshold temperature and below alarm threshold
        return BlinkToggle;
    
    if(x >= AlarmThreshold) //Turn on solid if higher than alarm threshold 
        return 1;
    
    return 0; //Turn off if below warning threshold 
}


void ManageLEDs(void)
{
    static int ctr = 0;
    int pwm_state;
    static unsigned int last_CentiSecCtr = 0;

    
    if((CentiSecCtr-last_CentiSecCtr) > LED_BLINK_PERIOD/2) //If half of the toggle period has passed.  2's complement on unsigned integer deals with rollover.
    {
        BlinkToggle = ~BlinkToggle;  //Toggle, resulting in LED_BLINK_PERIOD.
        last_CentiSecCtr = CentiSecCtr;
    }
    
    if(++ctr >= 16)
        ctr = 0;
    
    if(ctr < DimmerCtr)
        pwm_state = 1;
    else
        pwm_state = 0;
    
    
    
    AUXIO_PIN2 = pwm_state;
    
//********** Light LED's that depend on bottom side state ***********
   //if(pwm_state)
    //{
      EXTENDER_LED = SerialDataIn->status.bitfields.ZoomExtendState;
      CAM_PWR_LED = SerialDataIn->status.bitfields.CameraPowerState;// & pwm_state;
      LASER_PWR_LED = SerialDataIn->status.bitfields.LaserPowerState;// & pwm_state;  

      WATER_ALARM_LED = (SerialDataIn->status.bitfields.AftWaterAlarm) || (SerialDataIn->status.bitfields.FwdWaterAlarm);
      if(SerialDataIn->status.bitfields.OverTemperatureState)
      {
        TEMP_ALARM_LED = 1;
        top_status.bitfields.CameraPowerRequest = 0;  //Negate power-on request
      }
      else
        TEMP_ALARM_LED = OnOffBlink(SerialDataIn->CameraTemp,CAMERA_TEMP_WARNING_THRESHOLD,CAMERA_TEMP_ALARM_THRESHOLD);
      
              
      HUM_ALARM_LED = OnOffBlink(SerialDataIn->Humidity,CAN_HUMIDITY_WARNING_THRESHOLD,CAN_HUMIDITY_ALARM_THRESHOLD);
              
   // }
   // else
  //  {
  //    EXTENDER_LED = 0;
  //    CAM_PWR_LED = 0;
  //    LASER_PWR_LED = 0;
  //  }
       
      
//**** Set/Clear Buzzer depending on thresholds
//if((SerialDataIn->Humidity > CAN_HUMIDITY_BUZZER_THRESHOLD) ||  
//  (SerialDataIn->CameraTemp > CAMERA_TEMP_BUZZER_THRESHOLD) || 
//  (SerialDataIn->status.bitfields.OverTemperatureState == 1)  || 
//  (SerialDataIn->status.bitfields.AftWaterAlarm) || 
//  (SerialDataIn->status.bitfields.FwdWaterAlarm)) 
//    PIEZO_ALARM = 1; 
//else
    PIEZO_ALARM = 0;
    
 


//Light LED's based on top side state tracking (for testing)
    /* LEDs below handled by navproc */
/*              
    JOYSTCK_REV_LED = top_status.bitfields.JoystickReversalRequest;// & pwm_state;         
    JOYSTCK_SEN_LED = top_status.bitfields.JoystickSensitivityRequest;// & pwm_state;       
    PORT_LIGHT_LED = top_status.bitfields.PortLightRequest;// & pwm_state;    
    STBD_LIGHT_LED = top_status.bitfields.StbdLightRequest;// & pwm_state;    
    PORT_CAM_LED = top_status.bitfields.PortCamRequest;// & pwm_state;    
    STBD_CAM_LED = top_status.bitfields.StbdCamRequest;// & pwm_state;    
*/

}