/*
 * WESPION.c
 *
 *  Revised on: Dec 16, 2023
 *      Author: HwangSungWoo
 */



#include "WESPION.h"

WP_SwVersionTypeDef WespionVer;
WP_MachineTypeDef WP_Machine={0,};
WP_GymTypeDef WP_Gym={0,};
WP_WeightTypeDef  WP_Weight={0,};
WP_UserSettingTypeDef WP_UserSetting={0,};
WP_RegenRTypeDef WP_RegenR={0,};
WP_DeratingTypeDef WP_Derating={0,};
//WP_FfScaleTypeDef FfScale={0,};

void Initialize_WESPION(void)
{
//  WespionVer.VerMajer = 0x00;
//  WespionVer.VerMiner = 0x06;
//  WespionVer.VerSub   = 0x05;
//
//  WespionVer.VerYear = 2024;
//  WespionVer.VerMonth = 01;
//  WespionVer.VerDate = 25;

  WespionVer.VerMajer = RAM_Data.VerMajer;  ///VER_MAJER
  WespionVer.VerMiner = RAM_Data.VerMiner;  ///VER_MINER
  WespionVer.VerSub = RAM_Data.VerSub;      ///VER_SUB

  WespionVer.VerYear = RAM_Data.VerYear;    ///VER_YEAR
  WespionVer.VerMonth = RAM_Data.VerMonth;  ///VER_MONTH
  WespionVer.VerDate = RAM_Data.VerDate;    ///VER_DATE

  MotorCtrl_Initialize();

  Initialize_App();
  Initialize_Machine();
  Initialize_Gym();
  Initialize_Weight();

  Initialize_UserSetting();

  Initialize_RegenR();

  // Temporary initializations for Test
  Status_VoltRprt = OFF;
}

void Initialize_App(void)
{
  pinMode(56, OUTPUT);  // LED1
  pinMode(57, OUTPUT);  // LED2
  pinMode(87, OUTPUT);  // LED3
#if   HW_VER < 3
  pinMode(88, OUTPUT);  // LED4
  pinMode(89, OUTPUT);  // LED5
  pinMode(90, OUTPUT);  // LED6
  pinMode(91, OUTPUT);  // LED7
#endif
  digitalWrite(56, 1);
  digitalWrite(57, 1);
  digitalWrite(87, 1);
#if   HW_VER < 3
  digitalWrite(88, 1);
  digitalWrite(89, 1);
  digitalWrite(90, 1);
  digitalWrite(91, 1);
#endif
}

void Initialize_Machine(void)
{
//  WP_Machine.SensorGearRatio = 3.0f;
//  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;
//  WP_Machine.SpoolDiameter = 60.0f;        // [mm]
//  WP_Machine.MaxWeight = 10.0f;            // [kg]인데 현재는 A, 테스트 안전을 위해 막아둠
//  WP_Machine.CableMaxLength = 2650;     // [mm]
//  WP_Machine.SoftWindingHeight = 20;    // [mm]
//
//  WP_Machine.Scale_Weight2Current = 0.5f;  // [kg/A] -> C샘플, BS-105-10 기준 약 2
//  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;
//  WP_Machine.DefaultWeight = 0.6;       // [A] -> 나중엔 [kg]로 바꿔야 함
//  WP_Machine.FrictionWeight = 0.45;     // [A] -> 0.7kg에 해당
//  WP_Machine.FrictionSpeedPlus = -20.0f;
//  WP_Machine.FrictionSpeedMinus = 90.0f;

  WP_Machine.SensorGearRatio = RAM_Data.Machine_SensorGearRatio;                          ///MACHINE_SENSOR_GEAR_RATIO
  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;
  WP_Machine.SpoolDiameter = RAM_Data.Machine_SpoolDiameter;                    // [mm]   ///MACHINE_SPOOL_DIAMETER
  WP_Machine.MaxWeight = RAM_Data.Machine_MaxWeight;                            // [kg]   ///MACHINE_MAX_WEIGHT
  WP_Machine.CableMaxLength = (uint16_t)RAM_Data.Machine_CableMaxLength;        // [mm]   ///MACHINE_CABLE_MAX_LENGTH
  WP_Machine.SoftWindingHeight = RAM_Data.Machine_SoftWindingHeight;            // [mm]   ///MACHINE_SOFT_WINDING_HEIGHT

  WP_Machine.Scale_Weight2Current = RAM_Data.Machine_Scale_Weight2Current;      // [kg/A] ///MACHINE_SCALE_WEIGHT2CURRENT
  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;
  WP_Machine.DefaultWeight = RAM_Data.Machine_DefaultWeight;                    // [A] -> 나중엔 [kg]로 바꿔야 함 ///MACHINE_DEFAULT_WEIGHT
  WP_Machine.FrictionWeight = RAM_Data.Machine_FrictionWeight;                  // [A] -> 0.7kg에 해당         ///MACHINE_FRICTION_WEIGHT
  WP_Machine.FrictionSpeedPlus = RAM_Data.Machine_FrictionSpeedPlus;                      ///MACHINE_FRICTION_SPEED_PLUS
  WP_Machine.FrictionSpeedMinus = RAM_Data.Machine_FrictionSpeedMinus;                    ///MACHINE_FRICTION_SPEED_MINUS

  WP_Machine.MaxCurrent = 30.0f;
}

