/*
 * board_logic.c
 *
 */

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

#include "main.h"
#include "modbus_logic.h"
#include "board_logic.h"

#define BOARD_DESC_LEN (16)

extern TIM_HandleTypeDef htim1;
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim14;
static const char board_description[BOARD_DESC_LEN] = "RS485_Relay V2R1";
extern uint16_t tranfer_errors_count;

#define REL_MAIN_ON		HAL_GPIO_WritePin(RL_MAIN_GPIO_Port, RL_MAIN_Pin, GPIO_PIN_SET)
#define REL_MAIN_OFF	HAL_GPIO_WritePin(RL_MAIN_GPIO_Port, RL_MAIN_Pin, GPIO_PIN_RESET)
#define REL_AUX_ON		HAL_GPIO_WritePin(RL_AUX_GPIO_Port, RL_AUX_Pin, GPIO_PIN_SET)
#define REL_AUX_OFF		HAL_GPIO_WritePin(RL_AUX_GPIO_Port, RL_AUX_Pin, GPIO_PIN_RESET)
#define LED_MAIN_ON		HAL_GPIO_WritePin(LED_MAIN_GPIO_Port, LED_MAIN_Pin, GPIO_PIN_SET)
#define LED_MAIN_OFF	HAL_GPIO_WritePin(LED_MAIN_GPIO_Port, LED_MAIN_Pin, GPIO_PIN_RESET)
#define LED_AUX_ON		HAL_GPIO_WritePin(LED_AUX_GPIO_Port, LED_AUX_Pin, GPIO_PIN_SET)
#define LED_AUX_OFF		HAL_GPIO_WritePin(LED_AUX_GPIO_Port, LED_AUX_Pin, GPIO_PIN_RESET)
#define LED_ACT_ON		HAL_GPIO_WritePin(LED_ACT_GPIO_Port, LED_ACT_Pin, GPIO_PIN_SET)
#define LED_ACT_OFF		HAL_GPIO_WritePin(LED_ACT_GPIO_Port, LED_ACT_Pin, GPIO_PIN_RESET)


uint16_t relays = 0;
uint16_t status = 0;
uint16_t motor1_pwm = 0;
uint16_t motor2_pwm = 0;
uint16_t lights_pwm = 0;
volatile uint16_t lights_pwm_target = 0;
volatile int16_t  lights_pwm_delta = 0;
uint16_t led_time_act = 0;

void estop_reset(void) {
  HAL_GPIO_WritePin(RL_EN_GPIO_Port, RL_EN_Pin, GPIO_PIN_RESET);
  HAL_Delay(1);
  HAL_GPIO_WritePin(RL_EN_GPIO_Port, RL_EN_Pin, GPIO_PIN_SET);
}

void board_init(void) {
  // Activate (normalize) E-STOP trigger
  estop_reset();

  // Check if E-STOP is not shorted
  if (HAL_GPIO_ReadPin(ESTOP_GPIO_Port, ESTOP_Pin) == GPIO_PIN_RESET)
    status = status | 0b010;

  // 1ms timer start
  HAL_TIM_Base_Start_IT(&htim1);
}


uint16_t clamp_duty(uint16_t duty) {
  if (duty > PWM_DUTY_MAX) return PWM_DUTY_MAX;
  if (duty < PWM_DUTY_MIN) return PWM_DUTY_MIN;
  return duty;
}

void set_pwm(uint8_t unit, uint16_t duty) {
  uint32_t channel;
  TIM_HandleTypeDef* tim;

  switch (unit) {
    case 1: // motor 1
      tim = &htim3;
      channel = TIM_CHANNEL_1;
      break;
    case 2: // motor 2
      tim = &htim3;
      channel = TIM_CHANNEL_2;
      break;
    case 3: // lights
      tim = &htim14;
      channel = TIM_CHANNEL_1;
      break;
    default:
      return;
  }

  duty = clamp_duty(duty);
  if (duty == 0)
    HAL_TIM_PWM_Stop_IT(tim, channel);
  else {
    HAL_TIM_PWM_Start_IT(tim, channel);
    __HAL_TIM_SetCompare(tim, channel, duty);
    //HAL_Delay(1);
  }
}

void set_light(uint16_t duty) {
  lights_pwm_target = clamp_duty(duty);
  if (lights_pwm_target != lights_pwm) {
    lights_pwm_delta = (lights_pwm_target - lights_pwm) / (LIGHTS_TIME / PWM_LIGHTS_STEP);
    if (lights_pwm_delta == 0) {
      if (lights_pwm_target - lights_pwm > 0) lights_pwm_delta = 1;
      else lights_pwm_delta = -1;
    }
  }
}

