package BACK

import chisel3._
import chisel3.util._


trait BackBoardSlvQei{ this: BackBoardSlvLocal =>
  val phaseA    = for( i <- 0 until 2 ) yield { Wire(Bool()) }
  val phaseB    = for( i <- 0 until 2 ) yield { Wire(Bool()) }
  val phaseZ    = for( i <- 0 until 2 ) yield { Wire(Bool()) }
  val counter   = for( i <- 0 until 2 ) yield { Reg(UInt(33.W)) }
  val encordNxt = for( i <- 0 until 2 ) yield { Cat( phaseB(i), phaseA(i) ) }
  val encordCur = for( i <- 0 until 2 ) yield { RegNext(encordNxt(i)) }

  val isEnable  = for( i <- 0 until 2 ) yield { RegInit(false.B) }
  val isSign    = for( i <- 0 until 2 ) yield { Reg(Bool()) }
  val isSetVal  = for( i <- 0 until 2 ) yield { RegInit(false.B) }
  val initVal   = for( i <- 0 until 2 ) yield { Reg(UInt(32.W)) }
  val mode      = for( i <- 0 until 2 ) yield { Reg(UInt(2.W)) }
  val invDirect = for( i <- 0 until 2 ) yield { Reg(Bool()) }
  val is2Phase  = for( i <- 0 until 2 ) yield { Reg(Bool()) }
  val isZPhaseEnable  = for( i <- 0 until 2 ) yield { Reg(Bool()) }
  val isSetValPos = for( i <- 0 until 2 ) yield { ~RegNext( isSetVal(i), false.B ) &  isSetVal(i) }
  val isSetValNeg = for( i <- 0 until 2 ) yield {  RegNext( isSetVal(i), false.B ) & ~isSetVal(i) }

  val isAdvA = for( i <- 0 until 2 ) yield {
    ( encordCur(i) === "b00".U & encordNxt(i) === "b01".U & mode(i).extract(1))   |
    ( encordCur(i) === "b01".U & encordNxt(i) === "b11".U & mode(i) =/= "b00".U ) |
    ( encordCur(i) === "b11".U & encordNxt(i) === "b10".U & mode(i).extract(1))   |
    ( encordCur(i) === "b10".U & encordNxt(i) === "b00".U )    
  }


  val isAdvB = for( i <- 0 until 2 ) yield {
    ( encordNxt(i) === "b00".U & encordCur(i) === "b01".U )                    |
    ( encordNxt(i) === "b01".U & encordCur(i) === "b11".U & mode(i).extract(1))   |
    ( encordNxt(i) === "b11".U & encordCur(i) === "b10".U & mode(i) =/= "b00".U ) |
    ( encordNxt(i) === "b10".U & encordCur(i) === "b00".U & mode(i).extract(1))
  }

  val isMiss = for( i <- 0 until 2 ) yield { encordCur(i) === ~encordNxt(i) } 
  // val isKeep = for( i <- 0 until 2 ) yield { encordCur(i) ===  encordNxt(i) } 
  val isOverflow = for( i <- 0 until 2 ) yield { Wire(Bool()) } 

  val isCounter32Flip = for( i <- 0 until 2 ) yield { RegNext( counter(i).extract(32)) =/= counter(i).extract(32) }
  val isCounter31Flip = for( i <- 0 until 2 ) yield { RegNext( counter(i).extract(31)) =/= counter(i).extract(31) }


