 /*
 * Copyright (c) 2013 - 2016, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "../source/system/nt_system_prv.h"
#include "../source/system/nt_system_mem_prv.h"
#include "../source/system/nt_system_frmstr_prv.h"
#include "nt_modules.h"
#include "../source/modules/nt_modules_prv.h"
#include "nt_keydetectors.h"
#include "../source/electrodes/nt_electrodes_prv.h"
#include "nt_electrodes.h"
#include "nt_module_gpio.h"

#include "../source/safety/nt_safety_prv.h"
#include "../source/safety/nt_safety_gpio.h"
#include "nt_safety.h"
#include "nt_filters.h"
#include "fsl_clock.h"

/* Local functions */
static int32_t nt_module_safety_gpio_check_interface(const struct nt_module_data *module);

static int32_t _nt_module_safety_drive_electrodes_low(struct nt_module_data *module);
static int32_t _nt_module_safety_switch_electrodes_gpio(struct nt_module_data *module);
static int32_t _nt_module_safety_switch_electrodes_tsi(struct nt_module_data *module);

/** interface module safety */
const struct nt_module_safety_interface nt_safety_interface = 
{
  .init              = _nt_module_safety_init,
  .process           = _nt_module_safety_process,
  .name              = "nt_module_safety_interface",
};

/* Init the safety functions for the module. */ 
int32_t _nt_module_safety_init(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL);
  NT_ASSERT(module->rom->safety_interface != NULL);
  NT_ASSERT(module->rom->safety_interface->process != NULL);  
  NT_ASSERT(module->rom->electrodes != NULL);
  
  /* Enable all ports clock for gpios */
  CLOCK_EnableClock(kCLOCK_PortA);
  CLOCK_EnableClock(kCLOCK_PortB);
  CLOCK_EnableClock(kCLOCK_PortC);
  CLOCK_EnableClock(kCLOCK_PortD);  
  CLOCK_EnableClock(kCLOCK_PortE);  
  
  if (nt_module_safety_gpio_check_data(module) < NT_SUCCESS) 
  {
      return NT_FAILURE;
  }
    
  return NT_SUCCESS;
}

/* Process the safety on the module. */
int32_t _nt_module_safety_process(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL);

  nt_module_safety_check_electrodes(module);
  
  return NT_SUCCESS;
}

/* Check the safety interface. */
static int32_t nt_module_safety_gpio_check_interface(const struct nt_module_data *module)
{
  NT_ASSERT(module != NULL);
  NT_ASSERT(module->rom->safety_params.gpio->user_interface != NULL);
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  if (interface->set_pin_output == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->set_pin_input == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->set_pin_low == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->set_pin_high == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->init_timer == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->start_timer == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->stop_timer == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->timer_reset_counter == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->timer_get_counter == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->timer_get_overrun == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->set_pin_default_state == NULL) 
  {
    return NT_FAILURE;
  } else if (interface->get_pin_value == NULL) 
  {
    return NT_FAILURE;
  }
  return NT_SUCCESS;
}

/* Check the safety data.*/
int32_t nt_module_safety_gpio_check_data(const struct nt_module_data *module)
{
  NT_ASSERT(module != NULL); 
  if (module->rom->safety_params.gpio->user_interface == NULL) 
  {
      return NT_FAILURE; 
  }
  if (nt_module_safety_gpio_check_interface(module) < NT_SUCCESS) 
  {
      return NT_FAILURE; 
  }
  return NT_SUCCESS;
}

/* Single electrode short to GND */
int32_t nt_module_safety_check_electrode_short_gnd(const struct nt_electrode *elec)
{  
  NT_ASSERT(elec != NULL);
  uint32_t res;
  struct nt_module_data *module = nt_electrode_get_module_data(elec);
    
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  
  if(elec->gpio_input.gpio1 == NULL)   /* At least one GPIO must be assigned to execute the test */
  {
    return NT_NOT_SUPPORTED; 
  }
  
  uint32_t pin = elec->gpio_input.gpio1 & 0xFFU; 
  uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT;
 
  /* Sets GPIO mode input with pull-UP enabled */
  interface->set_pin_gpio_mode(port, pin); /* GPIO mode */
  interface->set_pin_input(port, pin);     /* GPIO input */
  interface->set_pin_pull_up(port, pin);   /* pull-up */
  
  res = interface->get_pin_value(port, pin); /* Read val back from the port/pin */ 
  
  if(elec->gpio_input.gpio2 != NULL) /* For 2nd GPIO if defined (mutual mode) */
  {
    pin = elec->gpio_input.gpio2 & 0xFFU;
    port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;
    
    interface->set_pin_gpio_mode(port, pin); /* GPIO mode */
    interface->set_pin_input(port, pin);     /* GPIO input */
    interface->set_pin_pull_up(port, pin);   /* pull-up */
        
    res = interface->get_pin_value(port, pin);   /* Read val back from the port/pin */
  }
  
  if(res == 0) /* If read "Zero" from port/pin */  
  {
    return NT_FAILURE;   
  }
    
  return NT_SUCCESS;  
}

