--[[
  Copyright (c) 2021-2023 by Plexim GmbH
  All rights reserved.

  A free license is granted to anyone to use this software for any legal
  non safety-critical purpose, including commercial applications, provided
  that:
  1) IT IS NOT USED TO DIRECTLY OR INDIRECTLY COMPETE WITH PLEXIM, and
  2) THIS COPYRIGHT NOTICE IS PRESERVED in its entirety.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  SOFTWARE.
--]]--
local Module = {}
local U = require('common.utils')

local static = {}

function Module.getBlock(globals, cpu)

  local Estimator = require('common.block').getBlock(globals, cpu)
  if static[cpu] == nil then
    static[cpu] = {
      numInstances = 0,
      instances = {},
      finalized = false,
    }
  end
  Estimator["instance"] = static[cpu].numInstances
  static[cpu].numInstances = static[cpu].numInstances + 1

  function Estimator:checkMaskParameters()
  end

  function Estimator:p_getDirectFeedthroughCode()
    local Require = ResourceList:new()
    local OutputCode = U.CodeLines:new()
    local OutputSignal = StringList:new()

    table.insert(static[self.cpu].instances, self.bid)

    Require:add('EST-%d' % {self.instance})

    local in_Iab = 1
    local in_Vab = 2
    -- local in_Vdc = 3
    local in_RsSet = 3
    -- local in_ForceAngleDir = 4
    local in_Enable = 4
    -- local in_IdqSet = 7
    -- local in_EnOnlineRs = 7
    -- local in_UpdateRs = 6
    local in_LSet = 5

    local out_the = 1
    -- local out_Idq = 2
    -- local out_1_Vdc = 3
    local out_wm = 2
    local out_state = 3
    local out_psi = 4
    -- local out_Rs_online = 5
    local out_Rs = 5
    -- local out_Idq_Ref = 8
    local out_Ls = 6

    self.rs_is_variable = false
    if Block.Mask.R_sel == 2 and
        not string.match(Block.InputSignal[in_RsSet][1], "UNCONNECTED") then
      self.rs_is_variable = true
    end

    self.ls_is_variable = false
    if Block.Mask.L_sel == 2 and
      not string.match(Block.InputSignal[in_LSet][1], "UNCONNECTED") then
      self.ls_is_variable = true
    end   

    local inputVarName = "est%iInputData" % {self.instance}
    local outputVarName = "est%iOutputData" % {self.instance}
    OutputCode:append('PLXHAL_EST_Inputs_t %s;\n' % {inputVarName})
    OutputCode:append('PLXHAL_EST_Outputs_t %s;\n' % {outputVarName})
    OutputCode:append('%s.ia = %s;\n' %
                          {inputVarName, Block.InputSignal[in_Iab][1]})
    OutputCode:append('%s.ib = %s;\n' %
                          {inputVarName, Block.InputSignal[in_Iab][2]})
    OutputCode:append('%s.va = %s;\n' %
                          {inputVarName, Block.InputSignal[in_Vab][1]})
    OutputCode:append('%s.vb = %s;\n' %
                          {inputVarName, Block.InputSignal[in_Vab][2]})
    OutputCode:append('%s.enable = %s;\n' %
                          {inputVarName, Block.InputSignal[in_Enable][1]})

    if self.rs_is_variable then
      OutputCode:append('%s.rs = %s;\n' % {inputVarName, Block.InputSignal[in_RsSet][1]})
    end
    if self.ls_is_variable then
      OutputCode:append('%s.ld = %s;\n' % {inputVarName, Block.InputSignal[in_LSet][1]})
    end

    OutputCode:append('PLXHAL_EST_update(%i, &%s, &%s);\n' %
                          {self.instance, inputVarName, outputVarName})

    OutputSignal[out_the] = {}
    OutputSignal[out_the][1] = "%s.angle_rad" % {outputVarName}
    -- OutputSignal[out_Idq] = {}
    -- OutputSignal[out_Idq][1] = "idq_A.value[0]"
    -- OutputSignal[out_Idq][2] = "idq_A.value[1]"
    -- OutputSignal[out_1_Vdc] = {}
    -- OutputSignal[out_1_Vdc][1] = "Estimator%iOutputData.oneOverDcBus_invV" % {instance}
    OutputSignal[out_wm] = {}
    OutputSignal[out_wm][1] = "%s.fe_rps" % {outputVarName}
    OutputSignal[out_state] = {}
    OutputSignal[out_state][1] = "%s.state" % {outputVarName}

    OutputSignal[out_psi] = {}
    OutputSignal[out_psi][1] = "%s.flux_wb" % {outputVarName}

    -- OutputSignal[out_Rs_online] = {}
    -- OutputSignal[out_Rs_online][1] = "EST_getRsOnLine_Ohm(Estimator%iHandle)" % {outputVarName}

    -- OutputSignal[out_Idq_Ref] = {}
    -- OutputSignal[out_Idq_Ref][1] = "Idq_ref_A.value[0]"
    -- OutputSignal[out_Idq_Ref][2] = "Idq_ref_A.value[1]"

    OutputSignal[out_Rs] = {}
    OutputSignal[out_Rs][1] = "%s.rs_ohm" % {outputVarName}

    OutputSignal[out_Ls] = {}
    OutputSignal[out_Ls][1] = "%s.ld_henry" % {outputVarName}

    self.templateDict = {}
    self.templateDict["|>BASE_NAME<|"] = Target.Variables.BASE_NAME

    -- timing
    self.templateDict["|>USER_SYSTEM_FREQ_MHz<|"] = Block.Mask.sysclk_MHz
    
    self.templateDict["|>USER_PWM_FREQ_kHz<|"] = Block.Mask.fpwm / 1000
    
    self.templateDict["|>USER_NUM_PWM_TICKS_PER_ISR_TICK<|"] = "%i" % {Block.Mask.pwm_per_ISR}
    
    self.templateDict["|>USER_VOLTAGE_FILTER_POLE_Hz<|"] = Block.Mask.vfilt_pole
    

    -- zero speed operation (Remove this)
    self.templateDict["|>USER_FORCE_ANGLE_FREQ_Hz<|"] = "%f" % {0.0}
    

    -- online resistance estimation (Probably want to remove this)
    self.templateDict["|>RsOnLine_DeltaInc_Ohm<|"] = Block.Mask.rs_online_inc
    
    self.templateDict["|>RsOnLine_DeltaDec_Ohm<|"] = Block.Mask.rs_online_dec
    
    self.templateDict["|>RsOnLine_min_Ohm<|"] = Block.Mask.rs_online_min
    
    self.templateDict["|>RsOnLine_max_Ohm<|"] = Block.Mask.rs_online_max
    
    self.templateDict["|>RsOnLine_angleDelta_rad<|"] = Block.Mask.rs_online_delta
    
    self.templateDict["|>RsOnLine_pole_rps<|"] = Block.Mask.rs_online_pole_hz * 2 * math.pi
    

    if Target.Name == "TI2806x" then
      self.templateDict["|>USER_NUM_ISR_TICKS_PER_CTRL_TICK<|"] = 1 -- not sure if needed
      self.templateDict["|>USER_NUM_CTRL_TICKS_PER_EST_TICK<|"] = "%i" % {Block.Mask.ISR_per_est}
      
    end

    if Block.Mask.Kappa == Block.Mask.Kappa then
      if not U.isPositiveScalar(Block.Mask.Kappa) then
        U.error("'Convergence factor' must be a positive scalar.")
      end
      self.kappa = Block.Mask.Kappa
    end

    -- motor parameters
    self.templateDict["|>USER_MOTOR_Rs_Ohm<|"] = Block.Mask.R
    self.templateDict["|>USER_MOTOR_Ls_d_H<|"] = Block.Mask.Ls[1]
    self.templateDict["|>USER_MOTOR_Ls_q_H<|"] = Block.Mask.Ls[1]
    self.templateDict["|>USER_MOTOR_NUM_POLE_PAIRS<|"] = "%i" % {1}
    
    self.templateDict["|>USER_MOTOR_RATED_FLUX_VpHz<|"] = Block.Mask.phi * 2 * math.pi
    

    if Block.Mask.MotorType == 1 then
      -- PM motor
      self.templateDict["|>USER_MOTOR_TYPE<|"] = "MOTOR_TYPE_PM"
      self.templateDict["|>USER_MOTOR_Rr_Ohm<|"] = "NULL"
      self.templateDict["|>USER_MOTOR_MAGNETIZING_CURRENT_A<|"] = "NULL"
    elseif Block.Mask.MotorType == 2 then
      -- Induction motor
      self.templateDict["|>USER_MOTOR_TYPE<|"] = "MOTOR_TYPE_INDUCTION"
      self.templateDict["|>USER_MOTOR_Rr_Ohm<|"] = "0.0"
      self.templateDict["|>USER_MOTOR_MAGNETIZING_CURRENT_A<|"] = "1.0"
    else
      assert('Unsupported motor type.')
    end

    -- 280049: not sure about
    -- pUserParams->BWc_rps
    -- pUserParams->BWdelta
    -- pUserParams->Kctrl_Wb_p_kgm2

    return {
      OutputCode = OutputCode,
      OutputSignal = OutputSignal,
      Require = Require,
      UserData = {bid = self:getId()}
    }
  end

  function Estimator:finalizeThis(c)
    c.PreInitCode:append(
        "EST_setFlag_enableForceAngle(EstimatorHandles[%i], false);" %
            {self.instance})
    c.PreInitCode:append(
        "EST_setFlag_enableRsRecalc(EstimatorHandles[%i], false);" %
            {self.instance})

    if self.kappa ~= nil then
      c.PreInitCode:append([[
        EST_setKappa(EstimatorHandles[%(handle)i], %(kappa)f);
      ]] % {
        handle = self.instance,
        kappa = self.kappa,
      })
    end

    local bgcode = [[
      if(EstimatorEnable[|<INSTANCE>|] > 0){
        if(!EstimatorWasOn[|<INSTANCE>|]){
          EST_enable(EstimatorHandles[|<INSTANCE>|]);
          EST_enableTraj(EstimatorHandles[|<INSTANCE>|]);
        }
      } else {
        EST_disable(EstimatorHandles[|<INSTANCE>|]);
        EST_disableTraj(EstimatorHandles[|<INSTANCE>|]);
      }
      EST_updateTrajState(EstimatorHandles[|<INSTANCE>|]);
      if(EST_updateState(EstimatorHandles[|<INSTANCE>|], 0.0)){
        EST_configureTraj(EstimatorHandles[|<INSTANCE>|]);
      }
      EstimatorWasOn[|<INSTANCE>|] = (EstimatorEnable[|<INSTANCE>|] > 0);
    ]]

    if self.rs_is_variable == true then
      -- this is a semi-official hack: https://e2e.ti.com/support/microcontrollers/c2000/f/171/t/851117
      bgcode = bgcode .. [[
      {
        EST_setRsOnLine_Ohm(EstimatorHandles[|<INSTANCE>|], EstimatorRs[|<INSTANCE>|]);
        EST_setFlag_updateRs(EstimatorHandles[|<INSTANCE>|], true);
      }
      ]]
    end    

    bgcode = string.gsub(bgcode, '|<INSTANCE>|', '%i' % {self.instance})
    c.BackgroundTaskCodeBlocks:append(bgcode)
  end

  function Estimator:finalize(c)
    if static[self.cpu].finalized then
      return
    end

    c.Include:append('%s_user.h' % {Target.Variables.BASE_NAME})
    c.Include:append('userParams.h')
    c.Include:append('est.h')
    c.Include:append('ctrl.h')

    local Coder = require('Coder') 
    local srcDir = Coder.GetFamilySrcDir()
    local destDir = Coder.GetInstallDir()

    U.copyTemplateFile(
      srcDir..'/tiinc/fast/user.h.template',
      destDir..'/'..Target.Variables.BASE_NAME..'_user.h', self.templateDict)

    U.copyTemplateFile(
      srcDir..'/tisrc/fast/user.c.template',
      destDir..'/'..Target.Variables.BASE_NAME..'_user.c', self.templateDict)

    U.copyTemplateFile( -- not a template, but will copy fine.
      srcDir..'/tiinc/fast/userParams.h',
      destDir..'/userParams.h', {})

    c.Declarations:append('EST_Handle EstimatorHandles[%i];' %
                              {static[self.cpu].numInstances})
    c.Declarations:append('USER_Params EstimatorUserParams[%i];' %
                              {static[self.cpu].numInstances})

    c.Declarations:append("static bool EstimatorEnable[%i];" %
                              {static[self.cpu].numInstances})
    c.Declarations:append("static bool EstimatorWasOn[%i];" %
                              {static[self.cpu].numInstances})
    c.Declarations:append("static float EstimatorRs[%i];" %
                              {static[self.cpu].numInstances})
    c.Declarations:append('void EST_setKappa(EST_Handle handle, float_t value);')
    
    local lsUpdateCode = ''
    if self.ls_is_variable then
      lsUpdateCode =  [[
        {
          MATH_Vec2 Ls_dq_H;
          Ls_dq_H.value[0] = aInputs->ld;
          Ls_dq_H.value[1] = aInputs->ld;
          EST_setLs_dq_H(EstimatorHandles[aChannel], &Ls_dq_H);
          //EST_setLs_H(EstimatorHandles[aChannel], aInputs->ld);
        }
      ]]
    end

    local rsUpdateCode = ''
    if self.rs_is_variable == true then
      rsUpdateCode =  [[
        EST_setFlag_enableRsOnLine(EstimatorHandles[aChannel], true);
      ]]
    end
    
    local dCode = [[
      void PLXHAL_EST_update(int16_t aChannel, const PLXHAL_EST_Inputs_t *aInputs, PLXHAL_EST_Outputs_t *aOutputs){
        EST_InputData_t inputData = {0, {0.0, 0.0}, {0.0, 0.0}, 0.0, 0.0};
        EST_OutputData_t outputData = {0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0, 0.0}, {0.0, 0.0}, 0, 0.0};

        MATH_Vec2 idq_ref_A;
        idq_ref_A.value[0] = 0;
        idq_ref_A.value[1] = 0;
        idq_ref_A.value[0] = EST_getIdRated_A(EstimatorHandles[aChannel]);
        EST_updateId_ref_A(EstimatorHandles[aChannel], (float32_t *)&(idq_ref_A.value[0]));
        inputData.Iab_A.value[0] = aInputs->ia;
        inputData.Iab_A.value[1] = aInputs->ib;
        inputData.Vab_V.value[0] = aInputs->va;
        inputData.Vab_V.value[1] = aInputs->vb;
        inputData.dcBus_V = 1;
        inputData.speed_ref_Hz = 0;
        inputData.speed_int_Hz = 0;

        %(lsUpdateCode)s
        %(rsUpdateCode)s

        EST_run(EstimatorHandles[aChannel], &inputData, &outputData);
        EST_setIdq_ref_A(EstimatorHandles[aChannel], &idq_ref_A);
        MATH_Vec2 idq_A;
        EST_getIdq_A(EstimatorHandles[aChannel], &idq_A);
        aOutputs->angle_rad = outputData.angle_rad;
        aOutputs->fe_rps = outputData.fe_rps;
        aOutputs->state = EST_getState(EstimatorHandles[aChannel]);
        aOutputs->flux_wb = EST_getFlux_Wb(EstimatorHandles[aChannel]);
        aOutputs->rs_ohm = EST_getRs_Ohm(EstimatorHandles[aChannel]);

        {
          MATH_Vec2 Ls_dq_H;
          EST_getLs_dq_H(EstimatorHandles[aChannel], &Ls_dq_H); //EST_getLs_H(EstimatorHandles[aChannel]);
          aOutputs->ld_henry = Ls_dq_H.value[0];
          aOutputs->lq_henry = Ls_dq_H.value[1]; 
        }
      
        // for background loop
        EstimatorRs[aChannel] = aInputs->rs;
        EstimatorEnable[aChannel] = aInputs->enable;
      }
    ]] % {lsUpdateCode = lsUpdateCode, rsUpdateCode = rsUpdateCode}
    c.Declarations:append(dCode)

    local code = [[
      {
        for (int i = 0; i < %d; i++) {
          USER_setParams(&EstimatorUserParams[i]);
          EstimatorUserParams[i].flag_bypassMotorId = true;
          USER_setParams_priv(&EstimatorUserParams[i]);
          EstimatorHandles[i] = EST_initEst(i);
          EST_setParams(EstimatorHandles[i], &EstimatorUserParams[i]);
          EstimatorEnable[i] = false;
          EstimatorRs[i] = 0;
          EstimatorWasOn[i] = false;
        }
      }
    ]]
    c.PreInitCode:append(code % {static[self.cpu].numInstances})

    for _, bid in ipairs(static[self.cpu].instances) do
      local est = globals.instances[bid]
      if est:getCpu() == self.cpu then
        est:finalizeThis(c)
      end
    end

    static[self.cpu].finalized = true
  end

  return Estimator
end

return Module
