#include "systemc.h"

enum statetype { S0, S1, S2, S3, S4 };

class LaserDistMeasurer : public sc_module
{
public:
   sc_in<sc_logic> clk, rst;
   sc_in<sc_logic> B, S;
   sc_out<sc_logic> L;
   sc_out<sc_lv<32> > D;
   sc_signal<statetype> state;
   sc_signal<sc_uint<33> > Dctr;

   SC_HAS_PROCESS(LaserDistMeasurer);
    
   LaserDistMeasurer(sc_module_name name ) : sc_module(name)
   {
      SC_METHOD(statemachine);
      sensitive << rst.pos() << clk.pos();
   }
   
   void statemachine()
   {
      if( rst.read() == SC_LOGIC_1 ) {
         L.write(SC_LOGIC_0);
         D.write(0);
         Dctr = 0;
         state.write(S0); // initial state
      }
      else {
         switch (state) {
            case S0:
               L.write(SC_LOGIC_0); // laser off
               D.write(0); // clear D
               state.write(S1);
               break;

            case S1:
               Dctr = 0; // clear count
               if (B.read() == SC_LOGIC_1)
                  state.write(S2);
               else
                  state.write(S1);
			   break;

            case S2:
               L.write(SC_LOGIC_1); // laser on
               state.write(S3);
               break;

            case S3:
               L.write(SC_LOGIC_0); // laser off
               Dctr = Dctr.read() + 1;
               if (S.read() == SC_LOGIC_1)
                  state.write(S4);
               else
                  state.write(S3);
               break;

            case S4:
               D.write(Dctr.read()>>1); // Calculate D
               state.write(S1);
               break;
         }
      }
   }
};

//--------------------------------------------------------------------------//

class testbench : public sc_module
{
public:
    LaserDistMeasurer laser_dist_1;
    
    sc_signal<sc_logic> clk, rst, B, S, L;
    sc_signal<sc_lv<32> > D;
    
    SC_HAS_PROCESS(testbench);

    testbench(sc_module_name name) : sc_module(name), 
                                     laser_dist_1("laser_dist_1")
    {
        SC_THREAD(test_vectors);
        SC_THREAD(oscillator);

        laser_dist_1.clk(clk);
        laser_dist_1.rst(rst);
        laser_dist_1.B(B);
        laser_dist_1.S(S);
        laser_dist_1.L(L);
        laser_dist_1.D(D);
    }
    
    
    void oscillator()
	{
		while(true) {
			clk.write(SC_LOGIC_0);
			wait(1.66667, SC_NS);
			clk.write(SC_LOGIC_1);
			wait(1.66667, SC_NS);
		}
	}
    
    void test_vectors() 
    {
        rst.write(SC_LOGIC_1);
        B.write(SC_LOGIC_0);
        S.write(SC_LOGIC_0);
        wait(clk.posedge_event());
        wait(1, SC_NS);
        rst.write(SC_LOGIC_0);
             
        B.write(SC_LOGIC_1);
        wait(L.posedge_event());
        
        for(int i=0; i<20; i++) wait(clk.posedge_event());
        
        wait(1, SC_NS);
        S.write(SC_LOGIC_1);
        
        wait(clk.posedge_event());
        wait(1, SC_NS);
        S.write(SC_LOGIC_0);
        
        wait(clk.posedge_event());
        wait(100, SC_NS);
        
        sc_stop();
    }

};

//--------------------------------------------------------------------------//

int sc_main (int argc, char * argv[])
{
    
	testbench* tb;
	
	tb = new testbench("testbench");
	
    sc_trace_file *tf = sc_create_vcd_trace_file("/Users/rlysecky/Desktop/sim_trace");
    sc_trace(tf, tb->clk, "clk");
    sc_trace(tf, tb->rst, "rst");
    sc_trace(tf, tb->B, "B");
    sc_trace(tf, tb->S, "S");
    sc_trace(tf, tb->L, "L");
    sc_trace(tf, tb->D, "D");
    sc_trace(tf, tb->laser_dist_1.state, "state");
    
    sc_start();
    sc_close_vcd_trace_file(tf);
    
    return 0;
}

//--------------------------------------------------------------------------//