#include <bwio.h>
#include <buffer.h>
#include <ipc.h>
#include <kernel.h>
#include <scheduler.h>
#include <td.h>
#include <tools.h>
#include <types.h>


int doSend(KernelStruct* Colonel, int TaskID, void* Msg, int MsgLen, void* ReplyMsg, int ReplyLen) {
  Colonel->Active->RetVal = SRR_FAILED;
  Buffer** AllSendQ = Colonel->AllSendQ;
  // check the TD availability
  int TIDIndex = GetMemoryIndexINT(TaskID);
  bwprintf(COM2,"doSend: TID: %d, TID Index:%d\n\r",TaskID, TIDIndex);
  TD* SenderTask = Colonel->Active;
  if(TIDIndex >= MAX_NUM_TD){
    bwprintf(COM2,"doSend: TID Index:%d out of MaxNumTD\n\r", TIDIndex);
    SenderTask->RetVal = TASK_DNE;
    return FAILURE;
  }
  TD* ReceiverTask = &(Colonel->Tasks[TIDIndex]);
  if(GetAvailability(ReceiverTask)){
    bwprintf(COM2,"doSend: Task %d Availability %d\n\r",TaskID, GetAvailability(ReceiverTask));
    SenderTask->RetVal = TASK_DNE;
    return FAILURE;
  }
  
  bwprintf(COM2,"doSend: Task %d SendMsgLen:%d\n\r",SenderTask->TaskID,MsgLen);
  int Result;
  if (ReceiverTask->TaskState == SendBlocked) {
    AP* Args = ReceiverTask->Args;
    int *ReceiverSlot = (int *)Args->arg1;
    int ReceiverLen = (int)(Args->arg2);
    bwprintf(COM2,"doSend: Receiver %d SendBlocked\n\r", TaskID);
    Result = byteCopy(ReceiverSlot, Msg, ReceiverLen, MsgLen);
    if (Result == FAILURE){
      bwprintf(COM2,"doSend: byteCopy Failed\n\r");
      ReceiverTask->RetVal = MSG_TRUNCATED;
      return Result;
    }
    SenderTask->TaskState = ReplyBlocked;
    ReceiverTask->TaskState = Ready;
    pushToScheduler(Colonel, ReceiverTask);
    BufferPopHead(AllSendQ[TIDIndex]);
  }
  else {
    bwprintf(COM2,"doSend: Receiver %d not Received, Task %d RBLocked\n\r",TaskID,SenderTask->TaskID);
    SenderTask->TaskState = ReceiveBlocked;
    bwprintf(COM2,"doSend: inside k2\n\r");
    FeedBuffer(AllSendQ[TIDIndex],(void *)(SenderTask->TaskID));
    bwprintf(COM2,"doSend: Guess 4, after feed buffer\n\r");
  }
  Colonel->Active = NULL;
  return SUCCESS;
}

int doReceive(KernelStruct* Colonel, int TaskID, void* Msg, int MsgLen) {
    TD* ReceiverTask = Colonel->Active; // me
    int TIDIndex = GetMemoryIndexINT(TaskID);
    TD* SenderTask = &(Colonel->Tasks[TIDIndex]);
    Buffer** AllSendQ = Colonel->AllSendQ;
    int Result;
    AP* Args = SenderTask->Args;

    void* SendMsg = Args->arg1;
    int SendMsgLen = (int)Args->arg2;


    if (SenderTask->TaskState!= ReceiveBlocked) {
      //bwprintf(COM2,"doReceive: Sender %d not send yet\n\r",SenderTask->TaskID);
      ReceiverTask->TaskState = SendBlocked;
      Colonel->Active = NULL;
    }else{
      //bwprintf(COM2,"doReceive,MsgLen:%d,SendMsgLen:%d\n\r",MsgLen,SendMsgLen);
      Result = byteCopy(Msg, SendMsg, MsgLen, SendMsgLen);
      //bwprintf(COM2,"doReceive: Receive Message from %d, result: %d\n\r",SenderTask->TaskID, Result);
      if (Result == FAILURE){
	ReceiverTask->RetVal = MSG_TRUNCATED;
	return Result;
      }
      SenderTask->TaskState = ReplyBlocked;
      //bwprintf(COM2,"doReceive: Sender%d Reply Blocked\n\r",TaskID);
      Result = BufferPopHead(AllSendQ[GetMemoryIndexINT(ReceiverTask->TaskID)]);
      //bwprintf(COM2,"doReceive: BufferPopHead %d\n\r");
      Colonel->Active->RetVal = MsgLen;
    }
    return SUCCESS;
}

int doReply(KernelStruct* Colonel, int TaskID, void* ReplyMsg, int ReplyLen) {
  TD* ReplyTask = Colonel->Active; // me
  int TIDIndex = GetMemoryIndexINT(TaskID);
  TD* SenderTask = &(Colonel->Tasks[TIDIndex]);
  if(TIDIndex >= MAX_NUM_TD){
    bwprintf(COM2,"doReply: TID:%d invalid \n\r", TIDIndex);
    SenderTask->RetVal = TASK_DNE;
    return FAILURE;
  }
  if(GetAvailability(SenderTask)){
    SenderTask->RetVal = TASK_DNE;
    return FAILURE;
  }
  
  AP* Args = SenderTask->Args;
  int Result;
  
  void* SenderReply = Args->arg3;
  int SenderLen = (int)Args->arg4;
  if(SenderTask->TaskState != ReplyBlocked){
    bwprintf(COM2,"doReply: Sender is not in ReplyBlocked state\n\r");
    Colonel->Active->RetVal = SRR_FAILED;
    return FAILURE;
  }
  
  Result = byteCopy(SenderReply, ReplyMsg, SenderLen, ReplyLen);
  if(Result != SUCCESS){
    bwprintf(COM2,"doReply: MSG_TRUNCATED\n\r");
    SenderTask->RetVal = MSG_TRUNCATED;
    return Result;
  }
  
  SenderTask->TaskState = Ready;
  SenderTask->RetVal = ReplyLen;
  Colonel->Active->RetVal = 0;
  pushToScheduler(Colonel, SenderTask);
  return SUCCESS;
}