/* Single electrode short to Vdd */
int32_t nt_module_safety_check_electrode_short_vdd(const struct nt_electrode *elec)
{
  NT_ASSERT(elec != NULL);
  uint32_t res;
  struct nt_module_data *module = nt_electrode_get_module_data(elec);
  
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  
  if(elec->gpio_input.gpio1 == NULL)    /* At least one GPIO must be assigned to execute the test */
  {
    return NT_NOT_SUPPORTED; 
  }
  
  uint32_t pin = elec->gpio_input.gpio1 & 0xFFU; 
  uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT;
 
  /* Sets GPIO mode input with pull-Down enabled */
  interface->set_pin_gpio_mode(port, pin); /* GPIO mode */
  interface->set_pin_input(port, pin);     /* GPIO input */
  interface->set_pin_pull_down(port, pin); /* pull-down */

  res = interface->get_pin_value(port, pin); /* Read val back from the port/pin */
  
  if(elec->gpio_input.gpio2 != NULL) /* For 2nd GPIO if defined (mutual mode) */
  {
    pin = elec->gpio_input.gpio2 & 0xFFU;
    port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;
    
    interface->set_pin_gpio_mode(port, pin);  /* GPIO mode */
    interface->set_pin_input(port, pin);      /* GPIO input */
    interface->set_pin_pull_down(port, pin);  /* pull-down */

    res = interface->get_pin_value(port, pin);  /* Read val back from the port/pin */
  } 
 
  if(res == 1) 
  {
    return NT_FAILURE; /* If read "One" from port/pin */  
  }

  return NT_SUCCESS;  
}

/* Single electrode short with adjacent electrode */
int32_t nt_module_safety_check_electrode_short_adj(const struct nt_electrode *elec)
{
  NT_ASSERT(elec != NULL);
  uint32_t res;
  struct nt_module_data *module = nt_electrode_get_module_data(elec);
  
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  
  if(elec->gpio_input.gpio1 == NULL)    /* At least one GPIO must be assigned to execute the test */
  {
    return NT_NOT_SUPPORTED; 
  } 
  
  _nt_module_safety_drive_electrodes_low(module); /* Set Zero output on all pins */
  
  uint32_t pin = elec->gpio_input.gpio1 & 0xFFU; 
  uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT;
  
  /* Sets GPIO mode input with pull-UP enabled */
  interface->set_pin_gpio_mode(port, pin); /* GPIO mode */
  interface->set_pin_input(port, pin);     /* GPIO input */
  interface->set_pin_pull_up(port, pin);   /* pull-up */

  res = interface->get_pin_value(port, pin);  /* Read val back from the port/pin */
  
  if(elec->gpio_input.gpio2 != NULL)       /* For 2nd GPIO if defined (mutual mode) */ 
  {
    pin = elec->gpio_input.gpio2 & 0xFFU;
    port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;
    
    interface->set_pin_gpio_mode(port, pin); /* GPIO mode */
    interface->set_pin_input(port, pin);     /* GPIO input */
    interface->set_pin_pull_up(port, pin);   /* pull-up */
    
     res = interface->get_pin_value(port, pin); /* Read val back from the port/pin */
   }
     
  if(res == 0) /* If read "Zero" from port/pin */ 
  {
    return NT_FAILURE;   
  }
  
  return NT_SUCCESS;  
}