void update_light(void) {
  static uint32_t count_ms = 0;
  // Check if we should change lights pwm for smooth transition
  if (lights_pwm != lights_pwm_target) {
    // Change pwm only every 20ms (PWM_LIGHTS_STEP)
    if(++count_ms == PWM_LIGHTS_STEP) {
      count_ms = 0;

      lights_pwm = clamp_duty(lights_pwm + lights_pwm_delta);
      // Set lights pwm to target if the difference is less than the delta
      if (abs(lights_pwm_target - lights_pwm) < abs(lights_pwm_delta))
        lights_pwm = lights_pwm_target;
      set_pwm(3, lights_pwm);
    }
  }
}

void loop_iterate()
{
  
  if(relays&REL_MAIN_BIT)
	  {
	  REL_MAIN_ON;
	  LED_MAIN_ON;
	  }
  else
	  {
	  REL_MAIN_OFF;
	  LED_MAIN_OFF;
	  }
  if(relays&REL_AUX_BIT)
	  {
	  REL_AUX_ON;
	  LED_AUX_ON;
	  }
  else
	  {
	  REL_AUX_OFF;
	  LED_AUX_OFF;
	  }


/*  */
  modbus_loop_iterate();
  HAL_Delay(1);
}

void update_service_indication(void)
{
	if (led_time_act) {
    	LED_ACT_ON;
    	led_time_act--;
  } else
    	LED_ACT_OFF;


}


uint8_t read_register(uint16_t address, uint16_t* value)
{
	if (address == 0x0001)
		*value = MODBUS_PROTOCOL_VERSION;
	else if (address == 0x0002)
		*value = MODBUS_FIRMWARE_VERSION;
	else if (address == 0x0003)
		*value = MODBUS_BOARD_TYPE;
	else if (address == 0x0004)
		*value = tranfer_errors_count;
	else if (address == 0x0005)
		*value = 0; // supported data rate: unknown
	else if (address >= 0x0010 && address <= 0x0099)
	{
		int index = address - 0x0010;
		if (index < BOARD_DESC_LEN)
			*value = board_description[index];
		else
			*value = 0;
	}
    else if (address == 0x2001)  //Read relays state
    {
        *value = relays;
    }
    else if (address == 0x2002)  //Read motor 1 pwm
    {
        *value = motor1_pwm;
    }
    else if (address == 0x2003)  //Read motor 2 pwm
    {
        *value = motor2_pwm;
    }
    else if (address == 0x2004)  //Read Lights pwm
    {
        *value = lights_pwm;
    }
    else if (address == 0x2010)  //Read status
    {
        *value = status;
        status &= 0b011; // Reset light button press event status
    }
    else if (address == 0x2011)  //Read E-STOP status
    {
        *value = (status & 0b010)>>1; // Read E-stop status bit and shift it to bit 0 position
    }
	else
		return 1;
	return 0;
}

uint8_t write_register(uint16_t address, uint16_t value)
{
  uint8_t ret;
  ret = 0;
  #ifdef UART_DEBUG
  printf("Write A=0x%X D=0x%X\n\r",address,value);
  #endif
  switch (address) {
    case 0x2001:
      if (status & 0b010) break;    // Check if we are in emergency stop mode
      relays = value;
      break;
    case 0x2002:
      motor1_pwm = value;
      set_pwm(1, motor1_pwm);
      break;
    case 0x2003:
      motor2_pwm = value;
      set_pwm(2, motor2_pwm);
      break;
    case 0x2004:
      set_light(value);
      break;
    case 0x2020:
      if (value == 1) {
        estop_reset();
        status &= 0b101;
      }
      break;
    default:
      ret = 1;
  }
	return ret;
}

// Pin external interrupts handler
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin == ESTOP_Pin) {
    status = status | 0b010;
    relays = 0;
  }
  if(GPIO_Pin == LIGHTS_SW_Pin) {
    // Set "lights switch pressed" status bit
    status |= 0b100;
    if (lights_pwm_target > 0) set_light(0);
    else set_light(255);
  }
  if(GPIO_Pin == WATER_Pin) {
    // Set or reset "water" status bit
    status |= HAL_GPIO_ReadPin(WATER_GPIO_Port, WATER_Pin);
    if (HAL_GPIO_ReadPin(WATER_GPIO_Port, WATER_Pin)) status |= 1;
    else status &= ~1;
  }
}

// Timers overflow interrupt
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
  // check if the interrupt comes from TIM1
  if(htim->Instance == TIM1) {

    update_service_indication();

    update_light();
  }
}