void Initialize_Gym(void)
{
  WP_Gym.AutoWeightRatio[L]= 1.0f;
  WP_Gym.AutoWeightRatio[R]= 1.0f;

  WP_Gym.Region.H_gnd = -1000;      //  [mm]
  WP_Gym.Region.H_base = 0;
  WP_Gym.Region.H_shock = 30;
  WP_Gym.Region.L_soft = 30;     //  가동범위 설정 시 불편함을 줄이기 위해  값을 줄여야 우리함.
  WP_Gym.Region.L_soft_inv = 1.0f / WP_Gym.Region.L_soft;
  WP_Gym.Region.H_LoSoft[L] = WP_Gym.Region.H_shock;   // 초기값임 -> 가동범위 세팅시 갱신 필요!
  WP_Gym.Region.H_LoSoft[R] = WP_Gym.Region.H_shock;
  WP_Gym.Region.L_RangeMin = 50.0f;
  WP_Gym.Region.RangeLo[L] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;                 // 초기값임 -> 가동범위 세팅시 갱신 필요!
  WP_Gym.Region.RangeLo[R] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;            //  가동범위 위에서 무게가 가벼워지면, 방심하다가 조금 내려갈 때 무게가 증가하면서 위험해질 수 있으므로 일단 구현 xx
  WP_Gym.Region.RangeHi[L] = 1800.0f;             // 임의의 값임 -> 향후 수정 필요!!
  WP_Gym.Region.RangeHi[R] = 1800.0f;
  WP_Gym.Region.H_HiSoft[L] = WP_Gym.Region.RangeHi[L] + WP_Gym.Region.L_soft;
  WP_Gym.Region.H_HiSoft[R] = WP_Gym.Region.RangeHi[R] + WP_Gym.Region.L_soft;

  WP_Gym.F_stop = 0.0;      //  3/19 kg 단위로 업데이트 하긴 했음
  WP_Gym.F_min = 1.1;    // 25.03.14 - 금속 기어마찰 의심돼서 올려봄
  WP_Gym.F_shock = 1.50;  // 25.03.14 - 금속 기어마찰 의심돼서 올려봄
  WP_Gym.F_idle = 1.7;

  WP_Gym.F_EccSet = 0.5;      //  초기값임.
  WP_Gym.F_EccLimit = 20.0;
  WP_Gym.EccSpdUp = 2;
  WP_Gym.EccSpdDown = -2;

  WP_Gym.AutoWeightRatio[L] = 1.0f;
  WP_Gym.AutoWeightRatio[R] = 1.0f;
}

void Initialize_Weight(void)
{
//  Common Parameters
  WP_Weight.RefSpdMove = 10;     // mm/s
  WP_Weight.RefSpdStop = 2.5;

  WP_Weight.Ecc_Step = 0.0010;     //  WeightController 주기(16÷2kHz) 당 변화량 -> 주기가 부정확 함.

//  WeightChanger
//  WP_Weight.ChangePeriod = 10.0f;    //  msec
//  WP_Weight.DiffSmall = 0.1f;       //  kg인데 현재는 A
//  WP_Weight.DiffLarge = 1.0f;
//  WP_Weight.ChangeMin = 1.0f;      //  kg/s인데 현재는 A/s
//  WP_Weight.ChangeMax = 2.0f;

//  WeightOnOff
  WP_Weight.Ctrl.OnOffStatus[L] = 0.0f;   // 초기값임.
  WP_Weight.Ctrl.OnOffStatus[R] = 0.0f;
  WP_Weight.Ctrl.OnOffScale[L] = 1.0f;   // 초기값임.
  WP_Weight.Ctrl.OnOffScale[R] = 1.0f;
  WP_Weight.Ctrl.OnOffStep = 0.00005;    // 적당한지 테스트 필요, 향후 스케쥴러로 변경!


//  WeightFeel
  WP_Weight.FfScale.Temp[L] = 0;
  WP_Weight.FfScale.Temp[R] = 0;
  WP_Weight.FfScale.Min = 0.1f;     // 0.3에서 일단 낮춤
  WP_Weight.FfScale.Max = 1.4f;
  WP_Weight.FfScale.Step = 0.00016;   // 무게 단위 후처리 변동으로 2배처리함 (3/19)
}

void Initialize_UserSetting(void)
{
  WP_UserSetting.TimeCali = 200;       // [msec]
  WP_UserSetting.TimeSafetyOFF = 3000;
  LED_Weight_R = 8;
  LED_Weight_G = 4;
  LED_Weight_B = 80;
}

void Initialize_RegenR(void)
{
  WP_RegenR.Vdc_Limit = 58.0f;    // 조정 필요!! (현재 임의 값 3/3)
  WP_RegenR.Vdc_Off = 55.0f;
  WP_RegenR.Vdc_margin_Inv = 1.0f / (WP_RegenR.Vdc_Limit - WP_RegenR.Vdc_Off);

  WP_RegenR.Idc_Charge_Limit = -10.0f;
  WP_RegenR.Idc_Off = -6.0f;
  WP_RegenR.Idc_margin_Inv = 1.0f / (WP_RegenR.Idc_Charge_Limit - WP_RegenR.Idc_Off);

  WP_RegenR.alpha_Vdc = 0.1f;    // 조정 필요!! (현재 임의 값 3/3)
//  WP_RegenR.alpha_Idc = 0.00001f;     // 25.02.25 최소한의 안정된 값을 갖기 위한 실험적 값
  WP_RegenR.alpha_Idc = 0.005f;     // 25.03.14 응답이 너무 느려서 키웠음
  WP_RegenR.RegenScale_Vdc = 0;
  WP_RegenR.RegenScale_Idc = 0;
  WP_RegenR.PWM_Vdc = 0;
  WP_RegenR.PWM_Idc = 0;
  WP_RegenR.PWM = 0;

  WP_RegenR.DebugStatus = 1;    // 1: debug ON
  WP_RegenR.Status = 0;

}

void Initialize_Derating(void)
{
  WP_Derating.Status = 0;
}