/* Check electrode baseline too low level */
int32_t nt_module_safety_check_baseline_low(const struct nt_electrode *elec)
{
  NT_ASSERT(elec != NULL);
  if(elec->baseline_level == NULL)   /* Baseline test only if baseline level was assigned */
  {
    return NT_NOT_SUPPORTED;  
  }
  
  struct nt_electrode_data *electrode_data = nt_electrode_get_data(elec);
  int32_t s_delta = (electrode_data->baseline - elec->baseline_level);
  
  if(s_delta < 0)  /* for too low baseline level must be negative delta */
  {
    s_delta = -s_delta;   /* make positive value */
    if( (s_delta << 3) > elec->baseline_level) /* baseline neg. change is more than 12.5% of normal baseline level */
    {
      return NT_FAILURE;
    }
  }
  return NT_SUCCESS;  
}

/* Check electrode baseline too high level */
int32_t nt_module_safety_check_baseline_high(const struct nt_electrode *elec)
{
  NT_ASSERT(elec != NULL);    
  if(elec->baseline_level == NULL)  /* Baseline test only if baseline level was assigned */
  {
    return NT_NOT_SUPPORTED;  
  }
  
  struct nt_electrode_data *electrode_data = nt_electrode_get_data(elec);
  int32_t s_delta = (electrode_data->baseline - elec->baseline_level);
  
  if(s_delta > 0)  /* for too high baseline level must be positive delta */
  {
    if( (s_delta << 2) > elec->baseline_level) /* baseline pos. change is more than 25% of normal baseline level */
    {
      return NT_FAILURE;
    }
  }    
  return NT_SUCCESS;
}

int32_t nt_module_safety_check_electrodes(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL); 
  uint32_t el_counter = module->electrodes_cnt;
  static uint32_t fault_cnt = 0;
  
  union nt_system_event_context context;
  
  /* Check if the GPIO methods are available, if unavailable skip */
  if(nt_module_safety_gpio_check_data(module) != NT_SUCCESS) 
  {
    return NT_FAILURE;
  }
  
  /* Disable enabled electrodes at first, disable the TSI IRQ ? */
  /*---------------------------------------------------------------------------- */
  _nt_module_safety_switch_electrodes_gpio(module); /* Switch all pins to GPIO */
  
  el_counter = module->electrodes_cnt;
  while (el_counter--)                    /* The main test loop */
  {
    struct nt_electrode *elec = (struct nt_electrode *)module->electrodes[el_counter]->rom;
    
    /* Check Short to GND */
    if(nt_module_safety_check_electrode_short_gnd(elec) == NT_FAILURE) 
    {
      fault_cnt++;                                       /* Increment fault counter */
      context.sig_out_of_limit.electrode = elec;         /* Store electrode not passed */           
      _nt_system_invoke_callback(NT_SYSTEM_EVENT_ELEC_SHORT_GND, &context); /* Call callback */
    }
    
    /* Check Short to VDD */
    if(nt_module_safety_check_electrode_short_vdd(elec) == NT_FAILURE) 
    {
      fault_cnt++;                                       /* Increment fault counter */
      context.sig_out_of_limit.electrode = elec;         /* Store electrode not passed */           
      _nt_system_invoke_callback(NT_SYSTEM_EVENT_ELEC_SHORT_VDD, &context);  /* Call callback */
      
    }
    
    /* Check Short to Adjacent */
    if(nt_module_safety_check_electrode_short_adj(elec) == NT_FAILURE) 
    {
      fault_cnt++;                                       /* Increment fault counter */
      context.sig_out_of_limit.electrode = elec;         /* Store electrode not passed */           
      _nt_system_invoke_callback(NT_SYSTEM_EVENT_ELEC_SHORT_ADJ, &context); /* Call callback */
      
    }
    
    /* Check baseline is too low */
    if(nt_module_safety_check_baseline_low(elec) == NT_FAILURE) 
    {
      fault_cnt++;                                      /* Increment fault counter */
      context.sig_out_of_limit.electrode = elec;        /* Store electrode not passed */            
      _nt_system_invoke_callback(NT_SYSTEM_EVENT_SIGNAL_LOW, &context); /* Call callback */
      
    }
    
    /* Check baseline is too high */
    if(nt_module_safety_check_baseline_high(elec) == NT_FAILURE) 
    {
      fault_cnt++;                                     /* Increment fault counter */
      context.sig_out_of_limit.electrode = elec;       /* Store electrode not passed */             
      _nt_system_invoke_callback(NT_SYSTEM_EVENT_SIGNAL_HIGH, &context);
    }
  }
    
  _nt_module_safety_switch_electrodes_tsi(module); /* Switch all pins to TSI */
    
  // Or disable TSI interrupt before Safety GPIO manipulation and re-enable after testing.
  /*---------------------------------------------------------------------------- */  
  /* Restore enabled electrodes back ? */

  if (fault_cnt == 0U)
    return NT_SUCCESS; 
  else
    return NT_FAILURE; 
}

