'/ #tuneable
call BuildHeadingDeviationMember(1,25,20,70,50,100)
call BuildROCMember(1,49,40,60,51,100)

headingDeviation = 20 '/the error input value, map to 100 and peg extremes
fuzzyDeviation(1)=good(HeadingDeviation)
fuzzyDeviation(2)=nominal(HeadingDeviation)
fuzzyDeviation(3)=shit(HeadingDeviation)

ROC = 50              '/ degs/sec input mapped to 0 to 100 for normal range - peg extremes
fuzzyROC(1)=Slow(ROC)
fuzzyROC(2)=Average(ROC)
fuzzyROC(3)=Fast(ROC)


sub BuildHeadingDeviationMember(byval goodstart as byte,byval goodend as byte,byval nominalstart as byte, byval nominalend as byte,byval shitstart as byte, byval shitend as byte)

   local tmp as single ,tmp1 as single,tmp2 as single,tmp3 as single
   local angle as single, yaxis as single
   local base as byte ,t as byte, ctr as byte

  ''/-good-

   tmp1 = 100/goodend : angle = atn(tmp1) '/ RAD
   base = goodend
   for t = 0 to 100
      if base < goodend then yaxis = base * TAN(ANGLE)
      if base > goodend then yaxis = 0
      Good(t) = int(yaxis)
      decr base
   next t

  ''/-nominal-

   tmp1 = nominalend - nominalstart: tmp1=tmp1 / 2 :ctr = tmp1 + nominalstart
   tmp2 = 100/tmp1 : angle = atn(tmp2)

   base = 0
   for t = 1 to ctr
      if t < nominalstart then yaxis = 0
      if t > nominalstart then yaxis = base * TAN(ANGLE)
      nominal(t) = int(yaxis)
      if t => nominalstart then incr base
   next t

   base = NominalEnd - ctr
   for t = ctr to 100
      if t > NominalEnd then yaxis = 0
      if t < NominalEnd then yaxis = base * TAN(ANGLE)
      Nominal(t) = int(Yaxis)
      if t =< NominalEnd then decr base
   next t

  ''/-shit-

   tmp = 100 - ShitStart
   tmp1 = 100 / tmp : angle = atn(tmp1)
   base = tmp

   for t = 100 to 0 step -1
      if base < ShitStart then Yaxis = base * TAN(angle)
      if base > GoodEnd then yaxis = 0
      shit(t) = int(yaxis)
      decr base
   next t
end sub


 
 

''/ -------------heading error / accel get merged and defuzzed to produce rudder rate of change---------------
''/ HeadingDeviation aka error = value returned from navigation function #wip
HeadingDeviation =  byteMap(V,0,30,0,100) '/ crisp input, replace v with input

'/ HOLDS THE % VALUES EACH MEMBER FUNCTION
FuzzyDeviation(1) = Good(HeadingDeviation)
FuzzyDeviation(2) = Nominal(HeadingDeviation)
FuzzyDeviation(3) = Shit(HeadingDeviation)

''/Accel is a value of "seastate" returned from a function that either merges, normalise or maxes imu accel x,y,z  outputs?
Accel = byteMap(V,0,30,0,100)' #test
FuzzyAccel(1) = Calm(accel)
FuzzyAccel(2) = Choppy(accel)
FuzzyAccel(3) = Ruff(accel)

'/convert input member functions to a 1,2,3 definition.  1=L 2=M 3=H
Max(Fuzzydeviation(1), dev_val, dev_mem) '/ ###_mem is the index # of the highest value of the fuzzy array
Max(FuzzyAccel(1), sea_val, sea_mem)     '/ ###_val is the value of the highest value of the array


'/ MERGE RULES - THIS DECIDES WHICH RUDDER RATE OUTPUT MEMBER FUNCTION GETS LOADED
if Dev_Mem = 3 and Sea_Mem= 3 then OP_MF= 3
if Dev_Mem = 3 and Sea_Mem= 2 then OP_MF= 3
if Dev_Mem = 3 and Sea_Mem= 1 then OP_MF= 2
if Dev_Mem = 2 and Sea_Mem= 3 then OP_MF= 2
if Dev_Mem = 2 and Sea_Mem= 2 then OP_MF= 1
if Dev_Mem = 2 and Sea_Mem= 1 then OP_MF= 1
if Dev_Mem = 1 and Sea_Mem= 3 then OP_MF= 2
if Dev_Mem = 1 and Sea_Mem= 2 then OP_MF= 3  '/ high here as this is where the control should home in on
if Dev_Mem = 1 and Sea_Mem= 1 then OP_MF= 3  '/ and once in this region we want fast response to keep it there????


 '/ load the mf values into an array for the MF_height to work with

select case OP_MF

   case 1:'/ lo value MF
      if dev_val > sea_val then
         MF_height(1) = dev_val
         MF_height(2) = sea_val
         MF_height(3) = 0
      else
         MF_height(1) = sea_val
         MF_height(2) = dev_val
         MF_height(3) = 0
      end if


   case 2:'/ med value MF
      if dev_mem > sea_mem then  '/ not a typo re:- the mem
         MF_height(1) = sea_val
         MF_height(2) = dev_val
         MF_height(3) = 0
      else
         MF_height(1) = 0
         MF_height(2) = sea_val
         MF_height(3) = dev_val
      end if
     '/ ^this is garbage, need a better way.


   case 3:'/ hi value MF
      if dev_val > sea_val then
         MF_height(1) = 0
         MF_height(2) = sea_val
         MF_height(3) = dev_val
      else
         MF_height(1) = 0
         MF_height(2) = dev_val
         MF_height(3) = sea_val
      end if

end select

'/MERGE the three defuzz member triangles into one array with heights capped as per above
for n=1 to 100
   if  HeadAccel_Low_Defuzz(n) >= MF_height(1) then HeadAccel_merged_Defuzz(n) = MF_height(1)
   if  HeadAccel_med_Defuzz(n) >= MF_height(2) then HeadAccel_merged_Defuzz(n) = MF_height(2)
   if  HeadAccel_hi_Defuzz(n)  >= MF_height(3) then HeadAccel_merged_Defuzz(n) = MF_height(3)
next n