  for( i <- 0 until 2 ){


    isOverflow(i) := false.B

    when( isZPhaseEnable(i) & phaseZ(i) ){ //Z Phase reset
      counter(i) := initVal(i)
    } .elsewhen( isSetValPos(i) ){
      counter(i) := initVal(i)
    } .elsewhen( isSetValNeg(i) ){
      counter(i) := 0.U
    } .otherwise{
      when(isEnable(i)){
        when(~is2Phase(i)){ //A Phase only
          when( RegNext( phaseA(i) ) & ~phaseA(i) ){
            when( isSetVal(i) & isCounter32Flip(i) ){
              isOverflow(i) := true.B
            } .otherwise{
              counter(i) := counter(i) + 1.U
            }
          }
        } .otherwise{ //AB Phase
          when( isMiss(i) ){
            printf("Warning! QEI Miss one pulse!\n")
          } .elsewhen( isAdvA(i) ){
            when( isSign(i) ) {
              when( invDirect(i) ){
                when( isSetVal(i) & isCounter31Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) - 1.U
                }
              } .otherwise{
                when( isSetVal(i) & isCounter31Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) + 1.U
                }
              }
            } .otherwise{
              when( invDirect(i) ){
                when( isSetVal(i) & isCounter32Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) - 1.U
                }
              } .otherwise{
                when( isSetVal(i) & isCounter32Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) + 1.U
                }
              }
            }
          } .elsewhen( isAdvB(i) ){
            when( isSign(i) ) {
              when( invDirect(i) ){
                when( isSetVal(i) & isCounter31Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) + 1.U
                }
              } .otherwise{
                when( isSetVal(i) & isCounter31Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) - 1.U
                }
              }
            } .otherwise{
              when( invDirect(i) ){
                when( isSetVal(i) & isCounter32Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) + 1.U
                }
              } .otherwise{
                when( isSetVal(i) & isCounter32Flip(i) ){
                  isOverflow(i) := true.B
                } .otherwise{
                  counter(i) := counter(i) - 1.U
                }
              }
            }
          } .otherwise{
            // assert( isKeep(i) )
          }
        }
        
      } .otherwise{ //~isEnable
        counter(i) := initVal(i)
      }
    } 
  }


}        


class Qei extends BackBoardSlvLocal with BackBoardSlvStatusPR with BackBoardSlvQei{
  val phaseAIO    = for( i <- 0 until 2 ) yield { IO(Input(Bool())) }
  val phaseBIO    = for( i <- 0 until 2 ) yield { IO(Input(Bool())) }
  val phaseZIO    = for( i <- 0 until 2 ) yield { IO(Input(Bool())) }


  for( i <- 0 until 2 ){
    phaseA(i) := RegNext(phaseAIO(i))
    phaseB(i) := RegNext(phaseBIO(i))
    phaseZ(i) := RegNext(phaseZIO(i))
  }

  for( i <- 0 until 2 ){

    when(isOverflow(i)){
      isEnable(i) := false.B
    } .elsewhen( (isCrcPass & downRegTemp(0)(3,0) === 0.U) & ~downRegTemp(2+i).extract(0) ){
      isEnable(i) := false.B
    } .elsewhen( (isCrcPass & downRegTemp(0)(3,0) === 0.U) &  downRegTemp(2+i).extract(0) ){
      isEnable(i) := true.B
    }

    when( isCrcPass & downRegTemp(0)(3,0) === 0.U ){
      isZPhaseEnable(i) := downRegTemp(2+(2*i)).extract(1)
      is2Phase(i)       := downRegTemp(2+(2*i)).extract(2)
      isSetVal(i)       := downRegTemp(2+(2*i)).extract(3)
      mode(i)           := downRegTemp(2+(2*i))(5,4)
      invDirect(i)      := downRegTemp(2+(2*i)).extract(6)
      isSign(i)         := downRegTemp(2+(2*i)).extract(7)
      
    }

    when( (isCrcPass & downRegTemp(0)(3,0) === 0.U) & ~downRegTemp(2+i).extract(3) ){
      initVal(i) := 0.U
    } .elsewhen( (isCrcPass & downRegTemp(0)(3,0) === 0.U) & downRegTemp(2+i).extract(3) ){
      initVal(i) := Cat( downRegTemp(9+(4*i)), downRegTemp(8+(4*i)), downRegTemp(7+(4*i)), downRegTemp(6+(4*i)) )
    }

  }

  when( statusPage === 0.U ){
    upReg(2) := Cat( isSign(0), invDirect(0), mode(0), isSetVal(0), is2Phase(0), isZPhaseEnable(0), isEnable(0) )
    upReg(3) := 0.U
    upReg(4) := Cat( isSign(1), invDirect(1), mode(1), isSetVal(1), is2Phase(1), isZPhaseEnable(1), isEnable(1) )
    upReg(5) := 0.U

    upReg(6) := counter(0)(7,0)
    upReg(7) := counter(0)(15,8)
    upReg(8) := counter(0)(23,16)
    upReg(9) := counter(0)(31,24)

    upReg(10) := counter(1)(7,0)
    upReg(11) := counter(1)(15,8)
    upReg(12) := counter(1)(23,16)
    upReg(13) := counter(1)(31,24)
  }

}