static int32_t _nt_module_safety_drive_electrodes_low(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL); 
  uint32_t el_counter = module->electrodes_cnt;
  
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
    
  while (el_counter--)
  {    
    struct nt_electrode_data *electrode_data = module->electrodes[el_counter];
    struct nt_electrode *elec = (struct nt_electrode *)electrode_data->rom;
    
    if(elec->gpio_input.gpio1 == NULL)
    {
        continue;     /* if electrode is not defined go to next electrode */
    }
    
    uint32_t pin = elec->gpio_input.gpio1 & 0xFFU;                /* Select pin  */
    uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT; /* Select port */  
    interface->set_pin_gpio_mode(port, pin);         /* Change pin to GPIO mode */
    interface->set_pin_low(port, pin);               /* Set GPIO pin zero level */ 
    
    if(elec->gpio_input.gpio2 != NULL)     /* For 2nd GPIO if defined (mutual mode) */
    {
      pin = elec->gpio_input.gpio2 & 0xFFU;                   /* Select pin  */
      port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;    /* Select port */
      interface->set_pin_gpio_mode(port, pin);       /* Change pin to GPIO mode */
      interface->set_pin_low(port, pin);             /* Set GPIO pin zero level */ 
    }
  }
  
  return NT_SUCCESS;   
}

/* Change all pins to GPIO mode */
static int32_t _nt_module_safety_switch_electrodes_gpio(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL);   
  uint32_t el_counter = module->electrodes_cnt;
  
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  
  while (el_counter--)
  {
    struct nt_electrode_data *electrode_data = module->electrodes[el_counter];
    struct nt_electrode *elec = (struct nt_electrode *)electrode_data->rom;
    
    if(elec->gpio_input.gpio1 == NULL)
    {
        continue;     /* if not defined go to next electrode */
    }
    uint32_t pin = elec->gpio_input.gpio1 & 0xFFU;                /* Select pin  */
    uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT; /* Select port */ 
    interface->set_pin_gpio_mode(port, pin);      /* Set pinmux to GPIO mode */
    
    if(elec->gpio_input.gpio2 != NULL)      /* For 2nd GPIO if defined (mutual mode) */
    {
      pin = elec->gpio_input.gpio2 & 0xFFU;                       /* Select pin  */
      port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;        /* Select port */
      interface->set_pin_gpio_mode(port, pin);    /* Set pinmux to GPIO mode */
    }
  }
  
  return NT_SUCCESS;   
}

/* Change all pins to periferal mode as TSI electrode */
static int32_t _nt_module_safety_switch_electrodes_tsi(struct nt_module_data *module)
{
  NT_ASSERT(module != NULL); 
  uint32_t el_counter = module->electrodes_cnt;
  
  const struct nt_module_gpio_user_interface *interface = 
    module->rom->safety_params.gpio->user_interface;
  
  while (el_counter--)
  {
    struct nt_electrode_data *electrode_data = module->electrodes[el_counter];
    struct nt_electrode *elec = (struct nt_electrode *)electrode_data->rom;
    
    if(elec->gpio_input.gpio1 == NULL)
    {
        continue;     /* if electrode is not defined go to next electrode */
    }
    uint32_t pin = elec->gpio_input.gpio1 & 0xFFU;                /* Select pin  */
    uint32_t port = elec->gpio_input.gpio1 >> NT_GPIO_PORT_SHIFT; /* Select port */ 
    interface->set_pin_tsi_mode(port, pin);   /* Set pinmux to TSI peripheral mode */
    
    if(elec->gpio_input.gpio2 != NULL)        /* For 2nd GPIO if defined (mutual mode) */               
    {
      pin = elec->gpio_input.gpio2 & 0xFFU;                       /* Select pin  */
      port = elec->gpio_input.gpio2 >> NT_GPIO_PORT_SHIFT;        /* Select port */
      interface->set_pin_tsi_mode(port, pin); /* Set pinmux to TSI peripheral mode */
    }
  }
  return NT_SUCCESS;   
}
