/*
 * led.c
 *
 *  Created on: Jan 18, 2018
 *      Author: wn
 */

#include "led.h"
#include "stm32f1xx_hal.h"
#include <PontCoopScheduler.h>
#include <diag/Trace.h>
#include <stdbool.h>

typedef enum { LED_ON, LED_OFF, LED_BLINK } eLedCmd;

typedef struct {
  eLedCmd cmdRed;
  bool stateRed;
  eLedCmd cmdGreen;
  bool stateGreen;
} tLedHandle;

tLedHandle ledHandle = { .cmdRed = LED_OFF, .stateRed = false, .cmdGreen = LED_OFF, .stateGreen = false  };

void ledExec(void *handle) {
  tLedHandle *lLedHandle = (tLedHandle*) handle;

  if ((lLedHandle->cmdRed == LED_OFF) && (lLedHandle->stateRed)) {
    HAL_GPIO_WritePin(GPIOC, ERROR_Pin, GPIO_PIN_RESET);
    lLedHandle->stateRed = false;
  }

  if ((lLedHandle->cmdRed == LED_ON) && (! lLedHandle->stateRed)) {
    HAL_GPIO_WritePin(GPIOC, ERROR_Pin, GPIO_PIN_SET);
    lLedHandle->stateRed = true;
  }

  if (lLedHandle->cmdRed == LED_BLINK) {
    HAL_GPIO_TogglePin(GPIOC, ERROR_Pin);
    lLedHandle->stateRed = true;
  }

  if ((lLedHandle->cmdGreen == LED_OFF) && (lLedHandle->stateGreen)) {
    HAL_GPIO_WritePin(GPIOC, LED2_Pin, GPIO_PIN_RESET);
    lLedHandle->stateGreen = false;
  }

  if ((lLedHandle->cmdGreen == LED_ON) && (! lLedHandle->stateGreen)) {
    HAL_GPIO_WritePin(GPIOC, LED2_Pin, GPIO_PIN_SET);
    lLedHandle->stateGreen = true;
  }

  if (lLedHandle->cmdGreen == LED_BLINK) {
    HAL_GPIO_TogglePin(GPIOC, LED2_Pin);
    lLedHandle->stateGreen = true;
  }
}

void ledGreenOn() {
  ledHandle.cmdGreen = LED_ON;
}
void ledGreenOff() {
  ledHandle.cmdGreen = LED_OFF;
}
void ledGreenBlink() {
  ledHandle.cmdGreen = LED_BLINK;
}
void ledRedOn() {
  ledHandle.cmdRed = LED_ON;
}
void ledRedOff() {
  ledHandle.cmdRed = LED_OFF;
}
void ledRedBlink() {
  ledHandle.cmdRed = LED_BLINK;
}


void ledInit() {
  schAdd(ledExec, &ledHandle, 0, 100);
  HAL_GPIO_WritePin(GPIOC, ERROR_Pin|LED2_Pin, GPIO_PIN_RESET);
}